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@48:597738b77f77, 2019-08-13 (annotated)

Committer:
DavidEGrayson
Date:
Tue Aug 13 21:21:17 2019 +0000
Revision:
48:597738b77f77
Parent:
47:9773dc14c834 
Changes from before the contest, I think.

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