David's line following code from the LVBots competition, 2015.
Dependencies: GeneralDebouncer Pacer PololuEncoder mbed
Fork of DeadReckoning by
main.cpp@27:2456f68be679, 2014-03-01 (annotated)
- Committer:
- DavidEGrayson
- Date:
- Sat Mar 01 01:46:35 2014 +0000
- Revision:
- 27:2456f68be679
- Parent:
- 26:7e7c376a7446
- Child:
- 28:4374035df5e0
Fixed a major bug in the line following (reduceSpeed return value was not used). Made finalSettleIn better by adding an integral term and increasing the settleSpeed from 200 to 300.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
DavidEGrayson | 0:e77a0edb9878 | 1 | #include <mbed.h> |
DavidEGrayson | 8:78b1ff957cba | 2 | #include <Pacer.h> |
DavidEGrayson | 21:c279c6a83671 | 3 | #include <GeneralDebouncer.h> |
DavidEGrayson | 19:a11ffc903774 | 4 | #include <math.h> |
DavidEGrayson | 0:e77a0edb9878 | 5 | |
DavidEGrayson | 21:c279c6a83671 | 6 | #include "main.h" |
DavidEGrayson | 8:78b1ff957cba | 7 | #include "motors.h" |
DavidEGrayson | 8:78b1ff957cba | 8 | #include "encoders.h" |
DavidEGrayson | 9:9734347b5756 | 9 | #include "leds.h" |
DavidEGrayson | 8:78b1ff957cba | 10 | #include "pc_serial.h" |
DavidEGrayson | 9:9734347b5756 | 11 | #include "test.h" |
DavidEGrayson | 12:835a4d24ae3b | 12 | #include "reckoner.h" |
DavidEGrayson | 16:8eaa5bc2bdb1 | 13 | #include "buttons.h" |
DavidEGrayson | 21:c279c6a83671 | 14 | #include "line_tracker.h" |
DavidEGrayson | 21:c279c6a83671 | 15 | |
DavidEGrayson | 21:c279c6a83671 | 16 | Reckoner reckoner; |
DavidEGrayson | 21:c279c6a83671 | 17 | LineTracker lineTracker; |
DavidEGrayson | 21:c279c6a83671 | 18 | |
DavidEGrayson | 21:c279c6a83671 | 19 | const int16_t drivingSpeed = 300; |
DavidEGrayson | 21:c279c6a83671 | 20 | |
DavidEGrayson | 21:c279c6a83671 | 21 | void setLeds(bool v1, bool v2, bool v3, bool v4) |
DavidEGrayson | 21:c279c6a83671 | 22 | { |
DavidEGrayson | 21:c279c6a83671 | 23 | led1 = v1; |
DavidEGrayson | 21:c279c6a83671 | 24 | led2 = v2; |
DavidEGrayson | 21:c279c6a83671 | 25 | led3 = v3; |
DavidEGrayson | 21:c279c6a83671 | 26 | led4 = v4; |
DavidEGrayson | 21:c279c6a83671 | 27 | } |
DavidEGrayson | 0:e77a0edb9878 | 28 | |
DavidEGrayson | 10:e4dd36148539 | 29 | int __attribute__((noreturn)) main() |
DavidEGrayson | 2:968338353aef | 30 | { |
DavidEGrayson | 2:968338353aef | 31 | pc.baud(115200); |
DavidEGrayson | 2:968338353aef | 32 | |
DavidEGrayson | 2:968338353aef | 33 | // Enable pull-ups on encoder pins and give them a chance to settle. |
DavidEGrayson | 9:9734347b5756 | 34 | encodersInit(); |
DavidEGrayson | 9:9734347b5756 | 35 | motorsInit(); |
DavidEGrayson | 16:8eaa5bc2bdb1 | 36 | buttonsInit(); |
DavidEGrayson | 4:1b20a11765c8 | 37 | |
DavidEGrayson | 8:78b1ff957cba | 38 | // Test routines |
DavidEGrayson | 9:9734347b5756 | 39 | //testMotors(); |
DavidEGrayson | 10:e4dd36148539 | 40 | //testEncoders(); |
DavidEGrayson | 12:835a4d24ae3b | 41 | //testLineSensors(); |
DavidEGrayson | 16:8eaa5bc2bdb1 | 42 | //testReckoner(); |
DavidEGrayson | 17:2df9861f53ee | 43 | //testButtons(); |
DavidEGrayson | 20:dbec34f0e76b | 44 | //testDriveHome(); |
DavidEGrayson | 21:c279c6a83671 | 45 | //testFinalSettleIn(); |
DavidEGrayson | 23:aae5cbe3b924 | 46 | //testCalibrate(); |
DavidEGrayson | 2:968338353aef | 47 | |
DavidEGrayson | 21:c279c6a83671 | 48 | // Real routines for the contest. |
DavidEGrayson | 21:c279c6a83671 | 49 | setLeds(1, 0, 0, 0); |
DavidEGrayson | 21:c279c6a83671 | 50 | waitForSignalToStart(); |
DavidEGrayson | 21:c279c6a83671 | 51 | setLeds(0, 1, 0, 0); |
DavidEGrayson | 25:73c2eedb3b91 | 52 | |
DavidEGrayson | 25:73c2eedb3b91 | 53 | loadCalibrationAndFindLine(); |
DavidEGrayson | 25:73c2eedb3b91 | 54 | //findLineAndCalibrate(); |
DavidEGrayson | 25:73c2eedb3b91 | 55 | |
DavidEGrayson | 27:2456f68be679 | 56 | //setLeds(1, 1, 0, 0); |
DavidEGrayson | 27:2456f68be679 | 57 | //turnRightToFindLine(); |
DavidEGrayson | 21:c279c6a83671 | 58 | setLeds(0, 0, 1, 0); |
DavidEGrayson | 21:c279c6a83671 | 59 | followLineToEnd(); |
DavidEGrayson | 21:c279c6a83671 | 60 | setLeds(1, 0, 1, 0); |
DavidEGrayson | 21:c279c6a83671 | 61 | driveHomeAlmost(); |
DavidEGrayson | 21:c279c6a83671 | 62 | setLeds(0, 1, 1, 0); |
DavidEGrayson | 21:c279c6a83671 | 63 | finalSettleIn(); |
DavidEGrayson | 21:c279c6a83671 | 64 | setLeds(1, 1, 1, 1); |
DavidEGrayson | 21:c279c6a83671 | 65 | while(1){} |
DavidEGrayson | 0:e77a0edb9878 | 66 | } |
DavidEGrayson | 12:835a4d24ae3b | 67 | |
DavidEGrayson | 12:835a4d24ae3b | 68 | void updateReckonerFromEncoders() |
DavidEGrayson | 12:835a4d24ae3b | 69 | { |
DavidEGrayson | 12:835a4d24ae3b | 70 | while(encoderBuffer.hasEvents()) |
DavidEGrayson | 12:835a4d24ae3b | 71 | { |
DavidEGrayson | 12:835a4d24ae3b | 72 | PololuEncoderEvent event = encoderBuffer.readEvent(); |
DavidEGrayson | 12:835a4d24ae3b | 73 | switch(event) |
DavidEGrayson | 12:835a4d24ae3b | 74 | { |
DavidEGrayson | 17:2df9861f53ee | 75 | case ENCODER_LEFT | POLOLU_ENCODER_EVENT_INC: |
DavidEGrayson | 17:2df9861f53ee | 76 | reckoner.handleTickLeftForward(); |
DavidEGrayson | 17:2df9861f53ee | 77 | break; |
DavidEGrayson | 17:2df9861f53ee | 78 | case ENCODER_LEFT | POLOLU_ENCODER_EVENT_DEC: |
DavidEGrayson | 17:2df9861f53ee | 79 | reckoner.handleTickLeftBackward(); |
DavidEGrayson | 17:2df9861f53ee | 80 | break; |
DavidEGrayson | 17:2df9861f53ee | 81 | case ENCODER_RIGHT | POLOLU_ENCODER_EVENT_INC: |
DavidEGrayson | 17:2df9861f53ee | 82 | reckoner.handleTickRightForward(); |
DavidEGrayson | 17:2df9861f53ee | 83 | break; |
DavidEGrayson | 17:2df9861f53ee | 84 | case ENCODER_RIGHT | POLOLU_ENCODER_EVENT_DEC: |
DavidEGrayson | 17:2df9861f53ee | 85 | reckoner.handleTickRightBackward(); |
DavidEGrayson | 17:2df9861f53ee | 86 | break; |
DavidEGrayson | 12:835a4d24ae3b | 87 | } |
DavidEGrayson | 12:835a4d24ae3b | 88 | } |
DavidEGrayson | 12:835a4d24ae3b | 89 | } |
DavidEGrayson | 17:2df9861f53ee | 90 | |
DavidEGrayson | 19:a11ffc903774 | 91 | float magnitude() |
DavidEGrayson | 19:a11ffc903774 | 92 | { |
DavidEGrayson | 19:a11ffc903774 | 93 | return sqrt((float)reckoner.x * reckoner.x + (float)reckoner.y * reckoner.y); |
DavidEGrayson | 19:a11ffc903774 | 94 | } |
DavidEGrayson | 19:a11ffc903774 | 95 | |
DavidEGrayson | 20:dbec34f0e76b | 96 | float dotProduct() |
DavidEGrayson | 20:dbec34f0e76b | 97 | { |
DavidEGrayson | 20:dbec34f0e76b | 98 | float s = (float)reckoner.sin / (1 << 30); |
DavidEGrayson | 20:dbec34f0e76b | 99 | float c = (float)reckoner.cos / (1 << 30); |
DavidEGrayson | 20:dbec34f0e76b | 100 | float magn = magnitude(); |
DavidEGrayson | 20:dbec34f0e76b | 101 | if (magn == 0){ return 0; } |
DavidEGrayson | 20:dbec34f0e76b | 102 | return ((float)reckoner.x * c + (float)reckoner.y * s) / magn; |
DavidEGrayson | 20:dbec34f0e76b | 103 | } |
DavidEGrayson | 20:dbec34f0e76b | 104 | |
DavidEGrayson | 18:b65fbb795396 | 105 | // The closer this is to zero, the closer we are to pointing towards the home position. |
DavidEGrayson | 18:b65fbb795396 | 106 | // It is basically a cross product of the two vectors (x, y) and (cos, sin). |
DavidEGrayson | 19:a11ffc903774 | 107 | float determinant() |
DavidEGrayson | 18:b65fbb795396 | 108 | { |
DavidEGrayson | 18:b65fbb795396 | 109 | // TODO: get rid of the magic numbers here (i.e. 30) |
DavidEGrayson | 18:b65fbb795396 | 110 | float s = (float)reckoner.sin / (1 << 30); |
DavidEGrayson | 18:b65fbb795396 | 111 | float c = (float)reckoner.cos / (1 << 30); |
DavidEGrayson | 19:a11ffc903774 | 112 | return (reckoner.x * s - reckoner.y * c) / magnitude(); |
DavidEGrayson | 19:a11ffc903774 | 113 | } |
DavidEGrayson | 19:a11ffc903774 | 114 | |
DavidEGrayson | 21:c279c6a83671 | 115 | int16_t reduceSpeed(int16_t speed, int32_t reduction) |
DavidEGrayson | 19:a11ffc903774 | 116 | { |
DavidEGrayson | 19:a11ffc903774 | 117 | if (reduction > speed) |
DavidEGrayson | 19:a11ffc903774 | 118 | { |
DavidEGrayson | 19:a11ffc903774 | 119 | return 0; |
DavidEGrayson | 19:a11ffc903774 | 120 | } |
DavidEGrayson | 19:a11ffc903774 | 121 | else |
DavidEGrayson | 19:a11ffc903774 | 122 | { |
DavidEGrayson | 19:a11ffc903774 | 123 | return speed - reduction; |
DavidEGrayson | 19:a11ffc903774 | 124 | } |
DavidEGrayson | 18:b65fbb795396 | 125 | } |
DavidEGrayson | 18:b65fbb795396 | 126 | |
DavidEGrayson | 21:c279c6a83671 | 127 | // Returns true if each line sensor has one third of a volt of difference between the |
DavidEGrayson | 21:c279c6a83671 | 128 | // maximum seen value and the minimum. |
DavidEGrayson | 21:c279c6a83671 | 129 | bool calibrationLooksGood() |
DavidEGrayson | 21:c279c6a83671 | 130 | { |
DavidEGrayson | 21:c279c6a83671 | 131 | for(uint8_t s = 0; s < LINE_SENSOR_COUNT; s++) |
DavidEGrayson | 21:c279c6a83671 | 132 | { |
DavidEGrayson | 21:c279c6a83671 | 133 | uint16_t contrast = lineTracker.calibratedMaximum[s] - lineTracker.calibratedMinimum[s]; |
DavidEGrayson | 21:c279c6a83671 | 134 | if (contrast < 9929) // 0xFFFF*0.5/3.3 = 9929 |
DavidEGrayson | 21:c279c6a83671 | 135 | { |
DavidEGrayson | 21:c279c6a83671 | 136 | return false; |
DavidEGrayson | 21:c279c6a83671 | 137 | } |
DavidEGrayson | 21:c279c6a83671 | 138 | } |
DavidEGrayson | 21:c279c6a83671 | 139 | return true; |
DavidEGrayson | 21:c279c6a83671 | 140 | } |
DavidEGrayson | 21:c279c6a83671 | 141 | |
DavidEGrayson | 21:c279c6a83671 | 142 | void waitForSignalToStart() |
DavidEGrayson | 21:c279c6a83671 | 143 | { |
DavidEGrayson | 21:c279c6a83671 | 144 | while(!button1DefinitelyPressed()) |
DavidEGrayson | 21:c279c6a83671 | 145 | { |
DavidEGrayson | 21:c279c6a83671 | 146 | updateReckonerFromEncoders(); |
DavidEGrayson | 21:c279c6a83671 | 147 | } |
DavidEGrayson | 21:c279c6a83671 | 148 | reckoner.reset(); |
DavidEGrayson | 21:c279c6a83671 | 149 | } |
DavidEGrayson | 21:c279c6a83671 | 150 | |
DavidEGrayson | 24:fc01d9125d3b | 151 | // Keep the robot pointing the in the right direction (1, 0). |
DavidEGrayson | 24:fc01d9125d3b | 152 | // This should basically keep it going straight. |
DavidEGrayson | 24:fc01d9125d3b | 153 | void updateMotorsToDriveStraight() |
DavidEGrayson | 24:fc01d9125d3b | 154 | { |
DavidEGrayson | 24:fc01d9125d3b | 155 | const int32_t straightDriveStrength = 1000; |
DavidEGrayson | 24:fc01d9125d3b | 156 | int16_t speedLeft = drivingSpeed; |
DavidEGrayson | 24:fc01d9125d3b | 157 | int16_t speedRight = drivingSpeed; |
DavidEGrayson | 24:fc01d9125d3b | 158 | int32_t reduction = reckoner.sin / (1<<15) * straightDriveStrength / (1 << 15); |
DavidEGrayson | 24:fc01d9125d3b | 159 | if (reduction > 0) |
DavidEGrayson | 24:fc01d9125d3b | 160 | { |
DavidEGrayson | 24:fc01d9125d3b | 161 | speedRight = reduceSpeed(speedRight, reduction); |
DavidEGrayson | 24:fc01d9125d3b | 162 | } |
DavidEGrayson | 24:fc01d9125d3b | 163 | else |
DavidEGrayson | 24:fc01d9125d3b | 164 | { |
DavidEGrayson | 24:fc01d9125d3b | 165 | speedLeft = reduceSpeed(speedLeft, -reduction); |
DavidEGrayson | 24:fc01d9125d3b | 166 | } |
DavidEGrayson | 24:fc01d9125d3b | 167 | motorsSpeedSet(speedLeft, speedRight); |
DavidEGrayson | 24:fc01d9125d3b | 168 | } |
DavidEGrayson | 24:fc01d9125d3b | 169 | |
DavidEGrayson | 24:fc01d9125d3b | 170 | void loadCalibrationAndFindLine() |
DavidEGrayson | 24:fc01d9125d3b | 171 | { |
DavidEGrayson | 24:fc01d9125d3b | 172 | lineTracker.calibratedMinimum[0] = 34872; |
DavidEGrayson | 24:fc01d9125d3b | 173 | lineTracker.calibratedMinimum[1] = 29335; |
DavidEGrayson | 24:fc01d9125d3b | 174 | lineTracker.calibratedMinimum[2] = 23845; |
DavidEGrayson | 24:fc01d9125d3b | 175 | lineTracker.calibratedMaximum[0] = 59726; |
DavidEGrayson | 24:fc01d9125d3b | 176 | lineTracker.calibratedMaximum[1] = 60110; |
DavidEGrayson | 24:fc01d9125d3b | 177 | lineTracker.calibratedMaximum[2] = 58446; |
DavidEGrayson | 24:fc01d9125d3b | 178 | |
DavidEGrayson | 24:fc01d9125d3b | 179 | GeneralDebouncer lineStatus(10000); |
DavidEGrayson | 24:fc01d9125d3b | 180 | while(1) |
DavidEGrayson | 24:fc01d9125d3b | 181 | { |
DavidEGrayson | 24:fc01d9125d3b | 182 | lineTracker.read(); |
DavidEGrayson | 24:fc01d9125d3b | 183 | lineTracker.updateCalibration(); |
DavidEGrayson | 24:fc01d9125d3b | 184 | updateReckonerFromEncoders(); |
DavidEGrayson | 24:fc01d9125d3b | 185 | updateMotorsToDriveStraight(); |
DavidEGrayson | 24:fc01d9125d3b | 186 | lineStatus.update(lineTracker.getLineVisible()); |
DavidEGrayson | 24:fc01d9125d3b | 187 | |
DavidEGrayson | 26:7e7c376a7446 | 188 | if(lineStatus.getState() == true && lineStatus.getTimeInCurrentStateMicroseconds() > 100000) |
DavidEGrayson | 24:fc01d9125d3b | 189 | { |
DavidEGrayson | 26:7e7c376a7446 | 190 | break; |
DavidEGrayson | 24:fc01d9125d3b | 191 | } |
DavidEGrayson | 24:fc01d9125d3b | 192 | } |
DavidEGrayson | 24:fc01d9125d3b | 193 | } |
DavidEGrayson | 20:dbec34f0e76b | 194 | |
DavidEGrayson | 25:73c2eedb3b91 | 195 | // THIS IS DEPRECATED. Instead we are using loadCalibrationAndFindLine. |
DavidEGrayson | 21:c279c6a83671 | 196 | void findLineAndCalibrate() |
DavidEGrayson | 20:dbec34f0e76b | 197 | { |
DavidEGrayson | 21:c279c6a83671 | 198 | Timer timer; |
DavidEGrayson | 21:c279c6a83671 | 199 | timer.start(); |
DavidEGrayson | 21:c279c6a83671 | 200 | |
DavidEGrayson | 21:c279c6a83671 | 201 | Timer goodCalibrationTimer; |
DavidEGrayson | 21:c279c6a83671 | 202 | bool goodCalibration = false; |
DavidEGrayson | 21:c279c6a83671 | 203 | |
DavidEGrayson | 21:c279c6a83671 | 204 | while(1) |
DavidEGrayson | 21:c279c6a83671 | 205 | { |
DavidEGrayson | 21:c279c6a83671 | 206 | lineTracker.read(); |
DavidEGrayson | 21:c279c6a83671 | 207 | lineTracker.updateCalibration(); |
DavidEGrayson | 24:fc01d9125d3b | 208 | updateReckonerFromEncoders(); |
DavidEGrayson | 24:fc01d9125d3b | 209 | updateMotorsToDriveStraight(); |
DavidEGrayson | 21:c279c6a83671 | 210 | |
DavidEGrayson | 21:c279c6a83671 | 211 | if (goodCalibration) |
DavidEGrayson | 21:c279c6a83671 | 212 | { |
DavidEGrayson | 22:44c032e59ff5 | 213 | if(goodCalibrationTimer.read_ms() >= 300) |
DavidEGrayson | 21:c279c6a83671 | 214 | { |
DavidEGrayson | 21:c279c6a83671 | 215 | // The calibration was good and we traveled for a bit of time after that, |
DavidEGrayson | 21:c279c6a83671 | 216 | // so we must be a bit over the line. |
DavidEGrayson | 21:c279c6a83671 | 217 | break; |
DavidEGrayson | 21:c279c6a83671 | 218 | } |
DavidEGrayson | 21:c279c6a83671 | 219 | } |
DavidEGrayson | 21:c279c6a83671 | 220 | else |
DavidEGrayson | 21:c279c6a83671 | 221 | { |
DavidEGrayson | 21:c279c6a83671 | 222 | if(calibrationLooksGood()) |
DavidEGrayson | 21:c279c6a83671 | 223 | { |
DavidEGrayson | 21:c279c6a83671 | 224 | goodCalibration = true; |
DavidEGrayson | 21:c279c6a83671 | 225 | goodCalibrationTimer.start(); |
DavidEGrayson | 21:c279c6a83671 | 226 | } |
DavidEGrayson | 21:c279c6a83671 | 227 | } |
DavidEGrayson | 21:c279c6a83671 | 228 | } |
DavidEGrayson | 21:c279c6a83671 | 229 | } |
DavidEGrayson | 21:c279c6a83671 | 230 | |
DavidEGrayson | 21:c279c6a83671 | 231 | void turnRightToFindLine() |
DavidEGrayson | 21:c279c6a83671 | 232 | { |
DavidEGrayson | 21:c279c6a83671 | 233 | while(1) |
DavidEGrayson | 21:c279c6a83671 | 234 | { |
DavidEGrayson | 21:c279c6a83671 | 235 | lineTracker.read(); |
DavidEGrayson | 21:c279c6a83671 | 236 | lineTracker.updateCalibration(); |
DavidEGrayson | 21:c279c6a83671 | 237 | updateReckonerFromEncoders(); |
DavidEGrayson | 21:c279c6a83671 | 238 | |
DavidEGrayson | 21:c279c6a83671 | 239 | if(lineTracker.getLineVisible()) |
DavidEGrayson | 21:c279c6a83671 | 240 | { |
DavidEGrayson | 21:c279c6a83671 | 241 | break; |
DavidEGrayson | 21:c279c6a83671 | 242 | } |
DavidEGrayson | 21:c279c6a83671 | 243 | |
DavidEGrayson | 21:c279c6a83671 | 244 | motorsSpeedSet(300, 100); |
DavidEGrayson | 21:c279c6a83671 | 245 | } |
DavidEGrayson | 21:c279c6a83671 | 246 | } |
DavidEGrayson | 21:c279c6a83671 | 247 | |
DavidEGrayson | 21:c279c6a83671 | 248 | void followLineToEnd() |
DavidEGrayson | 21:c279c6a83671 | 249 | { |
DavidEGrayson | 26:7e7c376a7446 | 250 | Timer timer; |
DavidEGrayson | 26:7e7c376a7446 | 251 | timer.start(); |
DavidEGrayson | 26:7e7c376a7446 | 252 | |
DavidEGrayson | 21:c279c6a83671 | 253 | GeneralDebouncer lineStatus(10000); |
DavidEGrayson | 21:c279c6a83671 | 254 | const uint32_t lineDebounceTime = 100000; |
DavidEGrayson | 21:c279c6a83671 | 255 | const int followLineStrength = 300; |
DavidEGrayson | 21:c279c6a83671 | 256 | |
DavidEGrayson | 21:c279c6a83671 | 257 | while(1) |
DavidEGrayson | 21:c279c6a83671 | 258 | { |
DavidEGrayson | 21:c279c6a83671 | 259 | lineTracker.read(); |
DavidEGrayson | 21:c279c6a83671 | 260 | updateReckonerFromEncoders(); |
DavidEGrayson | 21:c279c6a83671 | 261 | |
DavidEGrayson | 21:c279c6a83671 | 262 | lineStatus.update(lineTracker.getLineVisible()); |
DavidEGrayson | 21:c279c6a83671 | 263 | |
DavidEGrayson | 21:c279c6a83671 | 264 | bool lostLine = lineStatus.getState() == false && |
DavidEGrayson | 26:7e7c376a7446 | 265 | lineStatus.getTimeInCurrentStateMicroseconds() > lineDebounceTime; |
DavidEGrayson | 26:7e7c376a7446 | 266 | if(lostLine && timer.read_us() >= 300000) |
DavidEGrayson | 21:c279c6a83671 | 267 | { |
DavidEGrayson | 21:c279c6a83671 | 268 | break; |
DavidEGrayson | 21:c279c6a83671 | 269 | } |
DavidEGrayson | 21:c279c6a83671 | 270 | |
DavidEGrayson | 21:c279c6a83671 | 271 | int16_t speedLeft = drivingSpeed; |
DavidEGrayson | 21:c279c6a83671 | 272 | int16_t speedRight = drivingSpeed; |
DavidEGrayson | 21:c279c6a83671 | 273 | int16_t reduction = (lineTracker.getLinePosition() - 1500) * followLineStrength / 1500; |
DavidEGrayson | 21:c279c6a83671 | 274 | if(reduction < 0) |
DavidEGrayson | 21:c279c6a83671 | 275 | { |
DavidEGrayson | 27:2456f68be679 | 276 | speedLeft = reduceSpeed(speedLeft, -reduction); |
DavidEGrayson | 21:c279c6a83671 | 277 | } |
DavidEGrayson | 21:c279c6a83671 | 278 | else |
DavidEGrayson | 21:c279c6a83671 | 279 | { |
DavidEGrayson | 27:2456f68be679 | 280 | speedRight = reduceSpeed(speedRight, reduction); |
DavidEGrayson | 21:c279c6a83671 | 281 | } |
DavidEGrayson | 21:c279c6a83671 | 282 | |
DavidEGrayson | 21:c279c6a83671 | 283 | |
DavidEGrayson | 21:c279c6a83671 | 284 | motorsSpeedSet(speedLeft, speedRight); |
DavidEGrayson | 21:c279c6a83671 | 285 | } |
DavidEGrayson | 20:dbec34f0e76b | 286 | } |
DavidEGrayson | 20:dbec34f0e76b | 287 | |
DavidEGrayson | 20:dbec34f0e76b | 288 | void driveHomeAlmost() |
DavidEGrayson | 18:b65fbb795396 | 289 | { |
DavidEGrayson | 18:b65fbb795396 | 290 | Timer timer; |
DavidEGrayson | 18:b65fbb795396 | 291 | timer.start(); |
DavidEGrayson | 19:a11ffc903774 | 292 | |
DavidEGrayson | 19:a11ffc903774 | 293 | while(1) |
DavidEGrayson | 18:b65fbb795396 | 294 | { |
DavidEGrayson | 18:b65fbb795396 | 295 | updateReckonerFromEncoders(); |
DavidEGrayson | 19:a11ffc903774 | 296 | |
DavidEGrayson | 19:a11ffc903774 | 297 | float magn = magnitude(); |
DavidEGrayson | 19:a11ffc903774 | 298 | |
DavidEGrayson | 19:a11ffc903774 | 299 | if (magn < (1<<17)) |
DavidEGrayson | 18:b65fbb795396 | 300 | { |
DavidEGrayson | 19:a11ffc903774 | 301 | // We are within 8 encoder ticks, so go to the next step. |
DavidEGrayson | 19:a11ffc903774 | 302 | break; |
DavidEGrayson | 19:a11ffc903774 | 303 | } |
DavidEGrayson | 19:a11ffc903774 | 304 | |
DavidEGrayson | 19:a11ffc903774 | 305 | float det = determinant(); |
DavidEGrayson | 19:a11ffc903774 | 306 | |
DavidEGrayson | 21:c279c6a83671 | 307 | int16_t speedLeft = drivingSpeed; |
DavidEGrayson | 21:c279c6a83671 | 308 | int16_t speedRight = drivingSpeed; |
DavidEGrayson | 19:a11ffc903774 | 309 | if (magn < (1<<20)) // Within 64 encoder ticks of the origin, so slow down. |
DavidEGrayson | 18:b65fbb795396 | 310 | { |
DavidEGrayson | 21:c279c6a83671 | 311 | int16_t reduction = (1 - magn/(1<<20)) * drivingSpeed; |
DavidEGrayson | 19:a11ffc903774 | 312 | speedLeft = reduceSpeed(speedLeft, reduction); |
DavidEGrayson | 19:a11ffc903774 | 313 | speedRight = reduceSpeed(speedRight, reduction); |
DavidEGrayson | 19:a11ffc903774 | 314 | } |
DavidEGrayson | 19:a11ffc903774 | 315 | |
DavidEGrayson | 19:a11ffc903774 | 316 | if (det > 0) |
DavidEGrayson | 19:a11ffc903774 | 317 | { |
DavidEGrayson | 19:a11ffc903774 | 318 | speedLeft = reduceSpeed(speedLeft, det * 1000); |
DavidEGrayson | 18:b65fbb795396 | 319 | } |
DavidEGrayson | 18:b65fbb795396 | 320 | else |
DavidEGrayson | 18:b65fbb795396 | 321 | { |
DavidEGrayson | 19:a11ffc903774 | 322 | speedRight = reduceSpeed(speedRight, -det * 1000); |
DavidEGrayson | 18:b65fbb795396 | 323 | } |
DavidEGrayson | 19:a11ffc903774 | 324 | motorsSpeedSet(speedLeft, speedRight); |
DavidEGrayson | 18:b65fbb795396 | 325 | } |
DavidEGrayson | 18:b65fbb795396 | 326 | |
DavidEGrayson | 20:dbec34f0e76b | 327 | motorsSpeedSet(0, 0); |
DavidEGrayson | 20:dbec34f0e76b | 328 | } |
DavidEGrayson | 20:dbec34f0e76b | 329 | |
DavidEGrayson | 20:dbec34f0e76b | 330 | void finalSettleIn() |
DavidEGrayson | 20:dbec34f0e76b | 331 | { |
DavidEGrayson | 27:2456f68be679 | 332 | const int16_t settleSpeed = 300; |
DavidEGrayson | 27:2456f68be679 | 333 | const int16_t settleModificationStrength = 150; |
DavidEGrayson | 20:dbec34f0e76b | 334 | |
DavidEGrayson | 20:dbec34f0e76b | 335 | Timer timer; |
DavidEGrayson | 20:dbec34f0e76b | 336 | timer.start(); |
DavidEGrayson | 20:dbec34f0e76b | 337 | |
DavidEGrayson | 20:dbec34f0e76b | 338 | // State 0: rotating |
DavidEGrayson | 20:dbec34f0e76b | 339 | // State 1: Trying to get into final orientation: want [cos, sin] == [1<<30, 0]. |
DavidEGrayson | 20:dbec34f0e76b | 340 | uint8_t state = 0; |
DavidEGrayson | 20:dbec34f0e76b | 341 | |
DavidEGrayson | 27:2456f68be679 | 342 | Pacer reportPacer(200000); |
DavidEGrayson | 27:2456f68be679 | 343 | Pacer motorUpdatePacer(10000); |
DavidEGrayson | 21:c279c6a83671 | 344 | |
DavidEGrayson | 27:2456f68be679 | 345 | float integral = 0; |
DavidEGrayson | 27:2456f68be679 | 346 | |
DavidEGrayson | 27:2456f68be679 | 347 | motorsSpeedSet(-settleSpeed, settleSpeed); // avoid waiting 10 ms before we start settling |
DavidEGrayson | 27:2456f68be679 | 348 | |
DavidEGrayson | 19:a11ffc903774 | 349 | while(1) |
DavidEGrayson | 19:a11ffc903774 | 350 | { |
DavidEGrayson | 27:2456f68be679 | 351 | led1 = (state == 1); |
DavidEGrayson | 27:2456f68be679 | 352 | |
DavidEGrayson | 20:dbec34f0e76b | 353 | updateReckonerFromEncoders(); |
DavidEGrayson | 20:dbec34f0e76b | 354 | |
DavidEGrayson | 20:dbec34f0e76b | 355 | float dot = dotProduct(); |
DavidEGrayson | 20:dbec34f0e76b | 356 | int16_t speedModification = -dot * settleModificationStrength; |
DavidEGrayson | 20:dbec34f0e76b | 357 | if (speedModification > settleModificationStrength) |
DavidEGrayson | 20:dbec34f0e76b | 358 | { |
DavidEGrayson | 20:dbec34f0e76b | 359 | speedModification = settleModificationStrength; |
DavidEGrayson | 20:dbec34f0e76b | 360 | } |
DavidEGrayson | 20:dbec34f0e76b | 361 | else if (speedModification < -settleModificationStrength) |
DavidEGrayson | 20:dbec34f0e76b | 362 | { |
DavidEGrayson | 20:dbec34f0e76b | 363 | speedModification = -settleModificationStrength; |
DavidEGrayson | 20:dbec34f0e76b | 364 | } |
DavidEGrayson | 20:dbec34f0e76b | 365 | |
DavidEGrayson | 21:c279c6a83671 | 366 | if (state == 0 && timer.read_ms() >= 2000 && reckoner.cos > (1 << 29)) |
DavidEGrayson | 19:a11ffc903774 | 367 | { |
DavidEGrayson | 21:c279c6a83671 | 368 | // Stop turning and start trying to maintain the right position. |
DavidEGrayson | 20:dbec34f0e76b | 369 | state = 1; |
DavidEGrayson | 20:dbec34f0e76b | 370 | } |
DavidEGrayson | 20:dbec34f0e76b | 371 | |
DavidEGrayson | 21:c279c6a83671 | 372 | if (state == 1 && timer.read_ms() >= 5000) |
DavidEGrayson | 21:c279c6a83671 | 373 | { |
DavidEGrayson | 21:c279c6a83671 | 374 | // Stop moving. |
DavidEGrayson | 21:c279c6a83671 | 375 | break; |
DavidEGrayson | 21:c279c6a83671 | 376 | } |
DavidEGrayson | 21:c279c6a83671 | 377 | |
DavidEGrayson | 27:2456f68be679 | 378 | if (motorUpdatePacer.pace()) |
DavidEGrayson | 20:dbec34f0e76b | 379 | { |
DavidEGrayson | 27:2456f68be679 | 380 | int16_t rotationSpeed; |
DavidEGrayson | 27:2456f68be679 | 381 | if (state == 1) |
DavidEGrayson | 27:2456f68be679 | 382 | { |
DavidEGrayson | 27:2456f68be679 | 383 | float s = (float)reckoner.sin / (1 << 30); |
DavidEGrayson | 27:2456f68be679 | 384 | integral += s; |
DavidEGrayson | 27:2456f68be679 | 385 | rotationSpeed = -(s * 2400 + integral * 20); |
DavidEGrayson | 27:2456f68be679 | 386 | |
DavidEGrayson | 27:2456f68be679 | 387 | if (rotationSpeed > 300) |
DavidEGrayson | 27:2456f68be679 | 388 | { |
DavidEGrayson | 27:2456f68be679 | 389 | rotationSpeed = 300; |
DavidEGrayson | 27:2456f68be679 | 390 | } |
DavidEGrayson | 27:2456f68be679 | 391 | if (rotationSpeed < -300) |
DavidEGrayson | 27:2456f68be679 | 392 | { |
DavidEGrayson | 27:2456f68be679 | 393 | rotationSpeed = -300; |
DavidEGrayson | 27:2456f68be679 | 394 | } |
DavidEGrayson | 27:2456f68be679 | 395 | } |
DavidEGrayson | 27:2456f68be679 | 396 | else |
DavidEGrayson | 27:2456f68be679 | 397 | { |
DavidEGrayson | 27:2456f68be679 | 398 | rotationSpeed = settleSpeed; |
DavidEGrayson | 27:2456f68be679 | 399 | } |
DavidEGrayson | 27:2456f68be679 | 400 | |
DavidEGrayson | 27:2456f68be679 | 401 | int16_t speedLeft = -rotationSpeed + speedModification; |
DavidEGrayson | 27:2456f68be679 | 402 | int16_t speedRight = rotationSpeed + speedModification; |
DavidEGrayson | 27:2456f68be679 | 403 | motorsSpeedSet(speedLeft, speedRight); |
DavidEGrayson | 20:dbec34f0e76b | 404 | } |
DavidEGrayson | 21:c279c6a83671 | 405 | |
DavidEGrayson | 21:c279c6a83671 | 406 | if (state == 1 && reportPacer.pace()) |
DavidEGrayson | 21:c279c6a83671 | 407 | { |
DavidEGrayson | 21:c279c6a83671 | 408 | pc.printf("%11d %11d %11d %11d | %11f %11f\r\n", |
DavidEGrayson | 21:c279c6a83671 | 409 | reckoner.cos, reckoner.sin, reckoner.x, reckoner.y, |
DavidEGrayson | 21:c279c6a83671 | 410 | determinant(), dotProduct()); |
DavidEGrayson | 21:c279c6a83671 | 411 | } |
DavidEGrayson | 19:a11ffc903774 | 412 | } |
DavidEGrayson | 20:dbec34f0e76b | 413 | |
DavidEGrayson | 21:c279c6a83671 | 414 | // Done! Stop moving. |
DavidEGrayson | 20:dbec34f0e76b | 415 | motorsSpeedSet(0, 0); |
DavidEGrayson | 18:b65fbb795396 | 416 | } |