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@47:9773dc14c834, 2019-07-31 (annotated)

Committer:
DavidEGrayson
Date:
Wed Jul 31 07:14:09 2019 +0000
Revision:
47:9773dc14c834
Parent:
46:df2c2d25c070
Child:
48:597738b77f77
Got some awesome results using carefully calculated scale factors for the gyro!

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