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@21:c279c6a83671, 2014-02-27 (annotated)

Committer:
DavidEGrayson
Date:
Thu Feb 27 23:20:34 2014 +0000
Revision:
21:c279c6a83671
Parent:
20:dbec34f0e76b
Child:
22:44c032e59ff5
Wrote a whole bunch of code that could theoretically allow the robot to compete, but it has not been tested at all yet.

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