AutoPark (2).c

#pragma config(Hubs,  S1, HTMotor,  HTMotor,  none,     none)
#pragma config(Hubs,  S2, HTServo,  none,     none,     none)
#pragma config(Sensor, S1,     ,               sensorI2CMuxController)
#pragma config(Sensor, S2,     ,               sensorI2CMuxController)
#pragma config(Sensor, S3,     IR,             sensorHiTechnicIRSeeker1200)
#pragma config(Motor,  mtr_S1_C1_1,     LeftR,         tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C1_2,     RightR,        tmotorTetrix, openLoop, reversed)
#pragma config(Motor,  mtr_S1_C2_1,     LeftF,         tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C2_2,     RightF,        tmotorTetrix, openLoop, reversed)
#pragma config(Servo,  srvo_S2_C1_1,    Clamp,                tServoStandard)
#pragma config(Servo,  srvo_S2_C1_2,    IRlift,               tServoStandard)
#pragma config(Servo,  srvo_S2_C1_3,    servo3,               tServoNone)
#pragma config(Servo,  srvo_S2_C1_4,    servo4,               tServoNone)
#pragma config(Servo,  srvo_S2_C1_5,    servo5,               tServoNone)
#pragma config(Servo,  srvo_S2_C1_6,    servo6,               tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

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


  void Display()                                                        // Displays both Servo Position and Encoder Value
	{
                                                                        //            NXT Display
  nxtDisplayCenteredTextLine(0, "Servo Position");                      //         __________________
  nxtDisplayCenteredBigTextLine(2, "%d", e);                            //        |  Servo Position  |
                                                                        //        |      ##  ##      |
  nxtDisplayCenteredTextLine(5, "%d", nMotorEncoder[motorA]);           //        |      ##  ##      |
  nxtDisplayCenteredTextLine(7, "Encoder Position");                    //        |                  |
                                                                        //        |        ##        |
  wait1Msec(25);      // Update screen every 25 milliseconds            //        |__Encoder  Value__|
	}


servo[servo2] = 128;        // 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.
wait1Msec(500);             // delay to allow for servo to get to position.


// FOLLOWING CODE USED TO DETERMINE POSITION OF CENTER CONSOLE
	if (SensorValue[S3] == 5)     // center piece is in Position 3 with bar just off to right
   		{
   			nMotorEncoder[LeftF] = 0;  //clear the TETRIX encoders in motors left and right
        nMotorEncoder[RightF] = 0;
        wait1Msec(200);							// Delay before next command
				//Move forward off of wall
        motor[LeftF] = 70;
 				motor[RightF] = 70;
 				wait10Msec(1000);
				//Right Turn 15 degrees
        while (nMotorEncoder[LeftF] > -300) //while the encoder wheel turns one revolution
        {
	        motor[LeftF] = -60; //turn both motors on at 60 percent power
	        motor[RightF] = 60;
        }
        motor[LeftF] = 0; //turn both motors off
        motor[RightF] = 0;
        //Move forward to knock off bar
        motor[LeftF] = 70;
 				motor[RightF] = 70;
 				wait10Msec(3000);
     	}

//motor(LeftF)= 50;             // move away from wall
//motor(RightF)= 50;
//motor(LeftR)= 50;
//motor(RightR)= 50;
//wait1Msec(200);
//motor(LeftF)= 0;
//motor(RightF)= 0;
//motor(LeftR)= 0;
//motor(RightR)= 0;
//motor(LeftF)= 50;              // Turn right 20 degrees.  Need gyro commands
//motor(RightR)= -50;

 if (SensorValue[S3] > 1) & (SensorValue[S3] < 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
        //Move straight ahead
				motor[LeftF] = 70;
 				motor[RightF] = 70;
 				wait10Msec(3000);
		}

 if (SensorValue[S3] == 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
				//Move straight ahead
				motor[LeftF] = 70;
 				motor[RightF] = 70;
 				wait10Msec(3000);
		}

}