![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
Code to run the microcontrollers on the R5 competition bot
Dependencies: LineSensors mbed
DriveController.cpp@3:0d7687b6ef14, 2014-10-03 (annotated)
- Committer:
- Hypna
- Date:
- Fri Oct 03 18:54:42 2014 +0000
- Revision:
- 3:0d7687b6ef14
- Parent:
- 2:d0ce8e26cbc4
- Child:
- 4:ac6b2e5b240b
did things
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Hypna | 0:e60f22c1d573 | 1 | #include "DriveController.h" |
Hypna | 0:e60f22c1d573 | 2 | |
Hypna | 3:0d7687b6ef14 | 3 | #define REFLECTION_THRESHOLD = 0 |
Hypna | 2:d0ce8e26cbc4 | 4 | |
Hypna | 3:0d7687b6ef14 | 5 | /*DriveController::DriveController() : wheel1(PTC7), wheel2(PTC2), wheel3(PTE29), wheel4(PTE30), sensor01(PTC7), |
Hypna | 3:0d7687b6ef14 | 6 | sensor02(PTC0), sensor03(PTC3), sensor04(PTC4), sensor05(PTC5), sensor06(PTC6), sensor07(PTC10), sensor08(PTC11), |
Hypna | 3:0d7687b6ef14 | 7 | sensor09(PTC9), sensor10(PTC8), sensor11(PTA5), sensor12(PTA4), sensor13(PTA12), sensor14(PTD4), sensor15(PTA2), |
Hypna | 3:0d7687b6ef14 | 8 | sensor16(PTA1), sensor17(PTC12), sensor18(PTC13), sensor19(PTC16), sensor20(PTC17), sensor21(PTD2), sensor22(PTD0), |
Hypna | 3:0d7687b6ef14 | 9 | sensor23(PTD5), sensor24(PTD13), orientation(NORTH) |
Hypna | 2:d0ce8e26cbc4 | 10 | { |
Hypna | 2:d0ce8e26cbc4 | 11 | command.direction = NORTH; |
Hypna | 2:d0ce8e26cbc4 | 12 | command.distance = 0; |
Hypna | 3:0d7687b6ef14 | 13 | }*/ |
Hypna | 3:0d7687b6ef14 | 14 | |
Hypna | 3:0d7687b6ef14 | 15 | DriveController::DriveController() : wheel1(PTC7), wheel2(PTC2), wheel3(PTE29), wheel4(PTE30), orientation(NORTH) |
Hypna | 3:0d7687b6ef14 | 16 | { |
Hypna | 3:0d7687b6ef14 | 17 | command.direction = NORTH; |
Hypna | 3:0d7687b6ef14 | 18 | command.distance = 0; |
Hypna | 3:0d7687b6ef14 | 19 | |
Hypna | 3:0d7687b6ef14 | 20 | for(int i = 0; i < 24; i++) |
Hypna | 3:0d7687b6ef14 | 21 | { |
Hypna | 3:0d7687b6ef14 | 22 | sensors.push_back(new DigitalInOut(sensorPinNames[i])); |
Hypna | 3:0d7687b6ef14 | 23 | } |
Hypna | 2:d0ce8e26cbc4 | 24 | } |
Hypna | 2:d0ce8e26cbc4 | 25 | |
Hypna | 0:e60f22c1d573 | 26 | DriveController::void go() |
Hypna | 0:e60f22c1d573 | 27 | { |
Hypna | 2:d0ce8e26cbc4 | 28 | while(true) |
Hypna | 2:d0ce8e26cbc4 | 29 | { |
Hypna | 2:d0ce8e26cbc4 | 30 | getCommand(); |
Hypna | 3:0d7687b6ef14 | 31 | |
Hypna | 2:d0ce8e26cbc4 | 32 | if(currentCommand.distance != 0) |
Hypna | 2:d0ce8e26cbc4 | 33 | move(); |
Hypna | 3:0d7687b6ef14 | 34 | |
Hypna | 3:0d7687b6ef14 | 35 | sendComplete(); |
Hypna | 2:d0ce8e26cbc4 | 36 | } |
Hypna | 2:d0ce8e26cbc4 | 37 | } |
Hypna | 2:d0ce8e26cbc4 | 38 | |
Hypna | 2:d0ce8e26cbc4 | 39 | DriveController::void move() |
Hypna | 2:d0ce8e26cbc4 | 40 | { |
Hypna | 2:d0ce8e26cbc4 | 41 | int travelled = 0; |
Hypna | 3:0d7687b6ef14 | 42 | bool atIntersection = true; |
Hypna | 2:d0ce8e26cbc4 | 43 | |
Hypna | 2:d0ce8e26cbc4 | 44 | if(orientation != command.direction) |
Hypna | 2:d0ce8e26cbc4 | 45 | rotate(command.direction); |
Hypna | 2:d0ce8e26cbc4 | 46 | |
Hypna | 2:d0ce8e26cbc4 | 47 | while(travelled < command.distance) |
Hypna | 2:d0ce8e26cbc4 | 48 | { |
Hypna | 2:d0ce8e26cbc4 | 49 | forward(calculateError()); |
Hypna | 3:0d7687b6ef14 | 50 | |
Hypna | 3:0d7687b6ef14 | 51 | if(intersection() && !atIntersection) |
Hypna | 3:0d7687b6ef14 | 52 | { |
Hypna | 2:d0ce8e26cbc4 | 53 | travelled++; |
Hypna | 3:0d7687b6ef14 | 54 | atIntersection = true; |
Hypna | 3:0d7687b6ef14 | 55 | } |
Hypna | 3:0d7687b6ef14 | 56 | else if(!intersection()) |
Hypna | 3:0d7687b6ef14 | 57 | atIntersection = false; |
Hypna | 2:d0ce8e26cbc4 | 58 | } |
Hypna | 3:0d7687b6ef14 | 59 | |
Hypna | 3:0d7687b6ef14 | 60 | command.distance = 0; |
Hypna | 2:d0ce8e26cbc4 | 61 | } |
Hypna | 2:d0ce8e26cbc4 | 62 | |
Hypna | 2:d0ce8e26cbc4 | 63 | DriveController::double calculateError() |
Hypna | 2:d0ce8e26cbc4 | 64 | { |
Hypna | 0:e60f22c1d573 | 65 | |
Hypna | 0:e60f22c1d573 | 66 | } |
Hypna | 0:e60f22c1d573 | 67 | |
Hypna | 2:d0ce8e26cbc4 | 68 | DriveController::void forward(double correction) |
Hypna | 1:fa6eb0c33b2f | 69 | { |
Hypna | 2:d0ce8e26cbc4 | 70 | |
Hypna | 2:d0ce8e26cbc4 | 71 | } |
Hypna | 2:d0ce8e26cbc4 | 72 | |
Hypna | 2:d0ce8e26cbc4 | 73 | DriveController::bool intersection() |
Hypna | 2:d0ce8e26cbc4 | 74 | { |
Hypna | 3:0d7687b6ef14 | 75 | return (sensorStates[0]||sensorStates[8]||sensorStates[16])&&(sensorStates[1]||sensorStates[9]||sensorStates[17]) |
Hypna | 3:0d7687b6ef14 | 76 | &&(sensorStates[2]||sensorStates[10]||sensorStates[18])&&(sensorStates[4]||sensorStates[12]||sensorStates[20]) |
Hypna | 3:0d7687b6ef14 | 77 | &&(sensorStates[5]||sensorStates[13]||sensorStates[21])&&(sensorStates[6]||sensorStates[14]||sensorStates[22]) |
Hypna | 3:0d7687b6ef14 | 78 | &&(sensorStates[7]||sensorStates[15]||sensorStates[23]); |
Hypna | 2:d0ce8e26cbc4 | 79 | } |
Hypna | 1:fa6eb0c33b2f | 80 | |
Hypna | 2:d0ce8e26cbc4 | 81 | DriveController::void rotate(Direction diretion) |
Hypna | 2:d0ce8e26cbc4 | 82 | { |
Hypna | 2:d0ce8e26cbc4 | 83 | |
Hypna | 2:d0ce8e26cbc4 | 84 | } |
Hypna | 2:d0ce8e26cbc4 | 85 | |
Hypna | 2:d0ce8e26cbc4 | 86 | DriveController::void getCommand() |
Hypna | 2:d0ce8e26cbc4 | 87 | { |
Hypna | 2:d0ce8e26cbc4 | 88 | |
Hypna | 2:d0ce8e26cbc4 | 89 | } |
Hypna | 2:d0ce8e26cbc4 | 90 | |
Hypna | 2:d0ce8e26cbc4 | 91 | DriveControler::void readSensors() |
Hypna | 2:d0ce8e26cbc4 | 92 | { |
Hypna | 2:d0ce8e26cbc4 | 93 | |
Hypna | 2:d0ce8e26cbc4 | 94 | } |