David Grayson / Mbed 2 deprecated LineFollowing

David's line following code from the LVBots competition, 2015.

Dependencies:   GeneralDebouncer Pacer PololuEncoder mbed

Fork of DeadReckoning by David Grayson

main.cpp@28:4374035df5e0, 2014-03-01 (annotated)

Committer:
DavidEGrayson
Date:
Sat Mar 01 03:13:57 2014 +0000
Revision:
28:4374035df5e0
Parent:
27:2456f68be679
Child:
29:cfcf08d8ac79
Discovered that 4 out of the 6 analog inputs on the mbed are severely messed up.  Might need to get a new mbed or do digital filtering (a median of three readings out to work).

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