Defense.c

#pragma config(Hubs,  S1, HTServo,  HTMotor,  HTMotor,  HTMotor)
#pragma config(Sensor, S2,     HTIRS2,         sensorI2CCustom)
#pragma config(Sensor, S3,     HTGYRO,         sensorAnalogInactive)
#pragma config(Sensor, S4,     SONAR,         sensorSONAR)
#pragma config(Motor,  motorA,          green,         tmotorNormal, openLoop)
#pragma config(Motor,  motorB,          yellow,        tmotorNormal, openLoop)
#pragma config(Motor,  motorC,          red,           tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C2_1,     ballArm,       tmotorNormal, openLoop, reversed)
#pragma config(Motor,  mtr_S1_C2_2,     clawArm,          tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C3_1,     intake,        tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C3_2,     spool,         tmotorNormal, openLoop, reversed)
#pragma config(Motor,  mtr_S1_C4_1,     right,          tmotorNormal, openLoop, reversed)
#pragma config(Motor,  mtr_S1_C4_2,     left,         tmotorNormal, openLoop, encoder)
#pragma config(Servo,  srvo_S1_C1_1,    clawRelease,                 tServoStandard)
#pragma config(Servo,  srvo_S1_C1_2,    armRelease,               tServoStandard)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

/* $Id: O-Back.c 163 2012-04-07 20:21:35Z 4105 $ */

#define ALL_USER_INPUT
#define GYRO
#define MOVE_GYRO
#define MOVE_TIMED
#define RIGHT_GYRO_TURN
#define LEFT_GYRO_TURN
#define IR_SEEKER

#include "Autonomous.h"

/**
 * Starting on the outside, drive to the back parking area.
 */
task main()
{
  initializeRobot();

  // Wait for the beginning of autonomous phase.
  waitForStart();

  StartTask(prettyLights);

    if(startPause)
  {
    wait1Msec(5000);
  }

  // go forward
  moveGyro(MOTOR_FULL, MOVE_OFF_RAMP_TIME);

  wait1Msec(TURN_WAIT_MS);

  if(startColor == START_RED)
  {
    // turn left
    leftGyroTurn(-25, MOTOR_FULL);
	}
	else
	{
	  // turn right
    rightGyroTurn(25, MOTOR_FULL);
	}

  wait1Msec(TURN_WAIT_MS);

  // go to the back parking area
  moveGyro(MOTOR_FULL, MOVE_TO_BACK_TIME - 2500);

  wait1Msec(TURN_WAIT_MS);

  moveGyro(-MOTOR_FULL, 3000);

  wait1Msec(TURN_WAIT_MS);

  if(startColor == START_RED)
  {
    // turn left
    rightGyroTurn(25, MOTOR_FULL);
	}
	else
	{
	  // turn right
    leftGyroTurn(-25, MOTOR_FULL);
	}

	wait1Msec(TURN_WAIT_MS);

  moveGyro(-MOTOR_FULL, 3000);

  for(int i = 0; i < 3; i++)
  {
    turnToIRBeacon(MOTOR_FULL);
    moveGyro(-MOTOR_FULL, MOVE_TO_BEACON_TIME);
  }
}