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; }