diff --git a/main.c b/main.c index 642d94b..1808c3b 100644 --- a/main.c +++ b/main.c @@ -137,8 +137,28 @@ void driveTiles(int numberOfTiles, bool direction) { } task autonomous() { shootBall(); - driveTiles(2, true); // Move 2 forward to hit bottom flag - //driveTiles(1, false); + driveTiles(2, FORWARD); // Move 2 forward to hit bottom flag + driveTiles(1, REVERSE); + turnRight(1); + driveTiles(0.33, 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.25, FORWARD); // flip cap + ballOff(); + driveTiles(1, REVERSE); + turnLeft(1); + driveTiles(1, REVERSE); + turnRight(1); + driveTiles(0.33, REVERSE); + driveTiles(2.5, FORWARD); + ballIn(); + delay(100); + driveTiles(0.1, FORWARD); + turnLeft(1); + driveTiles(0.33, REVERSE); + delay(1000); + ballOff(); + shootBall(); + } void turnRight(int turns) { while(turns * POINTS_PER_TURN < nMotorEncoder[1]){ @@ -155,6 +175,15 @@ void clearEnc() { // Reset driving motor encoder values to 0 nMotorEncoder[1] = 0; nMotorEncoder[2] = 0; } +void flipOn() { + motor[bintake] = -MAX_SPEED; +} +void ballOff() { + motor[bintake] = 0; +} +void ballIn() { + motor[bintake] = MAX_SPEED; +} task usercontrol() { // In user control mode while (true) { joystickDrive(); // Joystick mapping function