Jan7Code

#pragma config(Sensor, dgtl1,  armMax,         sensorDigitalIn)
#pragma config(Motor,  port1,           rightDrive,    tmotorVex393_HBridge, openLoop)
#pragma config(Motor,  port3,           claw,          tmotorServoContinuousRotation, openLoop)
#pragma config(Motor,  port4,           arm,           tmotorServoContinuousRotation, openLoop)
#pragma config(Motor,  port10,          leftDrive,     tmotorVex393_HBridge, openLoop, reversed)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

task main()
{
	int cutoff = 27.5;
	bool prevDown = false;
	while(1==1)
	{
		if (vexRT[Ch2] > cutoff || vexRT[Ch2] < -cutoff)
		{
			motor[leftDrive] = vexRT[Ch2] / 2;
			motor[rightDrive] = vexRT[Ch2] / 2;
		}
		else if (vexRT[Ch1] > cutoff)
		{
			motor[leftDrive] = vexRT[Ch1] / 2;
			motor[rightDrive] = -vexRT[Ch1] / 2;
		}
		else if (vexRT[Ch1] < -cutoff)
		{
			motor[leftDrive] = vexRT[Ch1] / 2;
			motor[rightDrive] = -vexRT[Ch1] / 2;
		}
		else
		{
			motor[leftDrive] = 0;
			motor[rightDrive] = 0;
		}
		if(vexRT[Btn5U])
		{
			motor[arm] = -63.5;
		}
		else if(vexRT[Btn5D] && SensorValue[armMax] == 1)
		{
			motor[arm] = 63.5;
			prevDown = true;
		}
		else
		{
			if(prevDown)
			{	
				motor[arm] = -63.5;
				wait1Msec(35);
				prevDown = false;
			}
			
			motor[arm] = -10;
		}
		if(SensorValue[armMax] == 0)
		{
			motor[arm] = -63.5;
			wait1Msec(100);
			motor[arm] = 0;
			wait1Msec(500);
		}
		if(vexRT[Btn6U])
		{
			motor[claw] = -63.5;
		}
		else if(vexRT[Btn6D])
		{
			motor[claw] = 63.5;
		}
		else
		{
			motor[claw] = 0;
		}
	}

}