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@27:2456f68be679, 2014-03-01 (annotated)

Committer:
DavidEGrayson
Date:
Sat Mar 01 01:46:35 2014 +0000
Revision:
27:2456f68be679
Parent:
26:7e7c376a7446
Child:
28:4374035df5e0
Fixed a major bug in the line following (reduceSpeed return value was not used).  Made finalSettleIn better by adding an integral term and increasing the settleSpeed from 200 to 300.

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 27:2456f68be679 56 //setLeds(1, 1, 0, 0);
DavidEGrayson 27:2456f68be679 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 GeneralDebouncer lineStatus(10000);
DavidEGrayson 24:fc01d9125d3b 180 while(1)
DavidEGrayson 24:fc01d9125d3b 181 {
DavidEGrayson 24:fc01d9125d3b 182 lineTracker.read();
DavidEGrayson 24:fc01d9125d3b 183 lineTracker.updateCalibration();
DavidEGrayson 24:fc01d9125d3b 184 updateReckonerFromEncoders();
DavidEGrayson 24:fc01d9125d3b 185 updateMotorsToDriveStraight();
DavidEGrayson 24:fc01d9125d3b 186 lineStatus.update(lineTracker.getLineVisible());
DavidEGrayson 24:fc01d9125d3b 187
DavidEGrayson 26:7e7c376a7446 188 if(lineStatus.getState() == true && lineStatus.getTimeInCurrentStateMicroseconds() > 100000)
DavidEGrayson 24:fc01d9125d3b 189 {
DavidEGrayson 26:7e7c376a7446 190 break;
DavidEGrayson 24:fc01d9125d3b 191 }
DavidEGrayson 24:fc01d9125d3b 192 }
DavidEGrayson 24:fc01d9125d3b 193 }
DavidEGrayson 20:dbec34f0e76b 194
DavidEGrayson 25:73c2eedb3b91 195 // THIS IS DEPRECATED. Instead we are using loadCalibrationAndFindLine.
DavidEGrayson 21:c279c6a83671 196 void findLineAndCalibrate()
DavidEGrayson 20:dbec34f0e76b 197 {
DavidEGrayson 21:c279c6a83671 198 Timer timer;
DavidEGrayson 21:c279c6a83671 199 timer.start();
DavidEGrayson 21:c279c6a83671 200
DavidEGrayson 21:c279c6a83671 201 Timer goodCalibrationTimer;
DavidEGrayson 21:c279c6a83671 202 bool goodCalibration = false;
DavidEGrayson 21:c279c6a83671 203
DavidEGrayson 21:c279c6a83671 204 while(1)
DavidEGrayson 21:c279c6a83671 205 {
DavidEGrayson 21:c279c6a83671 206 lineTracker.read();
DavidEGrayson 21:c279c6a83671 207 lineTracker.updateCalibration();
DavidEGrayson 24:fc01d9125d3b 208 updateReckonerFromEncoders();
DavidEGrayson 24:fc01d9125d3b 209 updateMotorsToDriveStraight();
DavidEGrayson 21:c279c6a83671 210
DavidEGrayson 21:c279c6a83671 211 if (goodCalibration)
DavidEGrayson 21:c279c6a83671 212 {
DavidEGrayson 22:44c032e59ff5 213 if(goodCalibrationTimer.read_ms() >= 300)
DavidEGrayson 21:c279c6a83671 214 {
DavidEGrayson 21:c279c6a83671 215 // The calibration was good and we traveled for a bit of time after that,
DavidEGrayson 21:c279c6a83671 216 // so we must be a bit over the line.
DavidEGrayson 21:c279c6a83671 217 break;
DavidEGrayson 21:c279c6a83671 218 }
DavidEGrayson 21:c279c6a83671 219 }
DavidEGrayson 21:c279c6a83671 220 else
DavidEGrayson 21:c279c6a83671 221 {
DavidEGrayson 21:c279c6a83671 222 if(calibrationLooksGood())
DavidEGrayson 21:c279c6a83671 223 {
DavidEGrayson 21:c279c6a83671 224 goodCalibration = true;
DavidEGrayson 21:c279c6a83671 225 goodCalibrationTimer.start();
DavidEGrayson 21:c279c6a83671 226 }
DavidEGrayson 21:c279c6a83671 227 }
DavidEGrayson 21:c279c6a83671 228 }
DavidEGrayson 21:c279c6a83671 229 }
DavidEGrayson 21:c279c6a83671 230
DavidEGrayson 21:c279c6a83671 231 void turnRightToFindLine()
DavidEGrayson 21:c279c6a83671 232 {
DavidEGrayson 21:c279c6a83671 233 while(1)
DavidEGrayson 21:c279c6a83671 234 {
DavidEGrayson 21:c279c6a83671 235 lineTracker.read();
DavidEGrayson 21:c279c6a83671 236 lineTracker.updateCalibration();
DavidEGrayson 21:c279c6a83671 237 updateReckonerFromEncoders();
DavidEGrayson 21:c279c6a83671 238
DavidEGrayson 21:c279c6a83671 239 if(lineTracker.getLineVisible())
DavidEGrayson 21:c279c6a83671 240 {
DavidEGrayson 21:c279c6a83671 241 break;
DavidEGrayson 21:c279c6a83671 242 }
DavidEGrayson 21:c279c6a83671 243
DavidEGrayson 21:c279c6a83671 244 motorsSpeedSet(300, 100);
DavidEGrayson 21:c279c6a83671 245 }
DavidEGrayson 21:c279c6a83671 246 }
DavidEGrayson 21:c279c6a83671 247
DavidEGrayson 21:c279c6a83671 248 void followLineToEnd()
DavidEGrayson 21:c279c6a83671 249 {
DavidEGrayson 26:7e7c376a7446 250 Timer timer;
DavidEGrayson 26:7e7c376a7446 251 timer.start();
DavidEGrayson 26:7e7c376a7446 252
DavidEGrayson 21:c279c6a83671 253 GeneralDebouncer lineStatus(10000);
DavidEGrayson 21:c279c6a83671 254 const uint32_t lineDebounceTime = 100000;
DavidEGrayson 21:c279c6a83671 255 const int followLineStrength = 300;
DavidEGrayson 21:c279c6a83671 256
DavidEGrayson 21:c279c6a83671 257 while(1)
DavidEGrayson 21:c279c6a83671 258 {
DavidEGrayson 21:c279c6a83671 259 lineTracker.read();
DavidEGrayson 21:c279c6a83671 260 updateReckonerFromEncoders();
DavidEGrayson 21:c279c6a83671 261
DavidEGrayson 21:c279c6a83671 262 lineStatus.update(lineTracker.getLineVisible());
DavidEGrayson 21:c279c6a83671 263
DavidEGrayson 21:c279c6a83671 264 bool lostLine = lineStatus.getState() == false &&
DavidEGrayson 26:7e7c376a7446 265 lineStatus.getTimeInCurrentStateMicroseconds() > lineDebounceTime;
DavidEGrayson 26:7e7c376a7446 266 if(lostLine && timer.read_us() >= 300000)
DavidEGrayson 21:c279c6a83671 267 {
DavidEGrayson 21:c279c6a83671 268 break;
DavidEGrayson 21:c279c6a83671 269 }
DavidEGrayson 21:c279c6a83671 270
DavidEGrayson 21:c279c6a83671 271 int16_t speedLeft = drivingSpeed;
DavidEGrayson 21:c279c6a83671 272 int16_t speedRight = drivingSpeed;
DavidEGrayson 21:c279c6a83671 273 int16_t reduction = (lineTracker.getLinePosition() - 1500) * followLineStrength / 1500;
DavidEGrayson 21:c279c6a83671 274 if(reduction < 0)
DavidEGrayson 21:c279c6a83671 275 {
DavidEGrayson 27:2456f68be679 276 speedLeft = reduceSpeed(speedLeft, -reduction);
DavidEGrayson 21:c279c6a83671 277 }
DavidEGrayson 21:c279c6a83671 278 else
DavidEGrayson 21:c279c6a83671 279 {
DavidEGrayson 27:2456f68be679 280 speedRight = reduceSpeed(speedRight, reduction);
DavidEGrayson 21:c279c6a83671 281 }
DavidEGrayson 21:c279c6a83671 282
DavidEGrayson 21:c279c6a83671 283
DavidEGrayson 21:c279c6a83671 284 motorsSpeedSet(speedLeft, speedRight);
DavidEGrayson 21:c279c6a83671 285 }
DavidEGrayson 20:dbec34f0e76b 286 }
DavidEGrayson 20:dbec34f0e76b 287
DavidEGrayson 20:dbec34f0e76b 288 void driveHomeAlmost()
DavidEGrayson 18:b65fbb795396 289 {
DavidEGrayson 18:b65fbb795396 290 Timer timer;
DavidEGrayson 18:b65fbb795396 291 timer.start();
DavidEGrayson 19:a11ffc903774 292
DavidEGrayson 19:a11ffc903774 293 while(1)
DavidEGrayson 18:b65fbb795396 294 {
DavidEGrayson 18:b65fbb795396 295 updateReckonerFromEncoders();
DavidEGrayson 19:a11ffc903774 296
DavidEGrayson 19:a11ffc903774 297 float magn = magnitude();
DavidEGrayson 19:a11ffc903774 298
DavidEGrayson 19:a11ffc903774 299 if (magn < (1<<17))
DavidEGrayson 18:b65fbb795396 300 {
DavidEGrayson 19:a11ffc903774 301 // We are within 8 encoder ticks, so go to the next step.
DavidEGrayson 19:a11ffc903774 302 break;
DavidEGrayson 19:a11ffc903774 303 }
DavidEGrayson 19:a11ffc903774 304
DavidEGrayson 19:a11ffc903774 305 float det = determinant();
DavidEGrayson 19:a11ffc903774 306
DavidEGrayson 21:c279c6a83671 307 int16_t speedLeft = drivingSpeed;
DavidEGrayson 21:c279c6a83671 308 int16_t speedRight = drivingSpeed;
DavidEGrayson 19:a11ffc903774 309 if (magn < (1<<20)) // Within 64 encoder ticks of the origin, so slow down.
DavidEGrayson 18:b65fbb795396 310 {
DavidEGrayson 21:c279c6a83671 311 int16_t reduction = (1 - magn/(1<<20)) * drivingSpeed;
DavidEGrayson 19:a11ffc903774 312 speedLeft = reduceSpeed(speedLeft, reduction);
DavidEGrayson 19:a11ffc903774 313 speedRight = reduceSpeed(speedRight, reduction);
DavidEGrayson 19:a11ffc903774 314 }
DavidEGrayson 19:a11ffc903774 315
DavidEGrayson 19:a11ffc903774 316 if (det > 0)
DavidEGrayson 19:a11ffc903774 317 {
DavidEGrayson 19:a11ffc903774 318 speedLeft = reduceSpeed(speedLeft, det * 1000);
DavidEGrayson 18:b65fbb795396 319 }
DavidEGrayson 18:b65fbb795396 320 else
DavidEGrayson 18:b65fbb795396 321 {
DavidEGrayson 19:a11ffc903774 322 speedRight = reduceSpeed(speedRight, -det * 1000);
DavidEGrayson 18:b65fbb795396 323 }
DavidEGrayson 19:a11ffc903774 324 motorsSpeedSet(speedLeft, speedRight);
DavidEGrayson 18:b65fbb795396 325 }
DavidEGrayson 18:b65fbb795396 326
DavidEGrayson 20:dbec34f0e76b 327 motorsSpeedSet(0, 0);
DavidEGrayson 20:dbec34f0e76b 328 }
DavidEGrayson 20:dbec34f0e76b 329
DavidEGrayson 20:dbec34f0e76b 330 void finalSettleIn()
DavidEGrayson 20:dbec34f0e76b 331 {
DavidEGrayson 27:2456f68be679 332 const int16_t settleSpeed = 300;
DavidEGrayson 27:2456f68be679 333 const int16_t settleModificationStrength = 150;
DavidEGrayson 20:dbec34f0e76b 334
DavidEGrayson 20:dbec34f0e76b 335 Timer timer;
DavidEGrayson 20:dbec34f0e76b 336 timer.start();
DavidEGrayson 20:dbec34f0e76b 337
DavidEGrayson 20:dbec34f0e76b 338 // State 0: rotating
DavidEGrayson 20:dbec34f0e76b 339 // State 1: Trying to get into final orientation: want [cos, sin] == [1<<30, 0].
DavidEGrayson 20:dbec34f0e76b 340 uint8_t state = 0;
DavidEGrayson 20:dbec34f0e76b 341
DavidEGrayson 27:2456f68be679 342 Pacer reportPacer(200000);
DavidEGrayson 27:2456f68be679 343 Pacer motorUpdatePacer(10000);
DavidEGrayson 21:c279c6a83671 344
DavidEGrayson 27:2456f68be679 345 float integral = 0;
DavidEGrayson 27:2456f68be679 346
DavidEGrayson 27:2456f68be679 347 motorsSpeedSet(-settleSpeed, settleSpeed); // avoid waiting 10 ms before we start settling
DavidEGrayson 27:2456f68be679 348
DavidEGrayson 19:a11ffc903774 349 while(1)
DavidEGrayson 19:a11ffc903774 350 {
DavidEGrayson 27:2456f68be679 351 led1 = (state == 1);
DavidEGrayson 27:2456f68be679 352
DavidEGrayson 20:dbec34f0e76b 353 updateReckonerFromEncoders();
DavidEGrayson 20:dbec34f0e76b 354
DavidEGrayson 20:dbec34f0e76b 355 float dot = dotProduct();
DavidEGrayson 20:dbec34f0e76b 356 int16_t speedModification = -dot * settleModificationStrength;
DavidEGrayson 20:dbec34f0e76b 357 if (speedModification > settleModificationStrength)
DavidEGrayson 20:dbec34f0e76b 358 {
DavidEGrayson 20:dbec34f0e76b 359 speedModification = settleModificationStrength;
DavidEGrayson 20:dbec34f0e76b 360 }
DavidEGrayson 20:dbec34f0e76b 361 else if (speedModification < -settleModificationStrength)
DavidEGrayson 20:dbec34f0e76b 362 {
DavidEGrayson 20:dbec34f0e76b 363 speedModification = -settleModificationStrength;
DavidEGrayson 20:dbec34f0e76b 364 }
DavidEGrayson 20:dbec34f0e76b 365
DavidEGrayson 21:c279c6a83671 366 if (state == 0 && timer.read_ms() >= 2000 && reckoner.cos > (1 << 29))
DavidEGrayson 19:a11ffc903774 367 {
DavidEGrayson 21:c279c6a83671 368 // Stop turning and start trying to maintain the right position.
DavidEGrayson 20:dbec34f0e76b 369 state = 1;
DavidEGrayson 20:dbec34f0e76b 370 }
DavidEGrayson 20:dbec34f0e76b 371
DavidEGrayson 21:c279c6a83671 372 if (state == 1 && timer.read_ms() >= 5000)
DavidEGrayson 21:c279c6a83671 373 {
DavidEGrayson 21:c279c6a83671 374 // Stop moving.
DavidEGrayson 21:c279c6a83671 375 break;
DavidEGrayson 21:c279c6a83671 376 }
DavidEGrayson 21:c279c6a83671 377
DavidEGrayson 27:2456f68be679 378 if (motorUpdatePacer.pace())
DavidEGrayson 20:dbec34f0e76b 379 {
DavidEGrayson 27:2456f68be679 380 int16_t rotationSpeed;
DavidEGrayson 27:2456f68be679 381 if (state == 1)
DavidEGrayson 27:2456f68be679 382 {
DavidEGrayson 27:2456f68be679 383 float s = (float)reckoner.sin / (1 << 30);
DavidEGrayson 27:2456f68be679 384 integral += s;
DavidEGrayson 27:2456f68be679 385 rotationSpeed = -(s * 2400 + integral * 20);
DavidEGrayson 27:2456f68be679 386
DavidEGrayson 27:2456f68be679 387 if (rotationSpeed > 300)
DavidEGrayson 27:2456f68be679 388 {
DavidEGrayson 27:2456f68be679 389 rotationSpeed = 300;
DavidEGrayson 27:2456f68be679 390 }
DavidEGrayson 27:2456f68be679 391 if (rotationSpeed < -300)
DavidEGrayson 27:2456f68be679 392 {
DavidEGrayson 27:2456f68be679 393 rotationSpeed = -300;
DavidEGrayson 27:2456f68be679 394 }
DavidEGrayson 27:2456f68be679 395 }
DavidEGrayson 27:2456f68be679 396 else
DavidEGrayson 27:2456f68be679 397 {
DavidEGrayson 27:2456f68be679 398 rotationSpeed = settleSpeed;
DavidEGrayson 27:2456f68be679 399 }
DavidEGrayson 27:2456f68be679 400
DavidEGrayson 27:2456f68be679 401 int16_t speedLeft = -rotationSpeed + speedModification;
DavidEGrayson 27:2456f68be679 402 int16_t speedRight = rotationSpeed + speedModification;
DavidEGrayson 27:2456f68be679 403 motorsSpeedSet(speedLeft, speedRight);
DavidEGrayson 20:dbec34f0e76b 404 }
DavidEGrayson 21:c279c6a83671 405
DavidEGrayson 21:c279c6a83671 406 if (state == 1 && reportPacer.pace())
DavidEGrayson 21:c279c6a83671 407 {
DavidEGrayson 21:c279c6a83671 408 pc.printf("%11d %11d %11d %11d | %11f %11f\r\n",
DavidEGrayson 21:c279c6a83671 409 reckoner.cos, reckoner.sin, reckoner.x, reckoner.y,
DavidEGrayson 21:c279c6a83671 410 determinant(), dotProduct());
DavidEGrayson 21:c279c6a83671 411 }
DavidEGrayson 19:a11ffc903774 412 }
DavidEGrayson 20:dbec34f0e76b 413
DavidEGrayson 21:c279c6a83671 414 // Done! Stop moving.
DavidEGrayson 20:dbec34f0e76b 415 motorsSpeedSet(0, 0);
DavidEGrayson 18:b65fbb795396 416 }