task main() { wait1Msec(3); int beginning = beginInterface(); wait1Msec(500); int ending = interface(); wait1Msec(500); int timing = timeinterface(); servo(servo3) = 0; waitForStart(); wait10Msec(timing * 100); initialize(); wait10Msec(30); switch(beginning) // test 'nTaskToStart' in the switch { case 2: // Move away from the wall motor[MOTOR_LEFT] = -40; motor[MOTOR_RIGHT] = -40; driveTo(4); motor[MOTOR_LEFT] = 0; motor[MOTOR_RIGHT] = 0; // Turn toward the west wall gyroTurn(20, 87); motor[MOTOR_LEFT] = 0; motor[MOTOR_RIGHT] = 0; nMotorEncoder[MOTOR_LEFT] = 0; nMotorEncoder[MOTOR_RIGHT] = 0; // Drive toward the west wall motor[MOTOR_LEFT] = -40; motor[MOTOR_RIGHT] = -40; driveTo(25); motor[MOTOR_LEFT] = 0; motor[MOTOR_RIGHT] = 0; // Turn toward the baskets gyroTurn(-20, 40); nMotorEncoder[MOTOR_LEFT] = 0; nMotorEncoder[MOTOR_RIGHT] = 0; case 3: //Move away from the wall motor[MOTOR_LEFT] = -40; motor[MOTOR_RIGHT] = -40; driveTo(2); motor[MOTOR_LEFT] = 0; motor[MOTOR_RIGHT] = 0; case 4: case 1: default: } //------------------------------- // We're aligned with the baskets. // Start walking along them looking for the IR beacon motor[MOTOR_LEFT] = -60; motor[MOTOR_RIGHT] = -63; driveTo(4); if(CheckIR(4) || checkIR(5)) // The first basket { motor[MOTOR_LEFT] = 0; motor[MOTOR_RIGHT] = 0; wait10Msec(4); servoTarget(servo3) = 180; wait10Msec(50); //forward/back }else { // The second basket driveTo(13); if(checkIR(4) || checkIR(5)) { motor[MOTOR_LEFT] = 0; motor[MOTOR_RIGHT] = 0; servoTarget(servo3) = 180; wait10Msec(150); //forward/back }else{ // The third basket driveTo(35); if(sensorValue[IRsensor] == 6 || sensorValue[IRsensor] == 5) { motor[MOTOR_LEFT] = 0; motor[MOTOR_RIGHT] = 0; servoTarget(servo3) = 180; wait10Msec(150); //place block //forward/back }else{ // The fourth basket driveTo(45); motor[MOTOR_LEFT] = 0; motor[MOTOR_RIGHT] = 0; servoTarget(servo3) = 180; // Deploy the auto-scoring arm wait10Msec(150); //forward/back } } } servoTarget[servo3] = 0; // Retract the auto-scoring arm switch(ending) // test 'nTaskToStart' in the switch { case 1: // if 'nTaskToStart' is '1': // Drive to the end of the baskets. motor[MOTOR_LEFT] = 100; motor[MOTOR_RIGHT] = 97; while(abs(nMotorEncoder[MOTOR_RIGHT]) > 2000) { } motor[MOTOR_LEFT] = 0; motor[MOTOR_RIGHT] = 0; // Turn toward the ramp gyroTurn(20, 65); nMotorEncoder[MOTOR_LEFT] = 0; nMotorEncoder[MOTOR_RIGHT] = 0; // Drive in front of the ramp motor[MOTOR_LEFT] = 90; motor[MOTOR_RIGHT] = 90; driveTo(20); motor[MOTOR_LEFT] = 0; motor[MOTOR_RIGHT] = 0; // Turn toward the ramp agai gyroTurn(20, 30); nMotorEncoder[MOTOR_LEFT] = 0; nMotorEncoder[MOTOR_RIGHT] = 0; motor[MOTOR_LEFT] = 90; motor[MOTOR_RIGHT] = 90; driveTo(25); motor[MOTOR_LEFT] = 0; motor[MOTOR_RIGHT] = 0; gyroTurn(-20, 85); nMotorEncoder[MOTOR_LEFT] = 0; nMotorEncoder[MOTOR_RIGHT] = 0; motor[MOTOR_LEFT] = -100; motor[MOTOR_RIGHT] = -100; driveTo(40); motor[MOTOR_LEFT] = 0; motor[MOTOR_RIGHT] = 0; break; // break out of this switch statement and continue code after the '}' case 2: // if 'nTaskToStart' is '2': motor[MOTOR_LEFT] = -90; motor[MOTOR_RIGHT] = -90; driveTo(51); motor[MOTOR_LEFT] = 0; motor[MOTOR_RIGHT] = 0; nMotorEncoder[MOTOR_LEFT] = 0; nMotorEncoder[MOTOR_RIGHT] = 0; motor[MOTOR_LEFT] = -90; motor[MOTOR_RIGHT] = -90; driveTo(10); motor[MOTOR_LEFT] = 0; motor[MOTOR_RIGHT] = 0; gyroTurn(-40, 70); nMotorEncoder[MOTOR_LEFT] = 0; nMotorEncoder[MOTOR_RIGHT] = 0; motor[MOTOR_LEFT] = -50; motor[MOTOR_RIGHT] = -50; driveTo(35); motor[MOTOR_LEFT] = 0; motor[MOTOR_RIGHT] = 0; gyroTurn(-40, 90); nMotorEncoder[MOTOR_LEFT] = 0; nMotorEncoder[MOTOR_RIGHT] = 0; motor[MOTOR_LEFT] = -90; motor[MOTOR_RIGHT] = -90; driveTo(35); motor[MOTOR_LEFT] = 0; motor[MOTOR_RIGHT] = 0; /* gyroTurn(-20, 70); nMotorEncoder[MOTOR_LEFT] = 0; nMotorEncoder[MOTOR_RIGHT] = 0; motor[MOTOR_LEFT] = 90; motor[MOTOR_RIGHT] = 90; driveTo(40); motor[MOTOR_LEFT] = 0; motor[MOTOR_RIGHT] = 0; */ // start task Two break; // break out of this switch statement and continue code after the '}' default: // if 'nTaskToStart' is anything other than '1' or '2': // start task Three } }
task main() { waitForStart(); // Wait for the tele-op period to begin nMotorEncoder(motorLift1) = 0; // Reset the motor encoders for the arm servoTarget(clawL) = 225; // Initialize the claw to be open servoTarget(clawR) = 60; PlaySoundFile("Leroy.rso"); // Shout "LEEROY JENKINS!", just for fun wait1Msec(10); // Wait one tenth of a second while (true) { getJoystickSettings(joystick); // Read the value of the joysticks nxtDisplayTextLine(6, "Encoder: %d", nMotorEncoder[motorLift1]); // Display the value of the arm encoder // The following code makes the directional pad on controller 1 give precise digital control of the drivetrain: if (joystick.joy1_TopHat == 0) // If "up" on the directional pad is pressed: { go_forward(25); // Go forward at 25% speed } else if (joystick.joy1_TopHat == 1) // Else if "top right" on the directional pad is pressed: { strafe_forward_right(25); // Strafe diagonally forward and to the right at 25% speed } else if (joystick.joy1_TopHat == 2) // Else if "right" on the directional pad is pressed: { strafe_right(25); // Strafe to the right at 25% speed } else if (joystick.joy1_TopHat == 3) // Else if "bottom right" on the directional pad is pressed: { strafe_backward_right(25); // Strafe diagonally backward and to the right 25% speed } else if (joystick.joy1_TopHat == 4) // Else if "down" on the directional pad is pressed: { go_backward(25); // Go backward 25% speed } else if (joystick.joy1_TopHat == 5) // Else if "bottom left" on the directional pad is pressed: { strafe_backward_left(25); // Strafe diagonally backward and to the left 25% speed } else if (joystick.joy1_TopHat == 6) // Else if "left" on the directional pad is pressed: { strafe_left(25); // Strafe left 25% speed } else if (joystick.joy1_TopHat == 7) // Else if "top left" on the directional pad is pressed: { strafe_forward_left(25); // Strafe diagonally forward and to the left 25% speed } else if (joy1Btn(5)) // Else if button 6 is pressed: { rotate_clockwise(25); // Rotate the robot clockwise 25% speed } else if (joy1Btn(6)) // Else if button 5 is pressed: { rotate_counter_clockwise(25); // Rotate the robot counter-clockwise 25% speed } // The following code makes the joysticks on controller 1 give analogue control of the drivetrain: else { motor[motorFL] = scale_motor(joystick.joy1_y1 + joystick.joy1_x1 + joystick.joy1_x2); // Scale the motors to the average motor[motorFR] = scale_motor(joystick.joy1_y1 + -joystick.joy1_x1 - joystick.joy1_x2); //of the left joystick Y value and motor[motorBR] = scale_motor(joystick.joy1_y1 + joystick.joy1_x1 - joystick.joy1_x2); //the right joystick X value motor[motorBL] = scale_motor(joystick.joy1_y1 + -joystick.joy1_x1 + joystick.joy1_x2); } // The following code assigns the shoulder buttons on controller 2 to open and close the claw: if (joy2Btn(5)) { servoTarget(clawL) = 55; // If button 5 is pressed on controller 2, close the claw servoTarget(clawR) = 170; } else if (joy2Btn(6)) { servoTarget(clawL) = 115; // If button 6 is pressed on controller 2, open the claw servoTarget(clawR) = 100; } else if (joy2Btn(8)) { servoTarget(clawL) = ServoValue(clawL) + 1; // While button 7 is held, slowly close the claw servoTarget(clawR) = ServoValue(clawR) - 1; } else if (joy2Btn(7)) { servoTarget(clawL) = ServoValue(clawL) - 1; // While button 8 is held, slowly open the claw servoTarget(clawR) = ServoValue(clawR) + 1; } else { servoTarget(clawL) = ServoValue(clawL); // Otherwise, do not move the claw servoTarget(clawR) = ServoValue(clawR); } // The following code gives analogue control of the arm with the joystick, and assigns buttons 1-4 to move the arm to their respective rows on the scoring rack if (joy2Btn(1)) // If button 1 is pressed, move the arm to the lowest row on the scoring rack { moveArm(level1Value); } else if (joy2Btn(2)) // If button 2 is pressed, move the arm to the middle row on the scoring rack { moveArm(level2Value); } else if (joy2Btn(3)) // If button 3 is pressed, move the arm to the top row on the scoring rack { moveArm(level3Value); } else if (joy2Btn(4)) // If button 4 is pressed, lower the arm all the way { moveArm(0); } else { motor[motorLift1] = (joystick.joy2_y1 / 12); // Otherwise, control the arm with joystick 1 on controller 2 motor[motorLift1] = (joystick.joy2_y1 / 24); // Or control the arm at half speed with joystick 2 on controller 2 // Note: these values are divided by 12 and 24 to bring the arm speed down to a reasonable level } } }
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 }