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@39:b19dfc5d4d4b, 2019-07-25 (annotated)

Committer:
DavidEGrayson
Date:
Thu Jul 25 02:11:25 2019 +0000
Revision:
39:b19dfc5d4d4b
Parent:
38:5e93a479c244
Child:
40:6fa672be85ec
New motors!  100:1 HP.

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