Code to run the microcontrollers on the R5 competition bot
Dependencies: LineSensors mbed
Diff: DriveController.cpp
- Revision:
- 9:01c17b286a99
- Parent:
- 8:9030d2e3a1e8
- Child:
- 10:210c8f1e3a92
diff -r 9030d2e3a1e8 -r 01c17b286a99 DriveController.cpp --- a/DriveController.cpp Sun Oct 19 02:39:17 2014 +0000 +++ b/DriveController.cpp Fri Oct 24 22:46:24 2014 +0000 @@ -1,6 +1,9 @@ #include "DriveController.h" -DriveController::DriveController() : i2c(PTC2, PTC1), wheelSpeed1(PTB0), wheelSpeed2(PTB1), wheelSpeed3(PTB2), +#define MOTOR_SCALE 0.5 +#define CORRECTION_SCALE 0.09 + +DriveController::DriveController() : debugLog("log.txt"), i2c(PTC2, PTC1), wheelSpeed1(PTB0), wheelSpeed2(PTB1), wheelSpeed3(PTB2), wheelSpeed4(PTB3), wheelDirection1(PTE20), wheelDirection2(PTE21), wheelDirection3(PTE22), wheelDirection4(PTE23), sensors(PTC7, PTC0, PTC3, PTC4, PTC5, PTC6, PTC10, PTC11, PTC9, PTC8, PTA5, PTA4, PTA12, PTD4, PTA2, PTA1, PTC12, PTC13, PTC16, PTC17, PTD1, PTD0, PTD5, PTD13) @@ -10,6 +13,7 @@ void DriveController::go() { + debugLog << "Starting Drive Controller!" << endl; while(true) //test loop { move(); @@ -43,41 +47,62 @@ int error = 0; if(direction == 'B') + { + debugLog << "Moving Backward" << endl; wheelDirection1 = wheelDirection2 = wheelDirection3 = wheelDirection4 = 0; + } else + { + debugLog << "Moving Forward" << endl; wheelDirection1 = wheelDirection2 = wheelDirection3 = wheelDirection4 = 1; + } do { sensors.read(); sensors.lineDetect(sensorStates); + debugLog << "Updating Sensors" << endl; + debugLog << " 1 2 3 4 5 6 7 8" << endl; + debugLog << "----------------------------------" << endl; + for(int i = 0; i < 3; i++) + { + debugLog << i + 1 << " | "; + for(int j = 0; j < 8; j++) + { + debugLog << int(sensorStates[j][i]) << " "; + } + debugLog << endl; + } + if(intersection()) { - wheelSpeed1 = wheelSpeed3 = 1; - wheelSpeed2 = wheelSpeed4 = 1; + debugLog << "At Intersection" << endl; + wheelSpeed1 = wheelSpeed2 = wheelSpeed3 = wheelSpeed4 = MOTOR_SCALE; } else { error = calculateError(); + debugLog << "Error Calculated : " << error << endl; if(error < 0) { - wheelSpeed1 = wheelSpeed3 = 1.0 - abs(error)*0.09; - wheelSpeed2 = wheelSpeed4 = 1; + wheelSpeed1 = wheelSpeed3 = (1.0 - abs(error)*CORRECTION_SCALE)*MOTOR_SCALE; + wheelSpeed2 = wheelSpeed4 = MOTOR_SCALE; } else { - wheelSpeed1 = wheelSpeed3 = 1; - wheelSpeed2 = wheelSpeed4 = 1.0 - error*0.09; + wheelSpeed1 = wheelSpeed3 = MOTOR_SCALE; + wheelSpeed2 = wheelSpeed4 = (1.0 - error*CORRECTION_SCALE)*MOTOR_SCALE; } - if(atLine) - atLine = false; + atLine = false; } } while(!intersection() || atLine); wheelSpeed1 = wheelSpeed2 = wheelSpeed3 = wheelSpeed4 = 0; + debugLog << "Move Complete" << endl; + debugLog << "=====================================" << endl; } int DriveController::calculateError() @@ -143,26 +168,45 @@ { wheelDirection1 = wheelDirection3 = 0; wheelDirection2 = wheelDirection4 = 1; + debugLog << "Rotating Left" << endl; } else { wheelDirection1 = wheelDirection3 = 1; wheelDirection2 = wheelDirection3 = 0; + debugLog << "Rotating Right" << endl; } - wheelSpeed1 = wheelSpeed2 = wheelSpeed3 = wheelSpeed4 = 1; + wheelSpeed1 = wheelSpeed2 = wheelSpeed3 = wheelSpeed4 = MOTOR_SCALE; do { sensors.read(); sensors.lineDetect(sensorStates); + debugLog << "Updating Sensors" << endl; + debugLog << " 1 2 3 4 5 6 7 8" << endl; + debugLog << "----------------------------------" << endl; + for(int i = 0; i < 3; i++) + { + debugLog << i + 1 << " | "; + for(int j = 0; j < 8; j++) + { + debugLog << int(sensorStates[j][i]) << " "; + } + debugLog << endl; + } + if(!intersection()) atLine = false; } while(!intersection() || atLine); wheelSpeed1 = wheelSpeed2 = wheelSpeed3 = wheelSpeed4 = 0; + + debugLog << "Rotate Complete" << endl; + debugLog << "=====================================" << endl; + } void DriveController::getCommand()