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@25:73c2eedb3b91, 2014-02-28 (annotated)

Committer:
DavidEGrayson
Date:
Fri Feb 28 01:26:18 2014 +0000
Revision:
25:73c2eedb3b91
Parent:
24:fc01d9125d3b
Child:
26:7e7c376a7446
Some stuff.  I am about to take away the foundLineTimer in an attempt to make it better at finding the line.

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