diff --git a/main.c b/main.c index 73b6520..b33f95f 100644 --- a/main.c +++ b/main.c @@ -82,16 +82,63 @@ void pre_auton() { bStopTasksBetweenModes = true; } -void driveTiles(int numberOfTiles) { - while(numberOfTiles * TILE > nMotorEncoder[1]) [ - // TODO +void driveTiles(int numberOfTiles, boolean direction) { + + while(direction && numberOfTiles * TILE > nMotorEncoder[1]) [ + if(abs(nMotorEncoder[1]) - 10 > nMotorEncoder[2]) { + motor[driveLB] = 100; + motor[driveLF] = 100; + motor[driveRB] = 90; + motor[driveRF] = 90; + } + if(abs(nMotorEncoder[2]) - 10 > nMotorEncoder[1]) { + motor[driveLB] = 90; + motor[driveLF] = 90; + motor[driveRB] = 100; + motor[driveRF] = 100; + } else { + motor[driveLB] = 100; + motor[driveLF] = 100; + motor[driveRB] = 100; + motor[driveRF] = 100; + } + } + while(!direction && numberOfTiles * TILE > nMotorEncoder[1]) [ + if(abs(nMotorEncoder[1]) - 10 > nMotorEncoder[2]) { + motor[driveLB] = -100; + motor[driveLF] = -100; + motor[driveRB] = -90; + motor[driveRF] = -90; + } + if(abs(nMotorEncoder[2]) - 10 > nMotorEncoder[1]) { + motor[driveLB] = -90; + motor[driveLF] = -90; + motor[driveRB] = -100; + motor[driveRF] = -100; + } else { + motor[driveLB] = -100; + motor[driveLF] = -100; + motor[driveRB] = -100; + motor[driveRF] = -100; + } } } task autonomous() { shootBall(); - + driveTiles(2, true); + //driveTiles(1, false); +} +void turnRight(int turns) { + while(turns * 360 < nMotorEncoder[1] +} +void turnLeft(int turns) { + +} + +void clearEnc() { + nMotorEncoder[1] = 0; + nMotorEncoder[2] = 0; } - task usercontrol() { while (true) { //Dual Driving