|
|
|
|
|
motor[driveRF] = -MAX_AUTO_SPEED + DRIVE_OFFSET;
|
|
|
}
|
|
|
if(abs(nMotorEncoder[driveLB]) - DRIVE_OFFSET > nMotorEncoder[driveLB]) {
|
|
|
motor[driveLB] = -MAX_AUTO_SPEED + DRIVE_OFFSET;
|
|
|
motor[driveLF] = -MAX_AUTO_SPEED + DRIVE_OFFSET;
|
|
|
motor[driveRB] = -MAX_AUTO_SPEED;
|
|
|
motor[driveRF] = -MAX_AUTO_SPEED;
|
|
|
} else {
|
|
|
motor[driveLB] = -MAX_AUTO_SPEED;
|
|
|
motor[driveLF] = -MAX_AUTO_SPEED;
|
|
|
motor[driveRB] = -MAX_AUTO_SPEED;
|
|
|
motor[driveRF] = -MAX_AUTO_SPEED;
|
|
|
}
|
|
|
}
|
|
|
stopDriving();
|
|
|
}
|
|
|
// Explicit Parameters: A floating point number numberOfTurns that represents the number of tiles that the
|
|
|
// robot is to drive. Since it is a floating point number, we can move by half or any fraction movement.
|
|
|
// There is also the boolean value direction that controls which way the robot is to move. true is for forward,
|
|
|
// and false is for reverse.
|
|
|
// Output: The robot will drive the specified amount of tiles, in the specified direction. If the robot is not driving
|
|
|
// straight, the speeds of the left and right motors can be offset from each other to cancel out any slight drifts
|
|
|
// to the left or right. We subtract 200 from the distance no matter what here, because the robot moves
|
|
|
// that much after it is told to stop.
|
|
|
|
|
|
task autonomous() {
|
|
|
turntoRight(0.03);
|
|
|
shootBall();
|
|
|
turntoLeft(0.03);
|
|
|
driveTiles(2, FORWARD); // Move 2 forward to hit bottom flag
|
|
|
driveTiles(1, REVERSE);
|
|
|
turntoRight(1);
|
|
|
driveTiles(0.5, REVERSE); // Drive 1/3 of a tile backwards to hit the wall and align ourselves!
|
|
|
flipOn(); // Turn on the ball intake in reverse, which is what we can use to flip the caps
|
|
|
driveTiles(1.6, FORWARD); // flip cap
|
|
|
ballOff();
|
|
|
driveTiles(1.35, REVERSE);
|
|
|
turntoLeft(1);
|
|
|
driveTiles(1, REVERSE);
|
|
|
turntoRight(1);
|
|
|
driveTiles(0.6, REVERSE);
|
|
|
driveTiles(2, FORWARD); // Flip the other cap without turning on the spinner
|
|
|
flipOn(); // So we can pick up the ball that's under it!
|
|
|
driveTiles(0.5, FORWARD);
|
|
|
ballIn();
|
|
|
driveTiles(0.3, REVERSE);
|
|
|
wait(3);
|
|
|
driveTiles(0.5, REVERSE);
|
|
|
turntoLeft(0.75);
|
|
|
//driveTiles(0.2, REVERSE);
|
|
|
wait(3);
|
|
|
ballOff();
|
|
|
shootBall();
|
|
|
turntoRight(0.75);
|
|
|
driveTiles(1.9, REVERSE);
|
|
|
turntoLeft(1);
|
|
|
driveTiles(1, REVERSE);
|
|
|
turntoRight(1);
|
|
|
driveTiles(0.25, REVERSE);
|
|
|
driveTiles(3, FORWARD);
|
|
|
}
|
|
|
/*
|
|
|
This is the autonomous task. Here’s the path of the robot, described in words instead of code:
|
|
|
|
|
|
1. Start at the red tile closest to the flags.
|
|
|
|
|
|
2. Turn a tiny bit to the right to aim, and shoot the top flag with our preload.
|
|
|
|
|
|
3. Re-align ourselves and drive to hit the bottom flag.
|
|
|
|
|
|
4. Back up 1 tile, turn right, and back into the wall to align the robot.
|
|
|
|
|
|
5. Drive forward with the flipper turned on, and flip the cap from blue to red.
|
|
|
|
|
|
6. Back up, turn left, reverse 1 tile, turn right, back into the wall again to align ourselves.
|
|
|
|
|
|
7. Drive forward, push the cap off of the ball, turn of the flipper and flip the cap.
|
|
|
|
|
|
8. Run the motors to lift the ball up to the ball launcher. We wait a few seconds for the ball.
|
|
|
|
|
|
9. Turn to the left, wait a bit more, and shoot the top flag in the middle column of flags.
|
|
|
|
|
|
10. Turn right, back up to the starting tile, then turn left.
|
|
|
|
|
|
11. Reverse for 1 tile to be perpendicular with the platforms.
|
|
|
|
|
|
12. Turn to face the parking platforms, and reverse into the wall to align ourselves again.
|
|
|
|
|
|
13. Climb to the middle parking platform and stop.
|
|
|
*/
|
|
|
|
|
|
|
|
|
task usercontrol() {
|
|
|
while (true) {
|
|
|
joystickDrive();
|
|
|
buttonChecks();
|
|
|
}
|
|
|
}
|
|
|
// When the driver is in control, this task runs. For the entire duration of the driver control period, we need
|
|
|
// to be able to control the robot, so we put everything in a while loop. The task calls 2 previously mentioned
|
|
|
// functions, joystickDrive() and buttonChecks().
|