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
}
