Code to run the microcontrollers on the R5 competition bot
Dependencies: LineSensors mbed
DriveController.cpp@22:8f726dc175cd, 2015-04-18 (annotated)
- Committer:
- Hypna
- Date:
- Sat Apr 18 04:57:20 2015 +0000
- Revision:
- 22:8f726dc175cd
- Parent:
- 18:2bd595af51d2
stuff
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 | 14:7a4cf2ed2499 | 3 | #define MOTOR_SCALE 0.5 |
Hypna | 14:7a4cf2ed2499 | 4 | #define CORRECTION_SCALE 0.15 |
Hypna | 14:7a4cf2ed2499 | 5 | #define Kp 0.066666666667 |
Hypna | 14:7a4cf2ed2499 | 6 | #define Ki 0.006666666667 |
Hypna | 14:7a4cf2ed2499 | 7 | |
Hypna | 9:01c17b286a99 | 8 | |
Hypna | 13:aa6f64c73271 | 9 | DriveController::DriveController() : i2c(PTC2, PTC1), treadSpeed1(PTB0), treadSpeed2(PTB1), treadDirection1(PTE20), treadDirection2(PTE21), |
Hypna | 5:f53f06a866e9 | 10 | sensors(PTC7, PTC0, PTC3, PTC4, PTC5, PTC6, PTC10, PTC11, PTC9, PTC8, PTA5, PTA4, PTA12, PTD4, PTA2, PTA1, PTC12, PTC13, |
jmar11 | 16:b49db5c8e16c | 11 | PTC16, PTC17, PTD1, PTD0, PTD5, PTD13), orient(NORTH) |
Hypna | 2:d0ce8e26cbc4 | 12 | { |
jmar11 | 18:2bd595af51d2 | 13 | sensors.setThreshold(); |
jmar11 | 18:2bd595af51d2 | 14 | wait(10); |
jmar11 | 18:2bd595af51d2 | 15 | treadSpeed1.period_us(50); //setting pwm period for all wheels to 20khz |
jmar11 | 18:2bd595af51d2 | 16 | treadSpeed2.period_us(50); |
Hypna | 2:d0ce8e26cbc4 | 17 | } |
Hypna | 2:d0ce8e26cbc4 | 18 | |
jmar11 | 17:5046b27f5441 | 19 | void DriveController::move(typename ::abs_direction dir) |
Hypna | 0:e60f22c1d573 | 20 | { |
jmar11 | 16:b49db5c8e16c | 21 | int diff; |
Hypna | 5:f53f06a866e9 | 22 | bool atLine = true; //tracks whether the bot is still at the starting intersection |
Hypna | 12:edcae0f36e9c | 23 | double error = 0; |
Hypna | 2:d0ce8e26cbc4 | 24 | |
jmar11 | 16:b49db5c8e16c | 25 | diff=direction-orient; |
jmar11 | 16:b49db5c8e16c | 26 | |
jmar11 | 16:b49db5c8e16c | 27 | if(abs(diff)==3){ |
jmar11 | 16:b49db5c8e16c | 28 | diff/=-3; |
jmar11 | 16:b49db5c8e16c | 29 | |
jmar11 | 16:b49db5c8e16c | 30 | if(diff == 2)//back |
Hypna | 10:210c8f1e3a92 | 31 | { |
Hypna | 13:aa6f64c73271 | 32 | treadDirection1 = treadDirection2 = 0; |
Hypna | 9:01c17b286a99 | 33 | } |
jmar11 | 16:b49db5c8e16c | 34 | else if(direction == 0)//forward |
Hypna | 9:01c17b286a99 | 35 | { |
Hypna | 13:aa6f64c73271 | 36 | treadDirection1 = treadDirection2 = 1; |
Hypna | 9:01c17b286a99 | 37 | } |
Hypna | 2:d0ce8e26cbc4 | 38 | |
Hypna | 5:f53f06a866e9 | 39 | do |
Hypna | 2:d0ce8e26cbc4 | 40 | { |
Hypna | 6:83c1f8d6a65a | 41 | sensors.lineDetect(sensorStates); |
Hypna | 6:83c1f8d6a65a | 42 | |
Hypna | 5:f53f06a866e9 | 43 | if(intersection()) |
Hypna | 5:f53f06a866e9 | 44 | { |
Hypna | 13:aa6f64c73271 | 45 | treadSpeed1 = treadSpeed2 = MOTOR_SCALE; |
Hypna | 5:f53f06a866e9 | 46 | } |
Hypna | 5:f53f06a866e9 | 47 | else |
Hypna | 3:0d7687b6ef14 | 48 | { |
Hypna | 5:f53f06a866e9 | 49 | error = calculateError(); |
Hypna | 5:f53f06a866e9 | 50 | |
Hypna | 5:f53f06a866e9 | 51 | if(error < 0) |
Hypna | 5:f53f06a866e9 | 52 | { |
Hypna | 13:aa6f64c73271 | 53 | treadSpeed1 = (1.0 - fabs(error))*MOTOR_SCALE; |
Hypna | 13:aa6f64c73271 | 54 | treadSpeed2 = MOTOR_SCALE; |
Hypna | 6:83c1f8d6a65a | 55 | } |
Hypna | 5:f53f06a866e9 | 56 | else |
Hypna | 5:f53f06a866e9 | 57 | { |
Hypna | 13:aa6f64c73271 | 58 | treadSpeed1 = MOTOR_SCALE; |
Hypna | 13:aa6f64c73271 | 59 | treadSpeed2 = (1.0 - error)*MOTOR_SCALE; |
Hypna | 5:f53f06a866e9 | 60 | } |
Hypna | 5:f53f06a866e9 | 61 | |
Hypna | 9:01c17b286a99 | 62 | atLine = false; |
Hypna | 5:f53f06a866e9 | 63 | } |
Hypna | 6:83c1f8d6a65a | 64 | } while(!intersection() || atLine); |
Hypna | 5:f53f06a866e9 | 65 | |
Hypna | 13:aa6f64c73271 | 66 | treadSpeed1 = treadSpeed2 = 0; |
Hypna | 5:f53f06a866e9 | 67 | } |
Hypna | 5:f53f06a866e9 | 68 | |
Hypna | 12:edcae0f36e9c | 69 | double DriveController::calculateError() |
Hypna | 5:f53f06a866e9 | 70 | { |
Hypna | 12:edcae0f36e9c | 71 | double error; |
Hypna | 5:f53f06a866e9 | 72 | int bin = 0; |
Hypna | 5:f53f06a866e9 | 73 | |
Hypna | 5:f53f06a866e9 | 74 | for(int i = 7; i >= 0; i--) |
Hypna | 5:f53f06a866e9 | 75 | { |
Hypna | 13:aa6f64c73271 | 76 | bin += sensorStates[i][0]<<(7-i); |
Hypna | 2:d0ce8e26cbc4 | 77 | } |
Hypna | 3:0d7687b6ef14 | 78 | |
Hypna | 5:f53f06a866e9 | 79 | switch(bin) |
Hypna | 5:f53f06a866e9 | 80 | { |
Hypna | 12:edcae0f36e9c | 81 | case 1: error = 6; |
Hypna | 5:f53f06a866e9 | 82 | break; |
Hypna | 12:edcae0f36e9c | 83 | case 3: error = 5; |
Hypna | 5:f53f06a866e9 | 84 | break; |
Hypna | 12:edcae0f36e9c | 85 | case 2: error = 4; |
Hypna | 5:f53f06a866e9 | 86 | break; |
Hypna | 12:edcae0f36e9c | 87 | case 6: error = 3; |
Hypna | 5:f53f06a866e9 | 88 | break; |
Hypna | 12:edcae0f36e9c | 89 | case 4: error = 2; |
Hypna | 5:f53f06a866e9 | 90 | break; |
Hypna | 12:edcae0f36e9c | 91 | case 12: error = 1; |
Hypna | 5:f53f06a866e9 | 92 | break; |
Hypna | 12:edcae0f36e9c | 93 | case 8: error = 0; |
Hypna | 5:f53f06a866e9 | 94 | break; |
Hypna | 5:f53f06a866e9 | 95 | case 24: error = 0; |
Hypna | 5:f53f06a866e9 | 96 | break; |
Hypna | 12:edcae0f36e9c | 97 | case 16: error = 0; |
Hypna | 5:f53f06a866e9 | 98 | break; |
Hypna | 12:edcae0f36e9c | 99 | case 48: error = -1; |
Hypna | 5:f53f06a866e9 | 100 | break; |
Hypna | 12:edcae0f36e9c | 101 | case 32: error = -2; |
Hypna | 5:f53f06a866e9 | 102 | break; |
Hypna | 12:edcae0f36e9c | 103 | case 96: error = -3; |
Hypna | 5:f53f06a866e9 | 104 | break; |
Hypna | 12:edcae0f36e9c | 105 | case 64: error = -4; |
Hypna | 5:f53f06a866e9 | 106 | break; |
Hypna | 12:edcae0f36e9c | 107 | case 192: error = -5; |
Hypna | 5:f53f06a866e9 | 108 | break; |
Hypna | 12:edcae0f36e9c | 109 | case 128: error = -6; |
Hypna | 10:210c8f1e3a92 | 110 | break; |
Hypna | 10:210c8f1e3a92 | 111 | default: error = 0; |
Hypna | 5:f53f06a866e9 | 112 | } |
Hypna | 5:f53f06a866e9 | 113 | |
Hypna | 12:edcae0f36e9c | 114 | return error*CORRECTION_SCALE; |
Hypna | 2:d0ce8e26cbc4 | 115 | } |
Hypna | 2:d0ce8e26cbc4 | 116 | |
Hypna | 4:ac6b2e5b240b | 117 | bool DriveController::intersection() |
Hypna | 2:d0ce8e26cbc4 | 118 | { |
Hypna | 6:83c1f8d6a65a | 119 | |
Hypna | 6:83c1f8d6a65a | 120 | // add speed bump check later |
Hypna | 6:83c1f8d6a65a | 121 | |
Hypna | 13:aa6f64c73271 | 122 | return (sensorStates[0][0]&&sensorStates[1][0]&&sensorStates[2][0]&&sensorStates[3][0]) |
Hypna | 13:aa6f64c73271 | 123 | || (sensorStates[4][0]&&sensorStates[5][0]&&sensorStates[6][0]&&sensorStates[7][0]); |
Hypna | 2:d0ce8e26cbc4 | 124 | } |
Hypna | 1:fa6eb0c33b2f | 125 | |
jmar11 | 17:5046b27f5441 | 126 | void DriveController::rotate(typename ::abs_direction dir) |
Hypna | 2:d0ce8e26cbc4 | 127 | { |
jmar11 | 16:b49db5c8e16c | 128 | int diff; |
Hypna | 6:83c1f8d6a65a | 129 | bool atLine = true; |
Hypna | 2:d0ce8e26cbc4 | 130 | |
jmar11 | 16:b49db5c8e16c | 131 | diff=direction-orient |
jmar11 | 16:b49db5c8e16c | 132 | |
jmar11 | 16:b49db5c8e16c | 133 | if(abs(diff)==3){ |
jmar11 | 16:b49db5c8e16c | 134 | diff/=-3; |
Hypna | 6:83c1f8d6a65a | 135 | } |
jmar11 | 16:b49db5c8e16c | 136 | do{ |
jmar11 | 16:b49db5c8e16c | 137 | if(diff < 0) |
jmar11 | 16:b49db5c8e16c | 138 | { |
jmar11 | 16:b49db5c8e16c | 139 | treadDirection1 = 0; |
jmar11 | 16:b49db5c8e16c | 140 | treadDirection2 = 1; |
jmar11 | 16:b49db5c8e16c | 141 | diff++; |
jmar11 | 16:b49db5c8e16c | 142 | } |
jmar11 | 16:b49db5c8e16c | 143 | else if(diff > 0) |
jmar11 | 16:b49db5c8e16c | 144 | { |
jmar11 | 16:b49db5c8e16c | 145 | treadDirection1 = 0; |
jmar11 | 16:b49db5c8e16c | 146 | treadDirection2 = 1; |
jmar11 | 16:b49db5c8e16c | 147 | diff--; |
jmar11 | 16:b49db5c8e16c | 148 | } |
jmar11 | 16:b49db5c8e16c | 149 | |
jmar11 | 16:b49db5c8e16c | 150 | treadSpeed1 = treadSpeed2 = MOTOR_SCALE; |
jmar11 | 16:b49db5c8e16c | 151 | }while(diff!=0); |
Hypna | 6:83c1f8d6a65a | 152 | |
Hypna | 12:edcae0f36e9c | 153 | /*do |
Hypna | 6:83c1f8d6a65a | 154 | { |
Hypna | 6:83c1f8d6a65a | 155 | sensors.lineDetect(sensorStates); |
Hypna | 6:83c1f8d6a65a | 156 | |
Hypna | 6:83c1f8d6a65a | 157 | if(!intersection()) |
Hypna | 6:83c1f8d6a65a | 158 | atLine = false; |
Hypna | 6:83c1f8d6a65a | 159 | |
Hypna | 14:7a4cf2ed2499 | 160 | } |
Hypna | 14:7a4cf2ed2499 | 161 | while(!intersection() || atLine);*/ |
Hypna | 12:edcae0f36e9c | 162 | |
Hypna | 14:7a4cf2ed2499 | 163 | wait(0.9); |
Hypna | 6:83c1f8d6a65a | 164 | |
Hypna | 13:aa6f64c73271 | 165 | treadSpeed1 = treadSpeed2 = 0; |
Hypna | 2:d0ce8e26cbc4 | 166 | } |
Hypna | 2:d0ce8e26cbc4 | 167 | |
Hypna | 12:edcae0f36e9c | 168 | void DriveController::spinTest() |
Hypna | 12:edcae0f36e9c | 169 | { |
Hypna | 13:aa6f64c73271 | 170 | treadDirection1 = 1; |
Hypna | 13:aa6f64c73271 | 171 | treadDirection2 = 0; |
Hypna | 12:edcae0f36e9c | 172 | |
Hypna | 13:aa6f64c73271 | 173 | treadSpeed1 = treadSpeed2 = MOTOR_SCALE; |
Hypna | 12:edcae0f36e9c | 174 | |
Hypna | 12:edcae0f36e9c | 175 | while(true) |
Hypna | 12:edcae0f36e9c | 176 | wait(1); |
Hypna | 12:edcae0f36e9c | 177 | } |
jmar11 | 16:b49db5c8e16c | 178 | |
jmar11 | 16:b49db5c8e16c | 179 | typename ::abs_direction getOrient() |
jmar11 | 16:b49db5c8e16c | 180 | { |
jmar11 | 16:b49db5c8e16c | 181 | return orient; |
jmar11 | 16:b49db5c8e16c | 182 | } |