PenguinEmpire.h

/*
 * PenguinEmpire.h
 *
 *  Created on: Jan 15, 2017
 *      Author: Penguin Empire
 */

#ifndef SRC_PENGUINEMPIRE_H_
#define SRC_PENGUINEMPIRE_H_

#include "WPILib.h"
#include "MyJoystick.h"
#include "Joystick.h"
#include <Math.h>
#include <unistd.h>

#define NUMACTIONS 3


class Robot : public IterativeRobot
{
public:
	Encoder leftEncoder, rightEncoder;
	Joystick left, right, controller;
	Spark leftDriveA, leftDriveB, rightDriveA, rightDriveB, ropeClimber;
	DoubleSolenoid leftGearBox, rightGearBox, gearPusher;
	Compressor compressor;
//	PIDController aimController;

	MyJoystick* handheld;
	MyJoystick* rightJoystick;
	MyJoystick* leftJoystick;

	bool driveSwapped;
	bool enabled;
	bool pressureStatus;
	float current;

	bool autoAiming;
	bool isLeft;
	bool isRight;
	bool isCenter;

	std::shared_ptr<NetworkTable> table;
	std::vector<double> xCoords;
	double xCoordA;
	double xCoordB;

//	AutoSteps autoStep;
	enum Gears {
		up,
		down
	} leftGear, rightGear;

	Robot();
	~Robot();
	void RobotInit();
	void AutonomousInit();
	void AutonomousPeriodic();
	void AutoGearAim();
	void TeleopInit();
	void TeleopPeriodic();
	void ManualGearAim(bool aimButton);
	void DualTankDrive();
	void InitGears();
	void AutoShiftGears();
	void ManualShiftGears(bool upButton, bool downButton);
	void DebugShiftGears();
	void SwapDrive(bool right, bool left);
	void PushGear(bool pushButton, bool retractButton);
	void ClimbRope(bool climbButton);
	void TestPeriodic();
};

class PenguinDrive {
	Robot* myRobot;
	Joystick* leftDriveStick;
	Joystick* rightDriveStick;
	PenguinDrive(Joystick* left, Joystick* right);
	~PenguinDrive();
	void TankDrive(Spark leftA, Spark leftB, Spark rightA, Spark rightB);
};

enum Actions {
	Init,
	Forward,
	Backward,
	Left,
	Right,
	Wait,
	ExtendGearPusher,
	RetractGearPusher,
	ClimbRope,
	AutoAim
};

struct Step {
	Actions action;
	int distance;
	int angle;
	float leftSpeed;
	float rightSpeed;
	int time;
};
//Step step;

extern Step ActionsArray[]; // = {{Actions::Init,0,0,0,0}, {Actions::Forward,60,0,3.0,3.0}, {Actions::Backward,60,0,3.0,3.0}};

class Action {
public:
	Step s;
	Action(){myRobot = new Robot();};
	Robot* myRobot;
	virtual ~Action(){};
	virtual void StepAction(){};
};

class InitAction : public Action {
public:
	InitAction(){};
	~InitAction(){};
	void StepAction(){ myRobot->leftEncoder.Reset(); myRobot->rightEncoder.Reset(); };
};


class ForwardAction : public Action {
public:
	int d;
	float s;
	ForwardAction(int distance, float speed){d = distance; s = speed;};
	~ForwardAction(){
		//		if (leftEncoder.Get() <= d)          EXAMPLE IMPLEMENTATION
		//		{
		//			leftDriveA.Set(s);
		//			leftDriveB.Set(s);
		//		}

			if ((myRobot->leftEncoder.Get() + myRobot->rightEncoder.Get()) / 2 >= d){
				myRobot->leftDriveA.Set(s);
				myRobot->leftDriveB.Set(s);
				myRobot->rightDriveA.Set(-s);
				myRobot->rightDriveB.Set(-s);
			}
	};
	void StepAction(){};
};

class BackwardAction : public Action {
public:
	int d;
	float s;
	BackwardAction(int distance, float speed){d = distance; s = speed;};
	~BackwardAction(){};
	void StepAction(){
		if ((myRobot->leftEncoder.Get() + myRobot->rightEncoder.Get()) / 2 >= d){
			myRobot->leftDriveA.Set(-s);
			myRobot->leftDriveB.Set(-s);
			myRobot->rightDriveA.Set(s);
			myRobot->rightDriveB.Set(s);
	}};
};

class ExtendGearPusherAction : public Action {
public:
	ExtendGearPusherAction(){};
	~ExtendGearPusherAction(){};
	void StepAction(){
		myRobot->gearPusher.Set(DoubleSolenoid::kForward);
	}
};

class RetractGearPusherAction : public Action {
public:
	RetractGearPusherAction(){};
	~RetractGearPusherAction(){};
	void StepAction(){
		myRobot->gearPusher.Set(DoubleSolenoid::kReverse);
	}
};

class ClimbRopeAction : public Action {
public:
	int t;
	ClimbRopeAction(float time){t = time;};
	~ClimbRopeAction(){};
	void StepAction(){
		myRobot->ropeClimber.Set(1.0);
		sleep(t);
		myRobot->ropeClimber.Set(0.0);
	}
};

class AutoAimAction : public Action {
public:
	AutoAimAction(){};
	~AutoAimAction(){};
	void StepAction(){
		if (myRobot->isCenter != true){
			myRobot->AutoGearAim();
		}
	}
};

class PenguinAutonomous {
public:
	std::vector<Action> Sequence;
	PenguinAutonomous();
	~PenguinAutonomous(){};
//	virtual void StepInit(Step step);
	void SequenceInit();
	void RunSequence(){
		int size = 0; // = sizeof(Sequence);
		for (int j = 0; j < size; j++) {
			Sequence[j].StepAction();
		}
	};
};

extern PenguinAutonomous* penguinAuto;


#endif /* SRC_PENGUINEMPIRE_H_ */