Code to run the microcontrollers on the R5 competition bot
Dependencies: LineSensors mbed
Diff: DriveController.cpp
- Revision:
- 2:d0ce8e26cbc4
- Parent:
- 1:fa6eb0c33b2f
- Child:
- 3:0d7687b6ef14
diff -r fa6eb0c33b2f -r d0ce8e26cbc4 DriveController.cpp --- a/DriveController.cpp Wed Sep 24 21:03:56 2014 +0000 +++ b/DriveController.cpp Fri Oct 03 03:48:39 2014 +0000 @@ -1,16 +1,77 @@ #include "DriveController.h" +#define INTERSECTION_THRESHOLD = 8 + +DriveController::DriveController() +{ + direction = NORTH; + command.direction = NORTH; + command.distance = 0; +} + DriveController::void go() { + while(true) + { + getCommand(); + if(currentCommand.distance != 0) + move(); + } +} + +DriveController::void move() +{ + int travelled = 0; + + if(orientation != command.direction) + rotate(command.direction); + + while(travelled < command.distance) + { + forward(calculateError()); + if(intersection()) + travelled++; + } +} + +DriveController::double calculateError() +{ } -DriveController::void move(Direction direction) +DriveController::void forward(double correction) { - if(orientation != direction) + +} + +DriveController::bool intersection() +{ + int sensorTally = 0; + + readSensors(); + for(int i = 0; i < 24; i++) { - bool divert; - while(true) - { - readSensors(); + if(sensorStates[i]==true) + sensorTally++; + } + + if(sensorTally > INTERSECTION_THRESHOLD) + return true; + else + return false; +} +DriveController::void rotate(Direction diretion) +{ + +} + +DriveController::void getCommand() +{ + +} + +DriveControler::void readSensors() +{ + +} \ No newline at end of file