#pragma config(Motor,  port1,           leftMotor,     tmotorVex393, openLoop, reversed)
#pragma config(Motor,  port6,           clawMotor,     tmotorVex269, openLoop, reversed)
#pragma config(Motor,  port7,           armMotor,      tmotorVex393, openLoop, reversed)
#pragma config(Motor,  port10,          rightMotor,    tmotorVex393, openLoop)

//--------------------------------------------| FUNCTIONS |-------------------------------------------
void forward(int forwardTime)
{
	motor[rightMotor] = 75;
	motor[leftMotor]  = 75;
	wait1Msec(forwardTime);

	motor[rightMotor] = 0;
	motor[leftMotor]  = 0;
	wait1Msec(1000);
}
void backward(int backwardTime)
{
	motor[rightMotor] = -75;
	motor[leftMotor]  = -75;
	wait1Msec(backwardTime);

	motor[rightMotor] = 0;
	motor[leftMotor]  = 0;
	wait1Msec(1000);
}
void rightTurn(int rightTurnTime)
{
	motor[rightMotor] = -75;
	motor[leftMotor]  = 75;
	wait1Msec(rightTurnTime);

	motor[rightMotor] = 0;
	motor[leftMotor]  = 0;
	wait1Msec(1000);
}

void leftTurn(int leftTurnTime)
{
	motor[rightMotor] = 75;
	motor[leftMotor]  = -75;
	wait1Msec(leftTurnTime);

	motor[rightMotor] = 0;
	motor[leftMotor]  = 0;
	wait1Msec(1000);
}
//----------------------------------------------------------------------------------------------------

//============================================| TASK ONE |============================================
task One()
{
	forward(1000);
	backward(1000);
	rightTurn(1000);
	leftTurn(1000);
}

//====================================================================================================

//+++++++++++++++++++++++++++++++++++++++++++++| MAIN |+++++++++++++++++++++++++++++++++++++++++++++++
task main ()
{
	int threshold = 15;   // helps to eliminate 'noise' from a joystick that isn't perfectly at (0,0)
	// feel free to change this to match your needs.

	while(1 == 1)
	{
		if(abs(vexRT[Ch3]) > threshold)         // If the left joystick is greater than or less than the threshold:
		{
			motor[leftMotor]  = (vexRT[Ch3]);   // Left Joystick Y value.
		}
		else                                    // If the left joystick is within the threshold:
		{
			motor[leftMotor]  = 0;                // Stop the left motor (cancel noise)
		}

		if(abs(vexRT[Ch2]) > threshold)         // If the right joystick is greater than or less than the threshold:
		{
			motor[rightMotor] = (vexRT[Ch2]);   	// Right Joystick Y value.
		}
		else                                    // If the right joystick is within the threshold:
		{
			motor[rightMotor] = 0;                // Stop the right motor (cancel noise)
		}

		// Raise, lower or do not move arm
		if(vexRT[Btn5U] == 1)       	//If button 5U is pressed...
		{
			motor[armMotor] = 127;    	//...raise the arm.
		}
		else if(vexRT[Btn5D] == 1)  	//Else, if button 5D is pressed...
		{
			motor[armMotor] = -127;   	//...lower the arm.
		}
		else if(vexRT[Btn7U] == 1)		//If Left D-Pad is pressed up...
		{
			motor[armMotor] = 25;				//...raise the arm at half speed.
		}
		else if(vexRT[Btn7D] == 1)		//If Left D-Pad is pressed down...
		{
			motor[armMotor] = -20;			//...lower the arm at half speed.
		}
		else                      		//Else (neither button is pressed)...
		{
			motor[armMotor] = 0;      	//...stop the arm.
		}

		// Open, close or do not more claw
		if(vexRT[Btn6U] == 1)       	//If Button 6U is pressed...
		{
			motor[clawMotor] = 127;  		//...close the gripper.
		}
		else if(vexRT[Btn6D] == 1)  	//Else, if button 6D is pressed...
		{
			motor[clawMotor] = -127; 		//...open the gripper.
		}
		else if(vexRT[Btn8U] == 1)		//If Left D-Pad is pressed up...
		{
			motor[clawMotor] = 35;			//...close the gripper at half speed.
		}
		else if(vexRT[Btn8D] == 1)		//If Left D-Pad is pressed down...
		{
			motor[clawMotor] = -20;			//...open the gripper at half speed.
		}
		else                      		//Else (neither button is pressed)...
		{
			motor[clawMotor] = 0;    		//...stop the gripper.
		}
	}
}
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++