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@46:df2c2d25c070, 2019-07-28 (annotated)

Committer:
DavidEGrayson
Date:
Sun Jul 28 22:20:12 2019 +0000
Revision:
46:df2c2d25c070
Parent:
45:81dd782bc0b4
Child:
47:9773dc14c834
Some minor changes.  The overall dead reckoning is working now, but I don't think I did much to fix it.

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