#include <eGizmo_PBOT2018.h>
#include <eGizmo_PS2Controller.h>

//Grip Range
#define minGrip	90
#define maxGrip	135

//Tuck Range
#define minTuck	40
#define maxTuck	140

//Lift Range
#define minLift	40
#define maxLift	160

//Base Range
#define minBase	0
#define maxBase 160

//HOME
#define homeGrip 135
#define homeLift 40
#define homeTuck 60
#define homeBase 90

//SERVO NUMBERS
#define GRIP 1
#define BASE 2
#define LIFT 3
#define TUCK 4

eGizmo_PS2Controller ps2Data(&Serial);
eGizmo_PBOT2018 pBot;

int last_dataX, last_dataY;

void setup(){
	//Initialize PS2 & PBOT
		ps2Data.begin(9600);
		pBot.BEGIN();

	//wait for ps2 input
		pBot.STOP();
		//delay(2000);
		//Set all servo  to home position

			pBot.SERVO1(homeGrip);
			delay(15);
			pBot.SERVO2(homeBase);
			delay(15);
			pBot.SERVO3(homeLift);
			delay(15);
			pBot.SERVO4(homeTuck);
			delay(15);

		//Save position values
			pBot.servo1Pos = homeGrip;
			pBot.servo2Pos = homeBase;
			pBot.servo3Pos = homeLift;
			pBot.servo4Pos = homeTuck;

		//Wait for valid PS2 input
			while(!ps2Data.update());
}

void loop(){
	//Query PS2 cotroller data, wait for valid inputs
		if(ps2Data.update()){

			//Motor Controls
				//Update motor states only when left joystick values change
					if((last_dataY != ps2Data.lefty) || (last_dataX != ps2Data.leftx)){
						last_dataX = ps2Data.leftx;
						last_dataY = ps2Data.lefty;

						pBot.PS2MOTORCONTROL(last_dataX, last_dataY);
					}

			//E-gra Codes
				//Check for button press, do routines if button is pressed
					if(ps2Data.right){
						pBot.servo1Pos--;
						//Keep Values within acceptable limits
							pBot.servo1Pos = constrain(pBot.servo1Pos, minGrip, maxGrip);
							Serial.println(pBot.servo1Pos);

						//Write servo position values
							pBot.SERVO1(pBot.servo1Pos);
					}//if

					if(ps2Data.left){
						pBot.servo1Pos++;

						//Keep Values within acceptable limits
							pBot.servo1Pos = constrain(pBot.servo1Pos, minGrip, maxGrip);
							Serial.println(pBot.servo1Pos);

						//Write servo position values
							pBot.SERVO1(pBot.servo1Pos);
					}//if

					if(ps2Data.square){
						pBot.servo2Pos++;

						//Keep Values within acceptable limits
							pBot.servo2Pos = constrain(pBot.servo2Pos, minBase, maxBase);
							Serial.println(pBot.servo2Pos);

						//Write servo position values
							pBot.SERVO2(pBot.servo2Pos);
					}//if

					if(ps2Data.circle){
						pBot.servo2Pos--;

						//Keep Values within acceptable limits
							pBot.servo2Pos = constrain(pBot.servo2Pos, minBase, maxBase);
							Serial.println(pBot.servo2Pos);

						//Write servo position values
							pBot.SERVO2(pBot.servo2Pos);
					}//if

					if(ps2Data.cross){
						pBot.servo3Pos++;

						//Keep Values within acceptable limits
							pBot.servo3Pos = constrain(pBot.servo3Pos, minLift, maxLift);
							Serial.println(pBot.servo3Pos);

						//Write servo position values
							pBot.SERVO3(pBot.servo3Pos);
					}//if

					if(ps2Data.triangle){
						pBot.servo3Pos--;

						//Keep Values within acceptable limits
							pBot.servo3Pos = constrain(pBot.servo3Pos, minLift, maxLift);
							Serial.println(pBot.servo3Pos);

						//Write servo position values
							pBot.SERVO3(pBot.servo3Pos);
					}//if

					if(ps2Data.right2){
						pBot.servo4Pos++;

						//Keep Values within acceptable limits
							pBot.servo4Pos = constrain(pBot.servo4Pos, minTuck, maxTuck);
							Serial.println(pBot.servo4Pos);

						//Write servo position values
							pBot.SERVO4(pBot.servo4Pos);
					}//if

					if(ps2Data.right1){
						pBot.servo4Pos--;

						//Keep Values within acceptable limits
							pBot.servo4Pos = constrain(pBot.servo4Pos, minTuck, maxTuck);
							Serial.println(pBot.servo4Pos);

						//Write servo position values
							pBot.SERVO4(pBot.servo4Pos);
					}//if

					delay(15);

		}//if

}