102 lines
3.7 KiB
C
102 lines
3.7 KiB
C
|
||
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().
|