task main()
{
// This task dictates what the robot will do during the autonomous period
waitForStart(); // Wait for the start of the autonomous period
StartTask(counter); // Begin timing the program, for use later
PlaySoundFile("Leroy.rso"); // Shout "LEEROY JENKINS!", just for fun
nMotorEncoder(motorLift1) = 0; // Reset the motor encoders for the arm
servoTarget(clawL) = 55; // Close the claw to hold the ring
servoTarget(clawR) = 170;
wait1Msec(100); // Wait a tenth of a second
strafeToIR(); // Strafe parallel to the scoring rack until it is lined up with the IR beacon
wait1Msec(1000); // Wait for one second after this has been completed
strafeToIR(); // Strafe parallel to the scoring rack until it is lined up with the IR beacon
wait1Msec(1000); // Wait for one second after this has been completed
// Note: this is done twice to ensure accuracy
motor[motorFL] = 0; // Wait one tenth of a second in the stopped position
motor[motorFR] = 0;
motor[motorBR] = 0;
motor[motorBL] = 0;
wait1Msec(100);
// The following code is used to raise the arm to the correct height:
nMotorEncoderTarget[motorLift1] = -scoringPegHeight; // Set the motor encoder target for the arm to the height of the scoring peg
motor[motorLift1] = -armSpeed; // Raise the arm at the designated speed
while(nMotorRunState[motorLift1] != runStateIdle) // While the arm is still moving (hasn't reached the target yet)
{
// Do not continue
}
motor[motorLift1] = 0; // Once the arm has reached its target, stop moving
motor[motorFL] = 100; // Drive forward towards the scoring rack
motor[motorFR] = 100;
motor[motorBR] = 100;
motor[motorBL] = 100;
wait1Msec(3000);
motor[motorFL] = 0; // Wait one second in the stopped position
motor[motorFR] = 0;
motor[motorBR] = 0;
motor[motorBL] = 0;
wait1Msec(1000);
servoTarget(clawL) = 115; // Open the claw to place the ring on the scoring rack
servoTarget(clawR) = 100;
wait1Msec(1000);
motor[motorFL] = -100; // Reverse away from the scoring rack, leaving the ring on the rack
motor[motorFR] = -100;
motor[motorBR] = -100;
motor[motorBL] = -100;
wait1Msec(1800);
motor[motorFL] = 0; // Wait one second in the stopped position
motor[motorFR] = 0;
motor[motorBR] = 0;
motor[motorBL] = 0;
wait1Msec(1000);
servoTarget(clawL) = 55; // Close the claw
servoTarget(clawR) = 170;
wait1Msec(1000);
// The following code is used to lower the arm back down:
nMotorEncoderTarget[motorLift1] = 100; // Set the motor encoder target for the arm to the height of the scoring peg
motor[motorLift1] = armSpeed; // Raise the arm at the designated speed
while(nMotorRunState[motorLift1] != runStateIdle) // While the arm is still moving (hasn't reached the target yet)
{
// Do not continue
}
motor[motorLift1] = 0; // Once the arm has reached its target, stop moving
}
void clearEncoders () {
nMotorEncoder(leftWheel13) = 0;
nMotorEncoder(rightWheel13) = 0;
}
