AutoRampGyro3.c

#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTMotor,  none)
#pragma config(Hubs,  S2, HTMotor,  HTServo,  none,     none)
#pragma config(Sensor, S1,     ,               sensorI2CMuxController)
#pragma config(Sensor, S2,     ,               sensorI2CMuxController)
#pragma config(Sensor, S3,     IR,             sensorHiTechnicIRSeeker1200)
#pragma config(Sensor, S4,     Gyro,           sensorI2CHiTechnicGyro)
#pragma config(Motor,  mtr_S1_C1_1,     LeftF,         tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C1_2,     RightF,        tmotorTetrix, openLoop, reversed)
#pragma config(Motor,  mtr_S1_C2_1,     LiftR,         tmotorTetrix, PIDControl)
#pragma config(Motor,  mtr_S1_C2_2,     LiftF,         tmotorTetrix, PIDControl, reversed)
#pragma config(Motor,  mtr_S1_C3_1,     Spinner,       tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C3_2,     motorI,        tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S2_C1_1,     LeftR,         tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S2_C1_2,     RightR,        tmotorTetrix, openLoop, reversed)
#pragma config(Servo,  srvo_S2_C2_1,    Clamp,                tServoStandard)
#pragma config(Servo,  srvo_S2_C2_2,    IRlift,               tServoStandard)
#pragma config(Servo,  srvo_S2_C2_3,    SpinLift,             tServoStandard)
#pragma config(Servo,  srvo_S2_C2_4,    servo4,               tServoNone)
#pragma config(Servo,  srvo_S2_C2_5,    servo5,               tServoNone)
#pragma config(Servo,  srvo_S2_C2_6,    servo6,               tServoNone)
#pragma config(SrvoPosition,  Position01,            166, 128,  23, 128, 128, 128, 128, 128)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "JoystickDriver.c"

void rightturn(int rdegreesToTurn)
{
	// initial reading
	float rdegreesSoFar = 0;
	int rinitialTurnReading = SensorValue[Gyro];

	// start turn
				motor[LeftF] = -50;
 				motor[RightF] = 50;
 				motor[LeftR] = -50;
 				motor[RightR] = 50;


	// check if we have turned enough
	while (abs(rdegreesSoFar) < abs(rdegreesToTurn))
	{
	//update degreesSoFar
	wait1Msec(10);
	int rcurrentGyroReading = SensorValue[Gyro] - rinitialTurnReading;
	rdegreesSoFar = rdegreesSoFar + rcurrentGyroReading*.01;
	}

	// stop turning
				motor[LeftF] = 0;
 				motor[RightF] = 0;
 				motor[LeftR] = 0;
 				motor[RightR] = 0;

}

void leftturn(int ldegreesToTurn)
{
	// initial reading
	float ldegreesSoFar = 0;
	int linitialTurnReading = SensorValue[Gyro];

	// start turn
				motor[LeftF] = 50;
 				motor[RightF] = -50;
 				motor[LeftR] = 50;
 				motor[RightR] = -50;


	// check if we have turned enough
	while (abs(ldegreesSoFar) < abs(ldegreesToTurn))
	{
	//update degreesSoFar
	wait1Msec(10);
	int lcurrentGyroReading = SensorValue[Gyro] - linitialTurnReading;
	ldegreesSoFar = ldegreesSoFar + lcurrentGyroReading*.01;
	}

	// stop turning
				motor[LeftF] = 0;
 				motor[RightF] = 0;
 				motor[LeftR] = 0;
 				motor[RightR] = 0;

}

void forward(int ftimemillisecond)  //use until we have encoders
{

	// Move forward
				motor[LeftF] = 40;
 				motor[RightF] = 40;
 				motor[LeftR] = -40;
 				motor[RightR] = -40;
				wait1Msec(ftimemillisecond);
	// stop motors
				motor[LeftF] = 0;
 				motor[RightF] = 0;
 				motor[LeftR] = 0;
 				motor[RightR] = 0;
				wait1Msec(200);
}

void backward(int btimemillisecond)  //use until we have encoders
{

	// Move backward
				motor[LeftF] = -50;
 				motor[RightF] = -50;
 				motor[LeftR] = 50;
 				motor[RightR] = 50;
				wait1Msec(btimemillisecond);
	// stop motors
				motor[LeftF] = 0;
 				motor[RightF] = 0;
 				motor[LeftR] = 0;
 				motor[RightR] = 0;
				wait1Msec(200);
}

void Liftup(int Height)  //How tall to go
{

  			nMotorEncoder[LiftR] = 0;  //clear the TETRIX encoders in motors left and right
  			nMotorEncoder[LiftF] = 0;  //clear the TETRIX encoders in motors left and right
  	    wait1Msec(200);							// Delay before next command

 				while (nMotorEncoder[LiftF] < Height) //while the encoder wheel turns one revolution//
	        {
	        motor[LiftR] = 60;
					motor[LiftF] = 60;
	  			}

				motor[LiftR] = 0;
				motor[LiftF] = 0;
				wait1Msec(200);							// Delay before next command
}

void Basket(int Height3)
{
				nMotorEncoder[LiftR] = 0;  		//clear the TETRIX encoders in rear motor
				wait1Msec(200);
				while (nMotorEncoder[LiftR] < Height3) //Lift up the back end of basket
	        {
	    		motor[LiftR] = 60;
	  			}
			  motor[LiftR] =0;
				wait1Msec(2000);							//wait to drop balls in tube
 				while (nMotorEncoder[LiftR] > 0) //Lift up the back end of basket
	        {
	    		motor[LiftR] = -60;
	  			}
		 		motor[LiftR] =0;
}

void Liftdown(int Height2)
{
while (nMotorEncoder[LiftF] > Height2) //while the encoder wheel turns one revolution//
	        {
	    		motor[LiftR] = -60;
					motor[LiftF] = -60;
	  			}

motor[LiftR] = 0;
motor[LiftF] = 0;
}

task main()
{
  waitForStart();		                // Wait for the field to enter autonomous mode

//Position clamp down
  	servoTarget[IRlift] = 128;
  	servo[IRlift] = 128;
		servo[Clamp] = 227;        // changes the position of servo1 to 245
		servoTarget[Clamp] = 227;  // changes the position of servo1 to 160

//Move forward and grab tower
		forward(3650);
		servo[SpinLift] = 70;

		//wait 5 Sec.
wait1Msec(300);
		Liftup(3900);
		Basket(900);
		Liftdown(0);
//Maneuver to parking zone
		leftturn(125);
		forward(800);
		leftturn(22);
		forward(3500);
//		rightturn(20);
//		forward(500);

}