Auto.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, encoderPort, I2C_3)
#pragma config(Motor,  port5,           frontLeft,     tmotorNone, openLoop)
#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               !!*//

// Variables
int armSpeed = 90;
int tickGoal = 510;
int driveDist;
float dia = 2.75;
float ticksPerInch = 360/(dia*PI)

void initialize () {
	slaveMotor(liftRight, liftLeft);
	nMotorEncoder(claw) = 0;
	resetMotorEncoder(liftLeft);
	wait1Msec(100);
}

void openClaw () {
	motor[claw] = 110;
	wait1Msec(2000);
	motor[claw] = 0;
}

void closeClaw () {
	motor[claw] = -60;
	wait1Msec(2000);
	motor[claw] = 0;
}

void lowerArm() {
	clearTimer(T1);
	while(getMotorEncoder(liftLeft)>0 & time1[T1]<4000) motor(liftLeft) = 40;
	motor(liftLeft) = 0;
}

void raiseArm() {
	clearTimer(T1);
	while(getMotorEncoder(liftLeft)<tickGoal & time1[T1]<4000) motor(liftLeft) = -40;
	motor(liftLeft) = 0;
}

task main()
{
	int i;
	initialize();

	for (i=0; i<5; i++){
		motor[claw] = -60;
		wait1Msec(300);
		raiseArm();
		openClaw();
		wait1Msec(100);
		closeClaw();
		wait1Msec(20);
		lowerArm();
		openClaw();
		wait1Msec(200);
	}
}