Drive.c

#pragma config(I2C_Usage, I2C1, i2cSensors)
#pragma config(Sensor, I2C_1,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign )
#pragma config(Sensor, I2C_2,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign )
#pragma config(Sensor, I2C_3,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign )
#pragma config(Sensor, I2C_4,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign )
#pragma config(Motor,  port2,           frontRight,    tmotorNone, openLoop)
#pragma config(Motor,  port3,           backRight,     tmotorNone, openLoop)
#pragma config(Motor,  port4,           backLeft,      tmotorVex393TurboSpeed_MC29, PIDControl, reversed, encoderPort, I2C_3)
#pragma config(Motor,  port5,           frontLeft,     tmotorVex393TurboSpeed_MC29, openLoop, reversed)
#pragma config(Motor,  port6,           liftRight,     tmotorVex393TurboSpeed_MC29, openLoop)
#pragma config(Motor,  port7,           liftLeft,      tmotorVex393TurboSpeed_MC29, PIDControl, reversed, encoderPort, I2C_1)
#pragma config(Motor,  port8,           claw,          tmotorVex393_MC29, PIDControl, encoderPort, I2C_4)
#pragma config(Motor,  port9,           liftMobile,    tmotorVex393_MC29, PIDControl, encoderPort, I2C_2)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

int LS=0, RS=0, CV=0, LV=0, threshold = 15;// user control variables

void initialize(){
	slaveMotor(backRight, frontRight);
	slaveMotor(backLeft, frontLeft);
	slaveMotor(liftRight, liftLeft);
	resetMotorEncoder(liftLeft);
	resetMotorEncoder(backLeft);
	resetMotorEncoder(claw);
	wait1Msec(100);
}

task drive(){
	while(true){
		if (abs(vexRT[Ch3]) > threshold){
			LS = vexRT[Ch3];
			}	else {
			LS = 0;
		}
		if (abs(vexRT[Ch2]) > threshold){
			RS = vexRT[Ch2];
			}	else {
			RS = 0;
		}
		motor[frontRight] = RS;
		motor[frontLeft] = LS;
	}
}

task theClaw(){
	while(true){
		if (vexRT[Btn6U] == 1){
			CV = 127;
			}	else if (vexRT[Btn6D] == 1){
			CV = -127;
			}	else {
			CV = 0;
			stopTask(theClaw);
		}
		motor[claw] = CV;
	}
}

task armLift(){
	while(true){
		if(vexRT[Btn5U] == 1){
			LV = 127;
			} else if (vexRT[Btn5D] == 1){
			LV = -127;
			} else {
			LV = 0;
		}
		motor[liftLeft] = LV;
	}
}

task main(){
	initialize();
	startTask(drive);
  startTask(theClaw);
	startTask(armLift);
}