diff --git a/main.c b/main.c index 6062de1..4de7762 100644 --- a/main.c +++ b/main.c @@ -19,10 +19,10 @@ // some variable definitions #define MAX_SPEED 127 // Max speed of the motors -#define STOP 0 +#define STOP 0 #define DEADZONE 10 -/* +/* 23.4 / (2 * pi * 2.075) * 672.2 23.4 = inches in a tile 2.075 = exact radius of 4" omniwheels @@ -33,7 +33,7 @@ #define TILE 1206 // How much the wheels should spin in a 90 degree turn -#define POINTS_PER_TURN 320 +#define POINTS_PER_TURN 320 // definitions for driveTiles() #define FORWARD true @@ -41,12 +41,12 @@ void clearEnc() { // Reset driving motor encoder values to 0 nMotorEncoder[driveRB] = 0; - nMotorEncoder[driveLB] = 0; + nMotorEncoder[driveLB] = 0; } void shootBall() { motor[shoot] = MAX_SPEED; - wait(1.25); // Shooting takes 1.25 seconds. + wait(1.25); // Shooting takes 1.25 seconds. // Any unwanted extra movement will be undone by the rubber bands. motor[shoot] = STOP; } @@ -54,12 +54,12 @@ void turntoRight(float turns) { clearEnc(); while(turns * POINTS_PER_TURN > nMotorEncoder[driveLB]){ motor[driveLB] = 100; - motor[driveLF] = 100; + motor[driveLF] = 100; motor[driveRB] = -100; motor[driveRF] = -100; } motor[driveLB] = 0; - motor[driveLF] = 0; + motor[driveLF] = 0; motor[driveRB] = 0; motor[driveRF] = 0; } @@ -67,16 +67,16 @@ void turntoLeft(float turns) { clearEnc(); while(turns * POINTS_PER_TURN > nMotorEncoder[driveRB]){ motor[driveLB] = -100; - motor[driveLF] = -100; + motor[driveLF] = -100; motor[driveRB] = 100; motor[driveRF] = 100; - } + } motor[driveLB] = 0; - motor[driveLF] = 0; + motor[driveLF] = 0; motor[driveRB] = 0; motor[driveRF] = 0; } - + void flipOn() { motor[bintake] = -MAX_SPEED; @@ -100,7 +100,7 @@ void joystickDrive() { if(abs(vexRT[Ch2]) > DEADZONE) { motor[driveRB] = vexRT[Ch2]; motor[driveRF] = vexRT[Ch2]; - } + } else { motor[driveRB] = STOP; motor[driveRF] = STOP; @@ -131,24 +131,24 @@ void pre_auton() { bStopTasksBetweenModes = true; } -void driveTiles(float numberOfTiles, bool direction) { +void driveTiles(float numberOfTiles, bool direction) { // when direction is true, move forward, otherwise go in reverse clearEnc(); while(direction == FORWARD && numberOfTiles * TILE - 200 > nMotorEncoder[driveRB]) { if(abs(nMotorEncoder[driveRB]) - 10 > nMotorEncoder[driveLB]) { motor[driveLB] = 100; - motor[driveLF] = 100; + motor[driveLF] = 100; motor[driveRB] = 90; motor[driveRF] = 90; } if(abs(nMotorEncoder[driveLB]) - 10 > nMotorEncoder[driveRB]) { motor[driveLB] = 90; - motor[driveLF] = 90; + motor[driveLF] = 90; motor[driveRB] = 100; motor[driveRF] = 100; } else { motor[driveLB] = 100; - motor[driveLF] = 100; + motor[driveLF] = 100; motor[driveRB] = 100; motor[driveRF] = 100; } @@ -156,65 +156,68 @@ void driveTiles(float numberOfTiles, bool direction) { while(direction == REVERSE && numberOfTiles * TILE - 200 > -nMotorEncoder[driveRB]) { if(abs(nMotorEncoder[driveRB]) - 10 > nMotorEncoder[driveLB]) { motor[driveLB] = -100; - motor[driveLF] = -100; + motor[driveLF] = -100; motor[driveRB] = -90; motor[driveRF] = -90; } if(abs(nMotorEncoder[driveLB]) - 10 > nMotorEncoder[driveLB]) { motor[driveLB] = -90; - motor[driveLF] = -90; + motor[driveLF] = -90; motor[driveRB] = -100; motor[driveRF] = -100; } else { motor[driveLB] = -100; - motor[driveLF] = -100; + motor[driveLF] = -100; motor[driveRB] = -100; motor[driveRF] = -100; } } motor[driveLB] = 0; - motor[driveLF] = 0; + motor[driveLF] = 0; motor[driveRB] = 0; motor[driveRF] = 0; } task autonomous() { - turntoRight(0.05); + turntoRight(0.03); shootBall(); - turntoLeft(0.05); + 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.5, FORWARD); // flip cap + driveTiles(1.6, FORWARD); // flip cap ballOff(); - driveTiles(1.25, REVERSE); + driveTiles(1.35, REVERSE); turntoLeft(1); driveTiles(1, REVERSE); turntoRight(1); driveTiles(0.6, REVERSE); - driveTiles(2.5, FORWARD); // Flip the other cap without turning on the spinner + driveTiles(2.1, FORWARD); // Flip the other cap without turning on the spinner flipOn(); driveTiles(0.5, FORWARD); - driveTiles(0.1, FORWARD); - ballIn(); // So we can pick up the ball! + ballIn(); + driveTiles(0.1, REVERSE); + // So we can pick up the ball! wait(3); - driveTiles(0.1, FORWARD); + driveTiles(0.1, REVERSE); turntoLeft(1); - driveTiles(0.25, REVERSE); + driveTiles(0.2, REVERSE); wait(3); ballOff(); shootBall(); - driveTiles(0.25, FORWARD); // Hit middle column bottom flag + driveTiles(0.05, REVERSE); + driveTiles(0.33, FORWARD); // Hit middle column bottom flag //driveTiles(2, REVERSE); + wait(2); turntoRight(1); - driveTiles(2.5, REVERSE); + driveTiles(2.2, REVERSE); turntoLeft(1); driveTiles(1, REVERSE); turntoRight(1); driveTiles(0.25, REVERSE); driveTiles(3, FORWARD); - + } task usercontrol() { // In user control mode while (true) {