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@30:84be2d602dc0, 2014-03-04 (annotated)

Committer:
DavidEGrayson
Date:
Tue Mar 04 02:09:54 2014 +0000
Revision:
30:84be2d602dc0
Parent:
29:cfcf08d8ac79
Child:
31:739b91331f31
Seems like the QTR-3RC could work, based on my tests on P10.

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