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@40:6fa672be85ec, 2019-07-25 (annotated)

Committer:
DavidEGrayson
Date:
Thu Jul 25 02:53:34 2019 +0000
Revision:
40:6fa672be85ec
Parent:
39:b19dfc5d4d4b
Child:
41:3ead1dd2cc3a
Add TurnSensor and L3G code but I am not happy with how the Gyro drifts a degree every few seconds or so.

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