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@22:44c032e59ff5, 2014-02-28 (annotated)

Committer:
DavidEGrayson
Date:
Fri Feb 28 00:16:49 2014 +0000
Revision:
22:44c032e59ff5
Parent:
21:c279c6a83671
Child:
23:aae5cbe3b924
Fixed the code for calibrating.  Added testCalibrate.

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 22:44c032e59ff5 46 testCalibrate();
DavidEGrayson 2:968338353aef 47
DavidEGrayson 21:c279c6a83671 48 // Real routines for the contest.
DavidEGrayson 21:c279c6a83671 49 setLeds(1, 0, 0, 0);
DavidEGrayson 21:c279c6a83671 50 waitForSignalToStart();
DavidEGrayson 21:c279c6a83671 51 setLeds(0, 1, 0, 0);
DavidEGrayson 21:c279c6a83671 52 findLineAndCalibrate();
DavidEGrayson 21:c279c6a83671 53 setLeds(1, 1, 0, 0);
DavidEGrayson 21:c279c6a83671 54 turnRightToFindLine();
DavidEGrayson 21:c279c6a83671 55 setLeds(0, 0, 1, 0);
DavidEGrayson 21:c279c6a83671 56 followLineToEnd();
DavidEGrayson 21:c279c6a83671 57 setLeds(1, 0, 1, 0);
DavidEGrayson 21:c279c6a83671 58 driveHomeAlmost();
DavidEGrayson 21:c279c6a83671 59 setLeds(0, 1, 1, 0);
DavidEGrayson 21:c279c6a83671 60 finalSettleIn();
DavidEGrayson 21:c279c6a83671 61 setLeds(1, 1, 1, 1);
DavidEGrayson 21:c279c6a83671 62 while(1){}
DavidEGrayson 0:e77a0edb9878 63 }
DavidEGrayson 12:835a4d24ae3b 64
DavidEGrayson 12:835a4d24ae3b 65 void updateReckonerFromEncoders()
DavidEGrayson 12:835a4d24ae3b 66 {
DavidEGrayson 12:835a4d24ae3b 67 while(encoderBuffer.hasEvents())
DavidEGrayson 12:835a4d24ae3b 68 {
DavidEGrayson 12:835a4d24ae3b 69 PololuEncoderEvent event = encoderBuffer.readEvent();
DavidEGrayson 12:835a4d24ae3b 70 switch(event)
DavidEGrayson 12:835a4d24ae3b 71 {
DavidEGrayson 17:2df9861f53ee 72 case ENCODER_LEFT | POLOLU_ENCODER_EVENT_INC:
DavidEGrayson 17:2df9861f53ee 73 reckoner.handleTickLeftForward();
DavidEGrayson 17:2df9861f53ee 74 break;
DavidEGrayson 17:2df9861f53ee 75 case ENCODER_LEFT | POLOLU_ENCODER_EVENT_DEC:
DavidEGrayson 17:2df9861f53ee 76 reckoner.handleTickLeftBackward();
DavidEGrayson 17:2df9861f53ee 77 break;
DavidEGrayson 17:2df9861f53ee 78 case ENCODER_RIGHT | POLOLU_ENCODER_EVENT_INC:
DavidEGrayson 17:2df9861f53ee 79 reckoner.handleTickRightForward();
DavidEGrayson 17:2df9861f53ee 80 break;
DavidEGrayson 17:2df9861f53ee 81 case ENCODER_RIGHT | POLOLU_ENCODER_EVENT_DEC:
DavidEGrayson 17:2df9861f53ee 82 reckoner.handleTickRightBackward();
DavidEGrayson 17:2df9861f53ee 83 break;
DavidEGrayson 12:835a4d24ae3b 84 }
DavidEGrayson 12:835a4d24ae3b 85 }
DavidEGrayson 12:835a4d24ae3b 86 }
DavidEGrayson 17:2df9861f53ee 87
DavidEGrayson 19:a11ffc903774 88 float magnitude()
DavidEGrayson 19:a11ffc903774 89 {
DavidEGrayson 19:a11ffc903774 90 return sqrt((float)reckoner.x * reckoner.x + (float)reckoner.y * reckoner.y);
DavidEGrayson 19:a11ffc903774 91 }
DavidEGrayson 19:a11ffc903774 92
DavidEGrayson 20:dbec34f0e76b 93 float dotProduct()
DavidEGrayson 20:dbec34f0e76b 94 {
DavidEGrayson 20:dbec34f0e76b 95 float s = (float)reckoner.sin / (1 << 30);
DavidEGrayson 20:dbec34f0e76b 96 float c = (float)reckoner.cos / (1 << 30);
DavidEGrayson 20:dbec34f0e76b 97 float magn = magnitude();
DavidEGrayson 20:dbec34f0e76b 98 if (magn == 0){ return 0; }
DavidEGrayson 20:dbec34f0e76b 99 return ((float)reckoner.x * c + (float)reckoner.y * s) / magn;
DavidEGrayson 20:dbec34f0e76b 100 }
DavidEGrayson 20:dbec34f0e76b 101
DavidEGrayson 18:b65fbb795396 102 // The closer this is to zero, the closer we are to pointing towards the home position.
DavidEGrayson 18:b65fbb795396 103 // It is basically a cross product of the two vectors (x, y) and (cos, sin).
DavidEGrayson 19:a11ffc903774 104 float determinant()
DavidEGrayson 18:b65fbb795396 105 {
DavidEGrayson 18:b65fbb795396 106 // TODO: get rid of the magic numbers here (i.e. 30)
DavidEGrayson 18:b65fbb795396 107 float s = (float)reckoner.sin / (1 << 30);
DavidEGrayson 18:b65fbb795396 108 float c = (float)reckoner.cos / (1 << 30);
DavidEGrayson 19:a11ffc903774 109 return (reckoner.x * s - reckoner.y * c) / magnitude();
DavidEGrayson 19:a11ffc903774 110 }
DavidEGrayson 19:a11ffc903774 111
DavidEGrayson 21:c279c6a83671 112 int16_t reduceSpeed(int16_t speed, int32_t reduction)
DavidEGrayson 19:a11ffc903774 113 {
DavidEGrayson 19:a11ffc903774 114 if (reduction > speed)
DavidEGrayson 19:a11ffc903774 115 {
DavidEGrayson 19:a11ffc903774 116 return 0;
DavidEGrayson 19:a11ffc903774 117 }
DavidEGrayson 19:a11ffc903774 118 else
DavidEGrayson 19:a11ffc903774 119 {
DavidEGrayson 19:a11ffc903774 120 return speed - reduction;
DavidEGrayson 19:a11ffc903774 121 }
DavidEGrayson 18:b65fbb795396 122 }
DavidEGrayson 18:b65fbb795396 123
DavidEGrayson 21:c279c6a83671 124 // Returns true if each line sensor has one third of a volt of difference between the
DavidEGrayson 21:c279c6a83671 125 // maximum seen value and the minimum.
DavidEGrayson 21:c279c6a83671 126 bool calibrationLooksGood()
DavidEGrayson 21:c279c6a83671 127 {
DavidEGrayson 21:c279c6a83671 128 for(uint8_t s = 0; s < LINE_SENSOR_COUNT; s++)
DavidEGrayson 21:c279c6a83671 129 {
DavidEGrayson 21:c279c6a83671 130 uint16_t contrast = lineTracker.calibratedMaximum[s] - lineTracker.calibratedMinimum[s];
DavidEGrayson 21:c279c6a83671 131 if (contrast < 9929) // 0xFFFF*0.5/3.3 = 9929
DavidEGrayson 21:c279c6a83671 132 {
DavidEGrayson 21:c279c6a83671 133 return false;
DavidEGrayson 21:c279c6a83671 134 }
DavidEGrayson 21:c279c6a83671 135 }
DavidEGrayson 21:c279c6a83671 136 return true;
DavidEGrayson 21:c279c6a83671 137 }
DavidEGrayson 21:c279c6a83671 138
DavidEGrayson 21:c279c6a83671 139 void waitForSignalToStart()
DavidEGrayson 21:c279c6a83671 140 {
DavidEGrayson 21:c279c6a83671 141 while(!button1DefinitelyPressed())
DavidEGrayson 21:c279c6a83671 142 {
DavidEGrayson 21:c279c6a83671 143 updateReckonerFromEncoders();
DavidEGrayson 21:c279c6a83671 144 }
DavidEGrayson 21:c279c6a83671 145 reckoner.reset();
DavidEGrayson 21:c279c6a83671 146 }
DavidEGrayson 21:c279c6a83671 147
DavidEGrayson 20:dbec34f0e76b 148
DavidEGrayson 21:c279c6a83671 149 void findLineAndCalibrate()
DavidEGrayson 20:dbec34f0e76b 150 {
DavidEGrayson 21:c279c6a83671 151 const int32_t straightDriveStrength = 1000;
DavidEGrayson 21:c279c6a83671 152
DavidEGrayson 21:c279c6a83671 153 Timer timer;
DavidEGrayson 21:c279c6a83671 154 timer.start();
DavidEGrayson 21:c279c6a83671 155
DavidEGrayson 21:c279c6a83671 156 Timer goodCalibrationTimer;
DavidEGrayson 21:c279c6a83671 157 bool goodCalibration = false;
DavidEGrayson 21:c279c6a83671 158
DavidEGrayson 21:c279c6a83671 159 while(1)
DavidEGrayson 21:c279c6a83671 160 {
DavidEGrayson 21:c279c6a83671 161 lineTracker.read();
DavidEGrayson 21:c279c6a83671 162 lineTracker.updateCalibration();
DavidEGrayson 21:c279c6a83671 163 updateReckonerFromEncoders();
DavidEGrayson 21:c279c6a83671 164
DavidEGrayson 21:c279c6a83671 165 // Keep the robot pointing the in the right direction. This should basically keep it going straight.
DavidEGrayson 21:c279c6a83671 166 int16_t speedLeft = drivingSpeed;
DavidEGrayson 21:c279c6a83671 167 int16_t speedRight = drivingSpeed;
DavidEGrayson 21:c279c6a83671 168 int32_t reduction = reckoner.sin / (1<<15) * straightDriveStrength / (1 << 15);
DavidEGrayson 21:c279c6a83671 169 if (reduction > 0)
DavidEGrayson 21:c279c6a83671 170 {
DavidEGrayson 22:44c032e59ff5 171 speedRight = reduceSpeed(speedRight, reduction);
DavidEGrayson 21:c279c6a83671 172 }
DavidEGrayson 21:c279c6a83671 173 else
DavidEGrayson 21:c279c6a83671 174 {
DavidEGrayson 22:44c032e59ff5 175 speedLeft = reduceSpeed(speedLeft, -reduction);
DavidEGrayson 21:c279c6a83671 176 }
DavidEGrayson 21:c279c6a83671 177 motorsSpeedSet(speedLeft, speedRight);
DavidEGrayson 21:c279c6a83671 178
DavidEGrayson 21:c279c6a83671 179 if (goodCalibration)
DavidEGrayson 21:c279c6a83671 180 {
DavidEGrayson 22:44c032e59ff5 181 if(goodCalibrationTimer.read_ms() >= 300)
DavidEGrayson 21:c279c6a83671 182 {
DavidEGrayson 21:c279c6a83671 183 // The calibration was good and we traveled for a bit of time after that,
DavidEGrayson 21:c279c6a83671 184 // so we must be a bit over the line.
DavidEGrayson 21:c279c6a83671 185 break;
DavidEGrayson 21:c279c6a83671 186 }
DavidEGrayson 21:c279c6a83671 187 }
DavidEGrayson 21:c279c6a83671 188 else
DavidEGrayson 21:c279c6a83671 189 {
DavidEGrayson 21:c279c6a83671 190 if(calibrationLooksGood())
DavidEGrayson 21:c279c6a83671 191 {
DavidEGrayson 21:c279c6a83671 192 goodCalibration = true;
DavidEGrayson 21:c279c6a83671 193 goodCalibrationTimer.start();
DavidEGrayson 21:c279c6a83671 194 }
DavidEGrayson 21:c279c6a83671 195 }
DavidEGrayson 21:c279c6a83671 196 }
DavidEGrayson 21:c279c6a83671 197 }
DavidEGrayson 21:c279c6a83671 198
DavidEGrayson 21:c279c6a83671 199 void turnRightToFindLine()
DavidEGrayson 21:c279c6a83671 200 {
DavidEGrayson 21:c279c6a83671 201 while(1)
DavidEGrayson 21:c279c6a83671 202 {
DavidEGrayson 21:c279c6a83671 203 lineTracker.read();
DavidEGrayson 21:c279c6a83671 204 lineTracker.updateCalibration();
DavidEGrayson 21:c279c6a83671 205 updateReckonerFromEncoders();
DavidEGrayson 21:c279c6a83671 206
DavidEGrayson 21:c279c6a83671 207 if(lineTracker.getLineVisible())
DavidEGrayson 21:c279c6a83671 208 {
DavidEGrayson 21:c279c6a83671 209 break;
DavidEGrayson 21:c279c6a83671 210 }
DavidEGrayson 21:c279c6a83671 211
DavidEGrayson 21:c279c6a83671 212 motorsSpeedSet(300, 100);
DavidEGrayson 21:c279c6a83671 213 }
DavidEGrayson 21:c279c6a83671 214 }
DavidEGrayson 21:c279c6a83671 215
DavidEGrayson 21:c279c6a83671 216 void followLineToEnd()
DavidEGrayson 21:c279c6a83671 217 {
DavidEGrayson 21:c279c6a83671 218 GeneralDebouncer lineStatus(10000);
DavidEGrayson 21:c279c6a83671 219 const uint32_t lineDebounceTime = 100000;
DavidEGrayson 21:c279c6a83671 220 const int followLineStrength = 300;
DavidEGrayson 21:c279c6a83671 221
DavidEGrayson 21:c279c6a83671 222 while(1)
DavidEGrayson 21:c279c6a83671 223 {
DavidEGrayson 21:c279c6a83671 224 lineTracker.read();
DavidEGrayson 21:c279c6a83671 225 updateReckonerFromEncoders();
DavidEGrayson 21:c279c6a83671 226
DavidEGrayson 21:c279c6a83671 227 lineStatus.update(lineTracker.getLineVisible());
DavidEGrayson 21:c279c6a83671 228
DavidEGrayson 21:c279c6a83671 229 bool lostLine = lineStatus.getState() == false &&
DavidEGrayson 21:c279c6a83671 230 lineStatus.getTimeInCurrentStateMicroseconds() > lineDebounceTime;
DavidEGrayson 21:c279c6a83671 231
DavidEGrayson 21:c279c6a83671 232 if(lostLine)
DavidEGrayson 21:c279c6a83671 233 {
DavidEGrayson 21:c279c6a83671 234 break;
DavidEGrayson 21:c279c6a83671 235 }
DavidEGrayson 21:c279c6a83671 236
DavidEGrayson 21:c279c6a83671 237 if(lineTracker.getLineVisible())
DavidEGrayson 21:c279c6a83671 238 {
DavidEGrayson 21:c279c6a83671 239 break;
DavidEGrayson 21:c279c6a83671 240 }
DavidEGrayson 21:c279c6a83671 241
DavidEGrayson 21:c279c6a83671 242 int16_t speedLeft = drivingSpeed;
DavidEGrayson 21:c279c6a83671 243 int16_t speedRight = drivingSpeed;
DavidEGrayson 21:c279c6a83671 244 int16_t reduction = (lineTracker.getLinePosition() - 1500) * followLineStrength / 1500;
DavidEGrayson 21:c279c6a83671 245 if(reduction < 0)
DavidEGrayson 21:c279c6a83671 246 {
DavidEGrayson 21:c279c6a83671 247 reduceSpeed(speedLeft, -reduction);
DavidEGrayson 21:c279c6a83671 248 }
DavidEGrayson 21:c279c6a83671 249 else
DavidEGrayson 21:c279c6a83671 250 {
DavidEGrayson 21:c279c6a83671 251 reduceSpeed(speedRight, reduction);
DavidEGrayson 21:c279c6a83671 252 }
DavidEGrayson 21:c279c6a83671 253
DavidEGrayson 21:c279c6a83671 254
DavidEGrayson 21:c279c6a83671 255 motorsSpeedSet(speedLeft, speedRight);
DavidEGrayson 21:c279c6a83671 256 }
DavidEGrayson 20:dbec34f0e76b 257 }
DavidEGrayson 20:dbec34f0e76b 258
DavidEGrayson 20:dbec34f0e76b 259 void driveHomeAlmost()
DavidEGrayson 18:b65fbb795396 260 {
DavidEGrayson 18:b65fbb795396 261 Timer timer;
DavidEGrayson 18:b65fbb795396 262 timer.start();
DavidEGrayson 19:a11ffc903774 263
DavidEGrayson 19:a11ffc903774 264 while(1)
DavidEGrayson 18:b65fbb795396 265 {
DavidEGrayson 18:b65fbb795396 266 updateReckonerFromEncoders();
DavidEGrayson 19:a11ffc903774 267
DavidEGrayson 19:a11ffc903774 268 float magn = magnitude();
DavidEGrayson 19:a11ffc903774 269
DavidEGrayson 19:a11ffc903774 270 if (magn < (1<<17))
DavidEGrayson 18:b65fbb795396 271 {
DavidEGrayson 19:a11ffc903774 272 // We are within 8 encoder ticks, so go to the next step.
DavidEGrayson 19:a11ffc903774 273 break;
DavidEGrayson 19:a11ffc903774 274 }
DavidEGrayson 19:a11ffc903774 275
DavidEGrayson 19:a11ffc903774 276 float det = determinant();
DavidEGrayson 19:a11ffc903774 277
DavidEGrayson 21:c279c6a83671 278 int16_t speedLeft = drivingSpeed;
DavidEGrayson 21:c279c6a83671 279 int16_t speedRight = drivingSpeed;
DavidEGrayson 19:a11ffc903774 280 if (magn < (1<<20)) // Within 64 encoder ticks of the origin, so slow down.
DavidEGrayson 18:b65fbb795396 281 {
DavidEGrayson 21:c279c6a83671 282 int16_t reduction = (1 - magn/(1<<20)) * drivingSpeed;
DavidEGrayson 19:a11ffc903774 283 speedLeft = reduceSpeed(speedLeft, reduction);
DavidEGrayson 19:a11ffc903774 284 speedRight = reduceSpeed(speedRight, reduction);
DavidEGrayson 19:a11ffc903774 285 }
DavidEGrayson 19:a11ffc903774 286
DavidEGrayson 19:a11ffc903774 287 if (det > 0)
DavidEGrayson 19:a11ffc903774 288 {
DavidEGrayson 19:a11ffc903774 289 speedLeft = reduceSpeed(speedLeft, det * 1000);
DavidEGrayson 18:b65fbb795396 290 }
DavidEGrayson 18:b65fbb795396 291 else
DavidEGrayson 18:b65fbb795396 292 {
DavidEGrayson 19:a11ffc903774 293 speedRight = reduceSpeed(speedRight, -det * 1000);
DavidEGrayson 18:b65fbb795396 294 }
DavidEGrayson 19:a11ffc903774 295 motorsSpeedSet(speedLeft, speedRight);
DavidEGrayson 18:b65fbb795396 296 }
DavidEGrayson 18:b65fbb795396 297
DavidEGrayson 20:dbec34f0e76b 298 motorsSpeedSet(0, 0);
DavidEGrayson 20:dbec34f0e76b 299 }
DavidEGrayson 20:dbec34f0e76b 300
DavidEGrayson 20:dbec34f0e76b 301 void finalSettleIn()
DavidEGrayson 20:dbec34f0e76b 302 {
DavidEGrayson 20:dbec34f0e76b 303 const int16_t settleSpeed = 200;
DavidEGrayson 20:dbec34f0e76b 304 const int16_t settleModificationStrength = 100;
DavidEGrayson 20:dbec34f0e76b 305
DavidEGrayson 20:dbec34f0e76b 306 Timer timer;
DavidEGrayson 20:dbec34f0e76b 307 timer.start();
DavidEGrayson 20:dbec34f0e76b 308
DavidEGrayson 20:dbec34f0e76b 309 // State 0: rotating
DavidEGrayson 20:dbec34f0e76b 310 // State 1: Trying to get into final orientation: want [cos, sin] == [1<<30, 0].
DavidEGrayson 20:dbec34f0e76b 311 uint8_t state = 0;
DavidEGrayson 20:dbec34f0e76b 312
DavidEGrayson 21:c279c6a83671 313 Pacer reportPacer(200000);
DavidEGrayson 21:c279c6a83671 314
DavidEGrayson 19:a11ffc903774 315 while(1)
DavidEGrayson 19:a11ffc903774 316 {
DavidEGrayson 20:dbec34f0e76b 317 updateReckonerFromEncoders();
DavidEGrayson 20:dbec34f0e76b 318
DavidEGrayson 20:dbec34f0e76b 319 float dot = dotProduct();
DavidEGrayson 20:dbec34f0e76b 320 int16_t speedModification = -dot * settleModificationStrength;
DavidEGrayson 20:dbec34f0e76b 321 if (speedModification > settleModificationStrength)
DavidEGrayson 20:dbec34f0e76b 322 {
DavidEGrayson 20:dbec34f0e76b 323 speedModification = settleModificationStrength;
DavidEGrayson 20:dbec34f0e76b 324 }
DavidEGrayson 20:dbec34f0e76b 325 else if (speedModification < -settleModificationStrength)
DavidEGrayson 20:dbec34f0e76b 326 {
DavidEGrayson 20:dbec34f0e76b 327 speedModification = -settleModificationStrength;
DavidEGrayson 20:dbec34f0e76b 328 }
DavidEGrayson 20:dbec34f0e76b 329
DavidEGrayson 21:c279c6a83671 330 if (state == 0 && timer.read_ms() >= 2000 && reckoner.cos > (1 << 29))
DavidEGrayson 19:a11ffc903774 331 {
DavidEGrayson 21:c279c6a83671 332 // Stop turning and start trying to maintain the right position.
DavidEGrayson 20:dbec34f0e76b 333 state = 1;
DavidEGrayson 20:dbec34f0e76b 334 }
DavidEGrayson 20:dbec34f0e76b 335
DavidEGrayson 21:c279c6a83671 336 if (state == 1 && timer.read_ms() >= 5000)
DavidEGrayson 21:c279c6a83671 337 {
DavidEGrayson 21:c279c6a83671 338 // Stop moving.
DavidEGrayson 21:c279c6a83671 339 break;
DavidEGrayson 21:c279c6a83671 340 }
DavidEGrayson 21:c279c6a83671 341
DavidEGrayson 20:dbec34f0e76b 342 int16_t rotationSpeed;
DavidEGrayson 20:dbec34f0e76b 343 if (state == 1)
DavidEGrayson 20:dbec34f0e76b 344 {
DavidEGrayson 20:dbec34f0e76b 345 float s = (float)reckoner.sin / (1 << 30);
DavidEGrayson 21:c279c6a83671 346 rotationSpeed = -s * 600;
DavidEGrayson 20:dbec34f0e76b 347 }
DavidEGrayson 20:dbec34f0e76b 348 else
DavidEGrayson 20:dbec34f0e76b 349 {
DavidEGrayson 20:dbec34f0e76b 350 rotationSpeed = settleSpeed;
DavidEGrayson 20:dbec34f0e76b 351 }
DavidEGrayson 20:dbec34f0e76b 352
DavidEGrayson 20:dbec34f0e76b 353 int16_t speedLeft = -rotationSpeed + speedModification;
DavidEGrayson 20:dbec34f0e76b 354 int16_t speedRight = rotationSpeed + speedModification;
DavidEGrayson 20:dbec34f0e76b 355 motorsSpeedSet(speedLeft, speedRight);
DavidEGrayson 21:c279c6a83671 356
DavidEGrayson 21:c279c6a83671 357 if (state == 1 && reportPacer.pace())
DavidEGrayson 21:c279c6a83671 358 {
DavidEGrayson 21:c279c6a83671 359 pc.printf("%11d %11d %11d %11d | %11f %11f\r\n",
DavidEGrayson 21:c279c6a83671 360 reckoner.cos, reckoner.sin, reckoner.x, reckoner.y,
DavidEGrayson 21:c279c6a83671 361 determinant(), dotProduct());
DavidEGrayson 21:c279c6a83671 362 }
DavidEGrayson 19:a11ffc903774 363 }
DavidEGrayson 20:dbec34f0e76b 364
DavidEGrayson 21:c279c6a83671 365 // Done! Stop moving.
DavidEGrayson 20:dbec34f0e76b 366 motorsSpeedSet(0, 0);
DavidEGrayson 18:b65fbb795396 367 }