AutoParkGyroBack2.c

#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTServo,  none)
#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,     LeftR,         tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C2_2,     RightR,        tmotorTetrix, openLoop, reversed)
#pragma config(Servo,  srvo_S1_C3_1,    Clamp,                tServoStandard)
#pragma config(Servo,  srvo_S1_C3_2,    IRlift,               tServoStandard)
#pragma config(Servo,  srvo_S1_C3_3,    servo3,               tServoNone)
#pragma config(Servo,  srvo_S1_C3_4,    servo4,               tServoNone)
#pragma config(Servo,  srvo_S1_C3_5,    servo5,               tServoNone)
#pragma config(Servo,  srvo_S1_C3_6,    servo6,               tServoNone)
//*!!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] = 50;
 				motor[RightF] = 50;
 				motor[LeftR] = -50;
 				motor[RightR] = -50;
				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);
}


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

servo[Clamp] = 135;        // changes the position of IR sensor to 128(up in the air).  VALUES range from 0 to 255. Need to test so we can set a value.
servoTarget[Clamp] = 135;
servo[IRlift] = 128;        // changes the position of IR sensor to 128(up in the air).  VALUES range from 0 to 255.
wait1Msec(300);             // delay to allow for servo to get to position.

//Move forward and take sensor reading
		forward(1200);

// FOLLOWING CODE USED TO DETERMINE POSITION OF CENTER CONSOLE
	if (SensorValue[IR] == 5)     // center piece is in Position 3 with bar just off to right
 		{
 			wait1Msec(200);
//		wait1Msec(5000);
 		rightturn(81);
		forward(700);
		rightturn(74);
		backward(2000);
		}

	else if ((SensorValue[IR] > 0) & (SensorValue[IR] < 5))    //center piece is in Position 2 with bar straight ahead.  CHARGE!!
    {
				nMotorEncoder[LeftF] = 0;  //clear the TETRIX encoders in motors left and right
        nMotorEncoder[RightF] = 0;
        wait1Msec(200);							// Delay before next command
//		wait1Msec(5000);
    leftturn(81);  //turn robot around to backside rammer
		forward(200);
		leftturn(40);
    backward(1800);
		}

  else if (SensorValue[IR] == 0)     //center piece is in Position 1 with bar straight ahead.
    {
				nMotorEncoder[LeftF] = 0;  //clear the TETRIX encoders in motors left and right
        nMotorEncoder[RightF] = 0;
        wait1Msec(200);							// Delay before next command
		leftturn(51);
		forward(1000);
		leftturn(25);
		backward(1700);
    }

servo[IRlift] = 245;        // changes the position of IR sensor to 128(up in the air).  VALUES range from 0 to 255. Need to test so we can set a value.
servoTarget[IRlift] = 245;
}