diff --git a/main.c b/main.c index b33f95f..b25d77d 100644 --- a/main.c +++ b/main.c @@ -1,11 +1,11 @@ - #pragma config(I2C_Usage, I2C1, i2cSensors) +#pragma config(I2C_Usage, I2C1, i2cSensors) #pragma config(Sensor, I2C_1, , sensorQuadEncoderOnI2CPort, , AutoAssign ) #pragma config(Sensor, I2C_2, , sensorQuadEncoderOnI2CPort, , AutoAssign ) #pragma config(Motor, port1, , tmotorVex393_HBridge, openLoop) #pragma config(Motor, port2, shoot, tmotorVex393_MC29, openLoop, reversed) -#pragma config(Motor, port3, driveLB, tmotorVex393_MC29, openLoop, reversed, encoderPort, I2C_1) +#pragma config(Motor, port3, driveLB, tmotorVex393_MC29, openLoop, reversed, encoderPort, I2C_2) #pragma config(Motor, port4, driveLF, tmotorVex393_MC29, openLoop, reversed) -#pragma config(Motor, port5, driveRB, tmotorVex393_MC29, openLoop, reversed, encoderPort, I2C_2) +#pragma config(Motor, port5, driveRB, tmotorVex393_MC29, openLoop, reversed, encoderPort, I2C_1) #pragma config(Motor, port6, driveRF, tmotorVex393_MC29, openLoop) #pragma config(Motor, port7, bintake, tmotorVex393_MC29, openLoop, reversed) #pragma config(Motor, port8, , tmotorVex393_MC29, openLoop) @@ -37,7 +37,7 @@ void shootBall() { wait(1.25); // Shooting takes 1.25 seconds. motor[shoot] = STOP; } - +void clearEnc(); void joystickDrive() { if(abs(vexRT[Ch3]) > DEADZONE) { motor[driveLB] = vexRT[Ch3]; @@ -82,46 +82,50 @@ void pre_auton() { bStopTasksBetweenModes = true; } -void driveTiles(int numberOfTiles, boolean direction) { - - while(direction && numberOfTiles * TILE > nMotorEncoder[1]) [ - if(abs(nMotorEncoder[1]) - 10 > nMotorEncoder[2]) { +void driveTiles(int numberOfTiles, bool direction) { + clearEnc(); + while(direction && numberOfTiles * TILE > nMotorEncoder[1]) { + if(abs(nMotorEncoder[1]) - 10 > nMotorEncoder[2]) { motor[driveLB] = 100; motor[driveLF] = 100; - motor[driveRB] = 90; + motor[driveRB] = 90; motor[driveRF] = 90; } - if(abs(nMotorEncoder[2]) - 10 > nMotorEncoder[1]) { + if(abs(nMotorEncoder[2]) - 10 > nMotorEncoder[1]) { motor[driveLB] = 90; motor[driveLF] = 90; - motor[driveRB] = 100; + motor[driveRB] = 100; motor[driveRF] = 100; } else { - motor[driveLB] = 100; + motor[driveLB] = 100; motor[driveLF] = 100; - motor[driveRB] = 100; + motor[driveRB] = 100; motor[driveRF] = 100; } } - while(!direction && numberOfTiles * TILE > nMotorEncoder[1]) [ - if(abs(nMotorEncoder[1]) - 10 > nMotorEncoder[2]) { + while(!direction && numberOfTiles * TILE > nMotorEncoder[1]) { + if(abs(nMotorEncoder[1]) - 10 > nMotorEncoder[2]) { motor[driveLB] = -100; motor[driveLF] = -100; - motor[driveRB] = -90; + motor[driveRB] = -90; motor[driveRF] = -90; } - if(abs(nMotorEncoder[2]) - 10 > nMotorEncoder[1]) { + if(abs(nMotorEncoder[2]) - 10 > nMotorEncoder[1]) { motor[driveLB] = -90; motor[driveLF] = -90; - motor[driveRB] = -100; + motor[driveRB] = -100; motor[driveRF] = -100; } else { - motor[driveLB] = -100; + motor[driveLB] = -100; motor[driveLF] = -100; - motor[driveRB] = -100; + motor[driveRB] = -100; motor[driveRF] = -100; } } + motor[driveLB] = 0; + motor[driveLF] = 0; + motor[driveRB] = 0; + motor[driveRF] = 0; } task autonomous() { shootBall(); @@ -129,10 +133,14 @@ task autonomous() { //driveTiles(1, false); } void turnRight(int turns) { - while(turns * 360 < nMotorEncoder[1] + while(turns * 360 < nMotorEncoder[1]){ + + } } void turnLeft(int turns) { - + while(turns * 360 < nMotorEncoder[1]){ + + } } void clearEnc() {