Autonomous.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$ */

#define ALL_USER_INPUT
#define PARK_MODES
#define GYRO
#define MOVE_GYRO
#define RIGHT_GYRO_TURN
#define LEFT_GYRO_TURN
#define IR_SEEKER
#define BALL_GRAB

#include "Autonomous.h"

/**
 * Autonomous modes depending on user input.
 */
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(-80, MOTOR_FULL);
	}
	else
	{
	  // turn right
    rightGyroTurn(85, MOTOR_FULL);
	}

  motor[ballArm] = MOTOR_FULL;
  wait1Msec(450);
  motor[ballArm] = MOTOR_OFF;

  // move forward
  moveGyro(MOTOR_FULL, startPosition == START_INSIDE ? MOVE_TO_BBALL_TIME : MOVE_OUTSIDE_TO_BBALL_TIME);

  wait1Msec(TURN_WAIT_MS);

  if(startColor == START_RED)
  {
	  // slight left
	  leftGyroTurn(-120, MOTOR_FULL);
	}
	else
	{
	  // slight right
	  rightGyroTurn(125, MOTOR_FULL);
	}

  StartTask(BallGrab);

  wait1Msec(TURN_WAIT_MS);

  if(bBallPark == PARK_FRONT)
  {
    // go to the front parking zone
	  moveGyro(MOTOR_FULL, MOVE_TO_CORNER_TIME);
	}
  else //if(bBallPark == PARK_BACK)
  {
    // go to the back parking zone
    moveGyro(MOTOR_FULL, MOVE_TO_BACK_JUKE_TIME);

    if(SensorValue[SONAR] > 10)
    {
      PlaySound(soundBeepBeep);
      moveGyro(MOTOR_FULL, MOVE_TO_CORNER_TIME - MOVE_TO_BACK_JUKE_TIME);
      return;
    }

    wait1Msec(TURN_WAIT_MS);

    if(startColor == START_RED)
    {
	    rightGyroTurn(30, MOTOR_FULL);
	  }
	  else
	  {
	    leftGyroTurn(-15, MOTOR_FULL);
	  }

	  wait1Msec(TURN_WAIT_MS);

	  if(robotPark == PARK_FRONT)
    {
      // leave the bowling ball in the back and park in the front
      motor[ballArm] = MOTOR_FULL;
      wait1Msec(450);
      motor[ballArm] = MOTOR_OFF;

      moveGyro(MOTOR_FULL, MOVE_TO_BACK_TIME - 500);

      wait1Msec(500);

      moveGyro(-MOTOR_FULL, MOVE_FROM_BACK_TIME);

      for(int i = 0; i < 3; i++)
      {
        turnToIRBeacon(MOTOR_FULL);
        moveGyro(-MOTOR_FULL, MOVE_TO_BEACON_TIME);
      }
    }
    else
    {
      // bowling ball and robot in the back
      moveGyro(MOTOR_FULL, MOVE_TO_BACK_TIME);
    }
  }
}