David Grayson / Mbed 2 deprecated DeadReckoning

David's dead reckoning code for the LVBots competition on March 6th. Uses the mbed LPC1768, DRV8835, QTR-3RC, and two DC motors with encoders.

Dependencies:   PololuEncoder Pacer mbed GeneralDebouncer

main.cpp@41:3ead1dd2cc3a, 2019-07-25 (annotated)

Committer:
DavidEGrayson
Date:
Thu Jul 25 03:20:41 2019 +0000
Revision:
41:3ead1dd2cc3a
Parent:
40:6fa672be85ec
Child:
42:96671b71aac5
Gyro: Add a hacky offset of +6.5 in TurnSensor.cpp.  The turn sensor drifts much more slowly now, like maybe 1 degree per minute.

Who changed what in which revision?

UserRevisionLine numberNew 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 40:6fa672be85ec 15 #include "l3g.h"
DavidEGrayson 40:6fa672be85ec 16 #include "turn_sensor.h"
DavidEGrayson 21:c279c6a83671 17
DavidEGrayson 41:3ead1dd2cc3a 18 void doDeadReckoning();
DavidEGrayson 41:3ead1dd2cc3a 19
DavidEGrayson 21:c279c6a83671 20 Reckoner reckoner;
DavidEGrayson 21:c279c6a83671 21 LineTracker lineTracker;
DavidEGrayson 40:6fa672be85ec 22 TurnSensor turnSensor;
DavidEGrayson 37:23000a47ed2b 23 Logger logger;
DavidEGrayson 40:6fa672be85ec 24
DavidEGrayson 40:6fa672be85ec 25 uint32_t totalEncoderCounts = 0;
DavidEGrayson 40:6fa672be85ec 26 uint32_t nextLogEncoderCount = 0;
DavidEGrayson 40:6fa672be85ec 27 const uint32_t logSpacing = 100;
DavidEGrayson 21:c279c6a83671 28
DavidEGrayson 33:58a0ab6e9ad2 29 const int16_t drivingSpeed = 400;
DavidEGrayson 21:c279c6a83671 30
DavidEGrayson 21:c279c6a83671 31 void setLeds(bool v1, bool v2, bool v3, bool v4)
DavidEGrayson 21:c279c6a83671 32 {
DavidEGrayson 21:c279c6a83671 33 led1 = v1;
DavidEGrayson 21:c279c6a83671 34 led2 = v2;
DavidEGrayson 21:c279c6a83671 35 led3 = v3;
DavidEGrayson 21:c279c6a83671 36 led4 = v4;
DavidEGrayson 21:c279c6a83671 37 }
DavidEGrayson 0:e77a0edb9878 38
DavidEGrayson 10:e4dd36148539 39 int __attribute__((noreturn)) main()
DavidEGrayson 2:968338353aef 40 {
DavidEGrayson 2:968338353aef 41 pc.baud(115200);
DavidEGrayson 2:968338353aef 42
DavidEGrayson 40:6fa672be85ec 43 if (l3gInit())
DavidEGrayson 40:6fa672be85ec 44 {
DavidEGrayson 40:6fa672be85ec 45 // Error initializing the gyro.
DavidEGrayson 40:6fa672be85ec 46 setLeds(0, 0, 1, 1);
DavidEGrayson 40:6fa672be85ec 47 while(1);
DavidEGrayson 40:6fa672be85ec 48 }
DavidEGrayson 40:6fa672be85ec 49
DavidEGrayson 2:968338353aef 50 // Enable pull-ups on encoder pins and give them a chance to settle.
DavidEGrayson 9:9734347b5756 51 encodersInit();
DavidEGrayson 9:9734347b5756 52 motorsInit();
DavidEGrayson 16:8eaa5bc2bdb1 53 buttonsInit();
DavidEGrayson 4:1b20a11765c8 54
DavidEGrayson 8:78b1ff957cba 55 // Test routines
DavidEGrayson 9:9734347b5756 56 //testMotors();
DavidEGrayson 10:e4dd36148539 57 //testEncoders();
DavidEGrayson 39:b19dfc5d4d4b 58 //testMotorSpeed();
DavidEGrayson 32:83a13b06093c 59 //testLineSensors();
DavidEGrayson 40:6fa672be85ec 60 //testL3g();
DavidEGrayson 41:3ead1dd2cc3a 61 //testL3gAndCalibrate();
DavidEGrayson 40:6fa672be85ec 62 testTurnSensor();
DavidEGrayson 16:8eaa5bc2bdb1 63 //testReckoner();
DavidEGrayson 17:2df9861f53ee 64 //testButtons();
DavidEGrayson 34:6c84680d823a 65 //testDriveHome();
DavidEGrayson 21:c279c6a83671 66 //testFinalSettleIn();
DavidEGrayson 23:aae5cbe3b924 67 //testCalibrate();
DavidEGrayson 33:58a0ab6e9ad2 68 //testLineFollowing();
DavidEGrayson 29:cfcf08d8ac79 69 //testAnalog();
DavidEGrayson 31:739b91331f31 70 //testSensorGlitches();
DavidEGrayson 33:58a0ab6e9ad2 71 //testTurnInPlace();
DavidEGrayson 33:58a0ab6e9ad2 72 //testCloseness();
DavidEGrayson 37:23000a47ed2b 73 //testLogger();
DavidEGrayson 41:3ead1dd2cc3a 74
DavidEGrayson 41:3ead1dd2cc3a 75 loadCalibration();
DavidEGrayson 2:968338353aef 76
DavidEGrayson 41:3ead1dd2cc3a 77 doDeadReckoning();
DavidEGrayson 41:3ead1dd2cc3a 78 }
DavidEGrayson 41:3ead1dd2cc3a 79
DavidEGrayson 41:3ead1dd2cc3a 80 void doDeadReckoning()
DavidEGrayson 41:3ead1dd2cc3a 81 {
DavidEGrayson 21:c279c6a83671 82 setLeds(1, 0, 0, 0);
DavidEGrayson 21:c279c6a83671 83 waitForSignalToStart();
DavidEGrayson 25:73c2eedb3b91 84
DavidEGrayson 33:58a0ab6e9ad2 85 setLeds(0, 1, 0, 0);
DavidEGrayson 28:4374035df5e0 86 findLine();
DavidEGrayson 41:3ead1dd2cc3a 87
DavidEGrayson 27:2456f68be679 88 //setLeds(1, 1, 0, 0);
DavidEGrayson 27:2456f68be679 89 //turnRightToFindLine();
DavidEGrayson 33:58a0ab6e9ad2 90
DavidEGrayson 21:c279c6a83671 91 setLeds(0, 0, 1, 0);
DavidEGrayson 21:c279c6a83671 92 followLineToEnd();
DavidEGrayson 33:58a0ab6e9ad2 93
DavidEGrayson 21:c279c6a83671 94 setLeds(1, 0, 1, 0);
DavidEGrayson 21:c279c6a83671 95 driveHomeAlmost();
DavidEGrayson 33:58a0ab6e9ad2 96
DavidEGrayson 33:58a0ab6e9ad2 97 //setLeds(0, 1, 1, 0);
DavidEGrayson 33:58a0ab6e9ad2 98 //finalSettleIn();
DavidEGrayson 33:58a0ab6e9ad2 99
DavidEGrayson 21:c279c6a83671 100 setLeds(1, 1, 1, 1);
DavidEGrayson 37:23000a47ed2b 101 loggerReportLoop();
DavidEGrayson 37:23000a47ed2b 102 }
DavidEGrayson 37:23000a47ed2b 103
DavidEGrayson 37:23000a47ed2b 104 void loggerService()
DavidEGrayson 37:23000a47ed2b 105 {
DavidEGrayson 40:6fa672be85ec 106 if (totalEncoderCounts > nextLogEncoderCount)
DavidEGrayson 37:23000a47ed2b 107 {
DavidEGrayson 40:6fa672be85ec 108 nextLogEncoderCount += logSpacing;
DavidEGrayson 40:6fa672be85ec 109
DavidEGrayson 40:6fa672be85ec 110 struct LogEntry entry;
DavidEGrayson 40:6fa672be85ec 111 entry.turnAngle = turnSensor.getAngle() >> 16;
DavidEGrayson 40:6fa672be85ec 112 entry.x = reckoner.x >> 16;
DavidEGrayson 40:6fa672be85ec 113 entry.y = reckoner.y >> 16;
DavidEGrayson 40:6fa672be85ec 114 logger.log(&entry);
DavidEGrayson 37:23000a47ed2b 115 }
DavidEGrayson 0:e77a0edb9878 116 }
DavidEGrayson 12:835a4d24ae3b 117
DavidEGrayson 37:23000a47ed2b 118 void loggerReportLoop()
DavidEGrayson 37:23000a47ed2b 119 {
DavidEGrayson 37:23000a47ed2b 120 while(1)
DavidEGrayson 37:23000a47ed2b 121 {
DavidEGrayson 37:23000a47ed2b 122 if(button1DefinitelyPressed())
DavidEGrayson 37:23000a47ed2b 123 {
DavidEGrayson 37:23000a47ed2b 124 logger.dump();
DavidEGrayson 37:23000a47ed2b 125 }
DavidEGrayson 37:23000a47ed2b 126 }
DavidEGrayson 37:23000a47ed2b 127 }
DavidEGrayson 37:23000a47ed2b 128
DavidEGrayson 37:23000a47ed2b 129
DavidEGrayson 28:4374035df5e0 130 void loadCalibration()
DavidEGrayson 28:4374035df5e0 131 {
DavidEGrayson 32:83a13b06093c 132 /** QTR-3RC **/
DavidEGrayson 32:83a13b06093c 133 lineTracker.calibratedMinimum[0] = 100;
DavidEGrayson 32:83a13b06093c 134 lineTracker.calibratedMinimum[1] = 94;
DavidEGrayson 32:83a13b06093c 135 lineTracker.calibratedMinimum[2] = 103;
DavidEGrayson 32:83a13b06093c 136 lineTracker.calibratedMaximum[0] = 792;
DavidEGrayson 32:83a13b06093c 137 lineTracker.calibratedMaximum[1] = 807;
DavidEGrayson 32:83a13b06093c 138 lineTracker.calibratedMaximum[2] = 1000;
DavidEGrayson 32:83a13b06093c 139
DavidEGrayson 32:83a13b06093c 140 /** QTR-3A
DavidEGrayson 28:4374035df5e0 141 lineTracker.calibratedMinimum[0] = 34872;
DavidEGrayson 28:4374035df5e0 142 lineTracker.calibratedMinimum[1] = 29335;
DavidEGrayson 28:4374035df5e0 143 lineTracker.calibratedMinimum[2] = 23845;
DavidEGrayson 28:4374035df5e0 144 lineTracker.calibratedMaximum[0] = 59726;
DavidEGrayson 28:4374035df5e0 145 lineTracker.calibratedMaximum[1] = 60110;
DavidEGrayson 32:83a13b06093c 146 lineTracker.calibratedMaximum[2] = 58446;
DavidEGrayson 32:83a13b06093c 147 **/
DavidEGrayson 28:4374035df5e0 148 }
DavidEGrayson 28:4374035df5e0 149
DavidEGrayson 12:835a4d24ae3b 150 void updateReckonerFromEncoders()
DavidEGrayson 12:835a4d24ae3b 151 {
DavidEGrayson 12:835a4d24ae3b 152 while(encoderBuffer.hasEvents())
DavidEGrayson 12:835a4d24ae3b 153 {
DavidEGrayson 12:835a4d24ae3b 154 PololuEncoderEvent event = encoderBuffer.readEvent();
DavidEGrayson 12:835a4d24ae3b 155 switch(event)
DavidEGrayson 12:835a4d24ae3b 156 {
DavidEGrayson 17:2df9861f53ee 157 case ENCODER_LEFT | POLOLU_ENCODER_EVENT_INC:
DavidEGrayson 17:2df9861f53ee 158 reckoner.handleTickLeftForward();
DavidEGrayson 40:6fa672be85ec 159 totalEncoderCounts++;
DavidEGrayson 17:2df9861f53ee 160 break;
DavidEGrayson 17:2df9861f53ee 161 case ENCODER_LEFT | POLOLU_ENCODER_EVENT_DEC:
DavidEGrayson 17:2df9861f53ee 162 reckoner.handleTickLeftBackward();
DavidEGrayson 40:6fa672be85ec 163 totalEncoderCounts--;
DavidEGrayson 17:2df9861f53ee 164 break;
DavidEGrayson 17:2df9861f53ee 165 case ENCODER_RIGHT | POLOLU_ENCODER_EVENT_INC:
DavidEGrayson 17:2df9861f53ee 166 reckoner.handleTickRightForward();
DavidEGrayson 40:6fa672be85ec 167 totalEncoderCounts++;
DavidEGrayson 17:2df9861f53ee 168 break;
DavidEGrayson 17:2df9861f53ee 169 case ENCODER_RIGHT | POLOLU_ENCODER_EVENT_DEC:
DavidEGrayson 17:2df9861f53ee 170 reckoner.handleTickRightBackward();
DavidEGrayson 40:6fa672be85ec 171 totalEncoderCounts--;
DavidEGrayson 17:2df9861f53ee 172 break;
DavidEGrayson 12:835a4d24ae3b 173 }
DavidEGrayson 12:835a4d24ae3b 174 }
DavidEGrayson 12:835a4d24ae3b 175 }
DavidEGrayson 17:2df9861f53ee 176
DavidEGrayson 19:a11ffc903774 177 float magnitude()
DavidEGrayson 19:a11ffc903774 178 {
DavidEGrayson 19:a11ffc903774 179 return sqrt((float)reckoner.x * reckoner.x + (float)reckoner.y * reckoner.y);
DavidEGrayson 19:a11ffc903774 180 }
DavidEGrayson 19:a11ffc903774 181
DavidEGrayson 20:dbec34f0e76b 182 float dotProduct()
DavidEGrayson 20:dbec34f0e76b 183 {
DavidEGrayson 20:dbec34f0e76b 184 float s = (float)reckoner.sin / (1 << 30);
DavidEGrayson 20:dbec34f0e76b 185 float c = (float)reckoner.cos / (1 << 30);
DavidEGrayson 20:dbec34f0e76b 186 float magn = magnitude();
DavidEGrayson 20:dbec34f0e76b 187 if (magn == 0){ return 0; }
DavidEGrayson 20:dbec34f0e76b 188 return ((float)reckoner.x * c + (float)reckoner.y * s) / magn;
DavidEGrayson 20:dbec34f0e76b 189 }
DavidEGrayson 20:dbec34f0e76b 190
DavidEGrayson 18:b65fbb795396 191 // The closer this is to zero, the closer we are to pointing towards the home position.
DavidEGrayson 18:b65fbb795396 192 // It is basically a cross product of the two vectors (x, y) and (cos, sin).
DavidEGrayson 19:a11ffc903774 193 float determinant()
DavidEGrayson 18:b65fbb795396 194 {
DavidEGrayson 18:b65fbb795396 195 // TODO: get rid of the magic numbers here (i.e. 30)
DavidEGrayson 18:b65fbb795396 196 float s = (float)reckoner.sin / (1 << 30);
DavidEGrayson 18:b65fbb795396 197 float c = (float)reckoner.cos / (1 << 30);
DavidEGrayson 19:a11ffc903774 198 return (reckoner.x * s - reckoner.y * c) / magnitude();
DavidEGrayson 19:a11ffc903774 199 }
DavidEGrayson 19:a11ffc903774 200
DavidEGrayson 21:c279c6a83671 201 int16_t reduceSpeed(int16_t speed, int32_t reduction)
DavidEGrayson 19:a11ffc903774 202 {
DavidEGrayson 19:a11ffc903774 203 if (reduction > speed)
DavidEGrayson 19:a11ffc903774 204 {
DavidEGrayson 19:a11ffc903774 205 return 0;
DavidEGrayson 19:a11ffc903774 206 }
DavidEGrayson 19:a11ffc903774 207 else
DavidEGrayson 19:a11ffc903774 208 {
DavidEGrayson 19:a11ffc903774 209 return speed - reduction;
DavidEGrayson 19:a11ffc903774 210 }
DavidEGrayson 18:b65fbb795396 211 }
DavidEGrayson 18:b65fbb795396 212
DavidEGrayson 21:c279c6a83671 213 void waitForSignalToStart()
DavidEGrayson 21:c279c6a83671 214 {
DavidEGrayson 21:c279c6a83671 215 while(!button1DefinitelyPressed())
DavidEGrayson 21:c279c6a83671 216 {
DavidEGrayson 21:c279c6a83671 217 updateReckonerFromEncoders();
DavidEGrayson 38:5e93a479c244 218 }
DavidEGrayson 21:c279c6a83671 219 reckoner.reset();
DavidEGrayson 38:5e93a479c244 220 while(button1DefinitelyPressed())
DavidEGrayson 38:5e93a479c244 221 {
DavidEGrayson 38:5e93a479c244 222 updateReckonerFromEncoders();
DavidEGrayson 38:5e93a479c244 223 }
DavidEGrayson 38:5e93a479c244 224 wait(0.2);
DavidEGrayson 21:c279c6a83671 225 }
DavidEGrayson 21:c279c6a83671 226
DavidEGrayson 24:fc01d9125d3b 227 // Keep the robot pointing the in the right direction (1, 0).
DavidEGrayson 24:fc01d9125d3b 228 // This should basically keep it going straight.
DavidEGrayson 24:fc01d9125d3b 229 void updateMotorsToDriveStraight()
DavidEGrayson 24:fc01d9125d3b 230 {
DavidEGrayson 24:fc01d9125d3b 231 const int32_t straightDriveStrength = 1000;
DavidEGrayson 24:fc01d9125d3b 232 int16_t speedLeft = drivingSpeed;
DavidEGrayson 24:fc01d9125d3b 233 int16_t speedRight = drivingSpeed;
DavidEGrayson 24:fc01d9125d3b 234 int32_t reduction = reckoner.sin / (1<<15) * straightDriveStrength / (1 << 15);
DavidEGrayson 24:fc01d9125d3b 235 if (reduction > 0)
DavidEGrayson 24:fc01d9125d3b 236 {
DavidEGrayson 24:fc01d9125d3b 237 speedRight = reduceSpeed(speedRight, reduction);
DavidEGrayson 24:fc01d9125d3b 238 }
DavidEGrayson 24:fc01d9125d3b 239 else
DavidEGrayson 24:fc01d9125d3b 240 {
DavidEGrayson 24:fc01d9125d3b 241 speedLeft = reduceSpeed(speedLeft, -reduction);
DavidEGrayson 24:fc01d9125d3b 242 }
DavidEGrayson 24:fc01d9125d3b 243 motorsSpeedSet(speedLeft, speedRight);
DavidEGrayson 24:fc01d9125d3b 244 }
DavidEGrayson 24:fc01d9125d3b 245
DavidEGrayson 28:4374035df5e0 246 void updateMotorsToFollowLine()
DavidEGrayson 24:fc01d9125d3b 247 {
DavidEGrayson 34:6c84680d823a 248 const int followLineStrength = drivingSpeed * 5 / 4;
DavidEGrayson 28:4374035df5e0 249
DavidEGrayson 28:4374035df5e0 250 int16_t speedLeft = drivingSpeed;
DavidEGrayson 28:4374035df5e0 251 int16_t speedRight = drivingSpeed;
DavidEGrayson 28:4374035df5e0 252 int16_t reduction = (lineTracker.getLinePosition() - 1000) * followLineStrength / 1000;
DavidEGrayson 28:4374035df5e0 253 if(reduction < 0)
DavidEGrayson 28:4374035df5e0 254 {
DavidEGrayson 28:4374035df5e0 255 speedLeft = reduceSpeed(speedLeft, -reduction);
DavidEGrayson 28:4374035df5e0 256 }
DavidEGrayson 28:4374035df5e0 257 else
DavidEGrayson 28:4374035df5e0 258 {
DavidEGrayson 28:4374035df5e0 259 speedRight = reduceSpeed(speedRight, reduction);
DavidEGrayson 28:4374035df5e0 260 }
DavidEGrayson 24:fc01d9125d3b 261
DavidEGrayson 28:4374035df5e0 262 motorsSpeedSet(speedLeft, speedRight);
DavidEGrayson 28:4374035df5e0 263 }
DavidEGrayson 28:4374035df5e0 264
DavidEGrayson 28:4374035df5e0 265 void findLine()
DavidEGrayson 28:4374035df5e0 266 {
DavidEGrayson 24:fc01d9125d3b 267 GeneralDebouncer lineStatus(10000);
DavidEGrayson 24:fc01d9125d3b 268 while(1)
DavidEGrayson 24:fc01d9125d3b 269 {
DavidEGrayson 24:fc01d9125d3b 270 lineTracker.read();
DavidEGrayson 24:fc01d9125d3b 271 lineTracker.updateCalibration();
DavidEGrayson 24:fc01d9125d3b 272 updateReckonerFromEncoders();
DavidEGrayson 37:23000a47ed2b 273 loggerService();
DavidEGrayson 37:23000a47ed2b 274 updateMotorsToDriveStraight();
DavidEGrayson 24:fc01d9125d3b 275 lineStatus.update(lineTracker.getLineVisible());
DavidEGrayson 24:fc01d9125d3b 276
DavidEGrayson 33:58a0ab6e9ad2 277 if(lineStatus.getState() == true && lineStatus.getTimeInCurrentStateMicroseconds() > 20000)
DavidEGrayson 24:fc01d9125d3b 278 {
DavidEGrayson 26:7e7c376a7446 279 break;
DavidEGrayson 24:fc01d9125d3b 280 }
DavidEGrayson 24:fc01d9125d3b 281 }
DavidEGrayson 24:fc01d9125d3b 282 }
DavidEGrayson 20:dbec34f0e76b 283
DavidEGrayson 33:58a0ab6e9ad2 284 /**
DavidEGrayson 21:c279c6a83671 285 void turnRightToFindLine()
DavidEGrayson 21:c279c6a83671 286 {
DavidEGrayson 21:c279c6a83671 287 while(1)
DavidEGrayson 21:c279c6a83671 288 {
DavidEGrayson 21:c279c6a83671 289 lineTracker.read();
DavidEGrayson 21:c279c6a83671 290 lineTracker.updateCalibration();
DavidEGrayson 21:c279c6a83671 291 updateReckonerFromEncoders();
DavidEGrayson 21:c279c6a83671 292
DavidEGrayson 21:c279c6a83671 293 if(lineTracker.getLineVisible())
DavidEGrayson 21:c279c6a83671 294 {
DavidEGrayson 21:c279c6a83671 295 break;
DavidEGrayson 21:c279c6a83671 296 }
DavidEGrayson 21:c279c6a83671 297
DavidEGrayson 21:c279c6a83671 298 motorsSpeedSet(300, 100);
DavidEGrayson 21:c279c6a83671 299 }
DavidEGrayson 33:58a0ab6e9ad2 300 }**/
DavidEGrayson 21:c279c6a83671 301
DavidEGrayson 21:c279c6a83671 302 void followLineToEnd()
DavidEGrayson 21:c279c6a83671 303 {
DavidEGrayson 26:7e7c376a7446 304 Timer timer;
DavidEGrayson 26:7e7c376a7446 305 timer.start();
DavidEGrayson 26:7e7c376a7446 306
DavidEGrayson 21:c279c6a83671 307 GeneralDebouncer lineStatus(10000);
DavidEGrayson 34:6c84680d823a 308 const uint32_t lineDebounceTime = 1000000;
DavidEGrayson 21:c279c6a83671 309
DavidEGrayson 21:c279c6a83671 310 while(1)
DavidEGrayson 21:c279c6a83671 311 {
DavidEGrayson 21:c279c6a83671 312 lineTracker.read();
DavidEGrayson 21:c279c6a83671 313 updateReckonerFromEncoders();
DavidEGrayson 37:23000a47ed2b 314 loggerService();
DavidEGrayson 21:c279c6a83671 315
DavidEGrayson 21:c279c6a83671 316 lineStatus.update(lineTracker.getLineVisible());
DavidEGrayson 21:c279c6a83671 317
DavidEGrayson 21:c279c6a83671 318 bool lostLine = lineStatus.getState() == false &&
DavidEGrayson 26:7e7c376a7446 319 lineStatus.getTimeInCurrentStateMicroseconds() > lineDebounceTime;
DavidEGrayson 28:4374035df5e0 320 if(lostLine && timer.read_us() >= 2000000)
DavidEGrayson 21:c279c6a83671 321 {
DavidEGrayson 21:c279c6a83671 322 break;
DavidEGrayson 21:c279c6a83671 323 }
DavidEGrayson 21:c279c6a83671 324
DavidEGrayson 28:4374035df5e0 325 updateMotorsToFollowLine();
DavidEGrayson 21:c279c6a83671 326 }
DavidEGrayson 20:dbec34f0e76b 327 }
DavidEGrayson 20:dbec34f0e76b 328
DavidEGrayson 20:dbec34f0e76b 329 void driveHomeAlmost()
DavidEGrayson 18:b65fbb795396 330 {
DavidEGrayson 18:b65fbb795396 331 Timer timer;
DavidEGrayson 18:b65fbb795396 332 timer.start();
DavidEGrayson 19:a11ffc903774 333
DavidEGrayson 19:a11ffc903774 334 while(1)
DavidEGrayson 18:b65fbb795396 335 {
DavidEGrayson 18:b65fbb795396 336 updateReckonerFromEncoders();
DavidEGrayson 37:23000a47ed2b 337 loggerService();
DavidEGrayson 37:23000a47ed2b 338
DavidEGrayson 19:a11ffc903774 339 float magn = magnitude();
DavidEGrayson 19:a11ffc903774 340
DavidEGrayson 33:58a0ab6e9ad2 341 if (magn < (1<<(14+7)))
DavidEGrayson 18:b65fbb795396 342 {
DavidEGrayson 33:58a0ab6e9ad2 343 // We are within 128 encoder ticks, so go to the next step.
DavidEGrayson 19:a11ffc903774 344 break;
DavidEGrayson 19:a11ffc903774 345 }
DavidEGrayson 19:a11ffc903774 346
DavidEGrayson 19:a11ffc903774 347 float det = determinant();
DavidEGrayson 19:a11ffc903774 348
DavidEGrayson 21:c279c6a83671 349 int16_t speedLeft = drivingSpeed;
DavidEGrayson 21:c279c6a83671 350 int16_t speedRight = drivingSpeed;
DavidEGrayson 33:58a0ab6e9ad2 351 if (magn < (1<<(14+9))) // Within 512 encoder ticks of the origin, so slow down.
DavidEGrayson 18:b65fbb795396 352 {
DavidEGrayson 33:58a0ab6e9ad2 353 int16_t reduction = (1 - magn/(1<<(14+8))) * (drivingSpeed/2);
DavidEGrayson 19:a11ffc903774 354 speedLeft = reduceSpeed(speedLeft, reduction);
DavidEGrayson 19:a11ffc903774 355 speedRight = reduceSpeed(speedRight, reduction);
DavidEGrayson 19:a11ffc903774 356 }
DavidEGrayson 19:a11ffc903774 357
DavidEGrayson 19:a11ffc903774 358 if (det > 0)
DavidEGrayson 19:a11ffc903774 359 {
DavidEGrayson 19:a11ffc903774 360 speedLeft = reduceSpeed(speedLeft, det * 1000);
DavidEGrayson 18:b65fbb795396 361 }
DavidEGrayson 18:b65fbb795396 362 else
DavidEGrayson 18:b65fbb795396 363 {
DavidEGrayson 19:a11ffc903774 364 speedRight = reduceSpeed(speedRight, -det * 1000);
DavidEGrayson 18:b65fbb795396 365 }
DavidEGrayson 19:a11ffc903774 366 motorsSpeedSet(speedLeft, speedRight);
DavidEGrayson 18:b65fbb795396 367 }
DavidEGrayson 18:b65fbb795396 368
DavidEGrayson 20:dbec34f0e76b 369 motorsSpeedSet(0, 0);
DavidEGrayson 20:dbec34f0e76b 370 }
DavidEGrayson 20:dbec34f0e76b 371
DavidEGrayson 20:dbec34f0e76b 372 void finalSettleIn()
DavidEGrayson 20:dbec34f0e76b 373 {
DavidEGrayson 27:2456f68be679 374 const int16_t settleSpeed = 300;
DavidEGrayson 27:2456f68be679 375 const int16_t settleModificationStrength = 150;
DavidEGrayson 20:dbec34f0e76b 376
DavidEGrayson 20:dbec34f0e76b 377 Timer timer;
DavidEGrayson 20:dbec34f0e76b 378 timer.start();
DavidEGrayson 20:dbec34f0e76b 379
DavidEGrayson 20:dbec34f0e76b 380 // State 0: rotating
DavidEGrayson 20:dbec34f0e76b 381 // State 1: Trying to get into final orientation: want [cos, sin] == [1<<30, 0].
DavidEGrayson 20:dbec34f0e76b 382 uint8_t state = 0;
DavidEGrayson 20:dbec34f0e76b 383
DavidEGrayson 27:2456f68be679 384 Pacer reportPacer(200000);
DavidEGrayson 27:2456f68be679 385 Pacer motorUpdatePacer(10000);
DavidEGrayson 21:c279c6a83671 386
DavidEGrayson 27:2456f68be679 387 float integral = 0;
DavidEGrayson 27:2456f68be679 388
DavidEGrayson 27:2456f68be679 389 motorsSpeedSet(-settleSpeed, settleSpeed); // avoid waiting 10 ms before we start settling
DavidEGrayson 27:2456f68be679 390
DavidEGrayson 19:a11ffc903774 391 while(1)
DavidEGrayson 19:a11ffc903774 392 {
DavidEGrayson 27:2456f68be679 393 led1 = (state == 1);
DavidEGrayson 27:2456f68be679 394
DavidEGrayson 20:dbec34f0e76b 395 updateReckonerFromEncoders();
DavidEGrayson 37:23000a47ed2b 396 loggerService();
DavidEGrayson 37:23000a47ed2b 397
DavidEGrayson 20:dbec34f0e76b 398 float dot = dotProduct();
DavidEGrayson 20:dbec34f0e76b 399 int16_t speedModification = -dot * settleModificationStrength;
DavidEGrayson 20:dbec34f0e76b 400 if (speedModification > settleModificationStrength)
DavidEGrayson 20:dbec34f0e76b 401 {
DavidEGrayson 20:dbec34f0e76b 402 speedModification = settleModificationStrength;
DavidEGrayson 20:dbec34f0e76b 403 }
DavidEGrayson 20:dbec34f0e76b 404 else if (speedModification < -settleModificationStrength)
DavidEGrayson 20:dbec34f0e76b 405 {
DavidEGrayson 20:dbec34f0e76b 406 speedModification = -settleModificationStrength;
DavidEGrayson 20:dbec34f0e76b 407 }
DavidEGrayson 20:dbec34f0e76b 408
DavidEGrayson 21:c279c6a83671 409 if (state == 0 && timer.read_ms() >= 2000 && reckoner.cos > (1 << 29))
DavidEGrayson 19:a11ffc903774 410 {
DavidEGrayson 21:c279c6a83671 411 // Stop turning and start trying to maintain the right position.
DavidEGrayson 20:dbec34f0e76b 412 state = 1;
DavidEGrayson 20:dbec34f0e76b 413 }
DavidEGrayson 20:dbec34f0e76b 414
DavidEGrayson 21:c279c6a83671 415 if (state == 1 && timer.read_ms() >= 5000)
DavidEGrayson 21:c279c6a83671 416 {
DavidEGrayson 21:c279c6a83671 417 // Stop moving.
DavidEGrayson 21:c279c6a83671 418 break;
DavidEGrayson 21:c279c6a83671 419 }
DavidEGrayson 21:c279c6a83671 420
DavidEGrayson 27:2456f68be679 421 if (motorUpdatePacer.pace())
DavidEGrayson 20:dbec34f0e76b 422 {
DavidEGrayson 27:2456f68be679 423 int16_t rotationSpeed;
DavidEGrayson 27:2456f68be679 424 if (state == 1)
DavidEGrayson 27:2456f68be679 425 {
DavidEGrayson 27:2456f68be679 426 float s = (float)reckoner.sin / (1 << 30);
DavidEGrayson 27:2456f68be679 427 integral += s;
DavidEGrayson 27:2456f68be679 428 rotationSpeed = -(s * 2400 + integral * 20);
DavidEGrayson 27:2456f68be679 429
DavidEGrayson 27:2456f68be679 430 if (rotationSpeed > 300)
DavidEGrayson 27:2456f68be679 431 {
DavidEGrayson 27:2456f68be679 432 rotationSpeed = 300;
DavidEGrayson 27:2456f68be679 433 }
DavidEGrayson 27:2456f68be679 434 if (rotationSpeed < -300)
DavidEGrayson 27:2456f68be679 435 {
DavidEGrayson 27:2456f68be679 436 rotationSpeed = -300;
DavidEGrayson 27:2456f68be679 437 }
DavidEGrayson 27:2456f68be679 438 }
DavidEGrayson 27:2456f68be679 439 else
DavidEGrayson 27:2456f68be679 440 {
DavidEGrayson 27:2456f68be679 441 rotationSpeed = settleSpeed;
DavidEGrayson 27:2456f68be679 442 }
DavidEGrayson 27:2456f68be679 443
DavidEGrayson 27:2456f68be679 444 int16_t speedLeft = -rotationSpeed + speedModification;
DavidEGrayson 27:2456f68be679 445 int16_t speedRight = rotationSpeed + speedModification;
DavidEGrayson 27:2456f68be679 446 motorsSpeedSet(speedLeft, speedRight);
DavidEGrayson 20:dbec34f0e76b 447 }
DavidEGrayson 21:c279c6a83671 448
DavidEGrayson 21:c279c6a83671 449 if (state == 1 && reportPacer.pace())
DavidEGrayson 21:c279c6a83671 450 {
DavidEGrayson 21:c279c6a83671 451 pc.printf("%11d %11d %11d %11d | %11f %11f\r\n",
DavidEGrayson 21:c279c6a83671 452 reckoner.cos, reckoner.sin, reckoner.x, reckoner.y,
DavidEGrayson 21:c279c6a83671 453 determinant(), dotProduct());
DavidEGrayson 21:c279c6a83671 454 }
DavidEGrayson 19:a11ffc903774 455 }
DavidEGrayson 20:dbec34f0e76b 456
DavidEGrayson 21:c279c6a83671 457 // Done! Stop moving.
DavidEGrayson 20:dbec34f0e76b 458 motorsSpeedSet(0, 0);
DavidEGrayson 18:b65fbb795396 459 }