v4.1

#pragma config(Sensor, dgtl1,  highButton,     sensorDigitalIn)
#pragma config(Sensor, dgtl2,  lowButton,      sensorDigitalIn)
#pragma config(Motor,  port1,           claw2,         tmotorVex393_HBridge, openLoop)
#pragma config(Motor,  port2,           rightMotor,    tmotorServoContinuousRotation, openLoop, reversed)
#pragma config(Motor,  port3,           leftMotor,     tmotorServoContinuousRotation, openLoop)
#pragma config(Motor,  port4,           leftLift1,     tmotorServoContinuousRotation, openLoop, reversed)
#pragma config(Motor,  port5,           leftLift2,     tmotorServoContinuousRotation, openLoop)
#pragma config(Motor,  port6,           leftLift3,     tmotorServoContinuousRotation, openLoop, reversed)
#pragma config(Motor,  port7,           rightLift1,    tmotorServoContinuousRotation, openLoop)
#pragma config(Motor,  port8,           rightLift2,    tmotorServoContinuousRotation, openLoop, reversed)
#pragma config(Motor,  port9,           rightLift3,    tmotorServoContinuousRotation, openLoop)
#pragma config(Motor,  port10,          claw,          tmotorVex393_HBridge, openLoop, reversed)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

// 8 and 9 are the same

void grab(){
	motor[claw] = 80;
	motor[claw2] = 80;
}

void drop(){
	motor[claw] = -80;
	motor[claw2]= -80;
}

void forward() {//moves robot forward
	motor[leftMotor] = -127;
	motor[rightMotor] = -127;
}

void backward(){//moves robot backwards
	motor[leftMotor] = 127;
	motor[rightMotor] = 127;
}

void left(){//rotates the robot right
	motor[leftMotor] = -127;
	motor[rightMotor] = 127;
}

void right(){//rotates the robot left
	motor[leftMotor] = 127;
	motor[rightMotor] = -127;
}
void stopLifters(){ //stops lifters
	motor[leftLift1] = 0;
	motor[leftLift2] = 0;
	motor[leftLift3] = 0;
	motor[rightLift1] = 0;
	motor[rightLift2] = 0;
	motor[rightLift3] = 0;
}

void lift(){ //activates arm
	//	while(SensorValue(highButton) == 0){
	motor[leftLift1] = -127;
	motor[leftLift2] = -127;
	motor[leftLift3] = -127;
	motor[rightLift1] = -127;
	motor[rightLift2] = -127;
	motor[rightLift3] = -127;
	//	}
	/*	motor[leftLift1] = 0;
	motor[leftLift2] = 0;
	motor[leftLift3] = 0;
	motor[rightLift1] = 0;
	motor[rightLift2] = 0;
	motor[rightLift3] = 0;*/
}

void stopClaw(){
	motor[claw2]=0;
	motor[claw]=0;
}

void unLift(){ //unlifts
	//while(SensorValue(lowButton) == 0){
	motor[leftLift1] = 127;
	motor[leftLift2] = 127;
	motor[leftLift3] = 127;
	motor[rightLift1] = 127;
	motor[rightLift2] = 127;
	motor[rightLift3] = 127;
}

void hang(){ //hangs over the thing
	motor[leftLift1] = -80;
	motor[leftLift2] = -80;
	motor[leftLift3] = -80;
	motor[rightLift1] = -80;
	motor[rightLift2] = -80;
	motor[rightLift3] = -80;
}

void stopMotors(){ //STOPS ALL MOTORS
	motor[leftMotor] = 0;
	motor[rightMotor] = 0;
}

task main()
{
/*	stopClaw();
	clearTimer(T1);
	while(time1[T1]<650){
		grab();
	}
	stopClaw();
	clearTimer(T2);
	while(time1[T2]<2600){
		forward();
	}
	stopMotors();
	unLift();
	for(int i=0; i<4; i++){
		clearTimer(T3);
		while(time1[T3]<500){
			forward();
		}
		clearTimer(T3);
		while(time1[T3]<300){
			stopMotors();
		}
		clearTimer(T4);
		while(time1[T4]<500){
			backward();
		}
		clearTimer(T3);
		while(time1[T3]<500){
			stopMotors();
		}
		stopLifters();
	}
	*/
	while(true){
		while(vexRT[Btn7U] == 1){
			forward();
		}
		while(vexRT[Btn7L] == 1){
			left();
		}
		while(vexRT[Btn7R] == 1){
			right();
		}
		while(vexRT[Btn7D] == 1){
			backward();
		}
		/*	if(vexRT[Btn5D] == 1){
		clearTimer(T1);
		while(time1[T1]<1000){
		lift();
		}
		stopLifters();
		}
		if(vexRT[Btn5U] == 1){
		clearTimer(T1);
		while(time1[T1]<1100){
		unLift();
		}
		stopLifters();
		}*/
		if(vexRT[Btn8U] == 1){
			clearTimer(T1);
			while(time1[T1]<100){
				unLift();
			}
			stopLifters();
		}
		if(vexRT[Btn8D] == 1){
			clearTimer(T1);
			while(time1[T1]<100){
				lift();
			}
			stopLifters();
		}
		if(vexRT[Btn8L] == 1){
			stopLifters();
		}

		if(vexRT[Btn6D] == 1){
			stopClaw();
			clearTimer(T3);
			while(time1[T3]<50){
				drop();
			}
			stopClaw();
		}

		if(vexRT[Btn6U] == 1){
			grab();
			/*clearTimer(T3);
			while(time1[T3]<10){
				grab();
			}
			stopClaw();*/
		}

		stopMotors();
	}
}