void pre_auton()
{
initializeDisplay();
initializeGyro(); //Uncomment when we start using Gyro Sensor
initializePID();
initializeAutonomous();
while (bIfiRobotDisabled); //Keep the message displayed while robot is disabled
}
task main()
{
phase = AUTO;
driveCycles = 6;
humor = 80;
bDisplayDiagnostics = false;
initializeAutonomous();
bDisplayDiagnostics = true;
waitForStart();
motor [lift] = 100;
wait1Msec(750);
motor [lift] = 0;
wait1Msec(250);
while (nMotorEncoder [frontRight] > -(driveCycles * driveEncoderCycle))
{
drive(0, -25, 0);
}
while (SensorValue [sonar] > 50)
{
drive(0, 0, 25);
}
wait1Msec(550);
allStop();
StartTask(failSafe);
while (SensorValue [sonar] > 20)
{
drive(0, -20, 0);
}
StopTask(failSafe);
wait1Msec(500);
allStop();
servo [leftHook] = 168;
servo [rightHook] = 16;
wait1Msec(50);
score(60);
wait1Msec(200);
drive(0, 0, -100);
motor [arm] = 100;
servo [pivot] = 245;
wait1Msec(1000);
motor [arm] = 0;
wait1Msec(500);
allStop();
wait1Msec(50);
drive(-100, 0, 0);
wait1Msec(1700);
drive(0, -100, 0);
wait1Msec(4500);
allStop();
}
task main()
{
driveCycles = 6;
humor = 80;
bDisplayDiagnostics = false;
initializeAutonomous();
bDisplayDiagnostics = true;
waitForStart();
motor [lift] = 100;
wait1Msec(750);
motor [lift] = 0;
wait1Msec(250);
if (SensorValue [ir] == 5)
{
//while (nMotorEncoder [frontRight] > -(driveCycles * driveEncoderCycle))
//{
// drive(0, 25, 0);
//}
while (SensorValue [sonar] > 45)
{
drive(0, 100, 0);
}
allStop();
//begin precision rotate cycle
while (SensorValue [ir] > 4)
{
drive(0, 0, 60);
}
allStop();
ClearTimer(T2);
while (SensorValue [ir] < 6)
{
drive(0, 0, -60);
}
int spinTime = time1[T2];
allStop();
ClearTimer(T2);
while (time1[T2] < spinTime)
{
drive(0, 0, 60);
}
allStop();
}
}