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@32:83a13b06093c, 2014-03-04 (annotated)

Committer:
DavidEGrayson
Date:
Tue Mar 04 04:32:51 2014 +0000
Revision:
32:83a13b06093c
Parent:
31:739b91331f31
Child:
33:58a0ab6e9ad2
getting line sensors to work again;

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 32:83a13b06093c 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 32:83a13b06093c 47 testLineFollowing();
DavidEGrayson 29:cfcf08d8ac79 48 //testAnalog();
DavidEGrayson 31:739b91331f31 49 //testSensorGlitches();
DavidEGrayson 2:968338353aef 50
DavidEGrayson 21:c279c6a83671 51 // Real routines for the contest.
DavidEGrayson 28:4374035df5e0 52 loadCalibration();
DavidEGrayson 28:4374035df5e0 53
DavidEGrayson 21:c279c6a83671 54 setLeds(1, 0, 0, 0);
DavidEGrayson 21:c279c6a83671 55 waitForSignalToStart();
DavidEGrayson 21:c279c6a83671 56 setLeds(0, 1, 0, 0);
DavidEGrayson 25:73c2eedb3b91 57
DavidEGrayson 28:4374035df5e0 58 findLine();
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 32:83a13b06093c 74 /** QTR-3RC **/
DavidEGrayson 32:83a13b06093c 75 lineTracker.calibratedMinimum[0] = 100;
DavidEGrayson 32:83a13b06093c 76 lineTracker.calibratedMinimum[1] = 94;
DavidEGrayson 32:83a13b06093c 77 lineTracker.calibratedMinimum[2] = 103;
DavidEGrayson 32:83a13b06093c 78 lineTracker.calibratedMaximum[0] = 792;
DavidEGrayson 32:83a13b06093c 79 lineTracker.calibratedMaximum[1] = 807;
DavidEGrayson 32:83a13b06093c 80 lineTracker.calibratedMaximum[2] = 1000;
DavidEGrayson 32:83a13b06093c 81
DavidEGrayson 32:83a13b06093c 82 /** QTR-3A
DavidEGrayson 28:4374035df5e0 83 lineTracker.calibratedMinimum[0] = 34872;
DavidEGrayson 28:4374035df5e0 84 lineTracker.calibratedMinimum[1] = 29335;
DavidEGrayson 28:4374035df5e0 85 lineTracker.calibratedMinimum[2] = 23845;
DavidEGrayson 28:4374035df5e0 86 lineTracker.calibratedMaximum[0] = 59726;
DavidEGrayson 28:4374035df5e0 87 lineTracker.calibratedMaximum[1] = 60110;
DavidEGrayson 32:83a13b06093c 88 lineTracker.calibratedMaximum[2] = 58446;
DavidEGrayson 32:83a13b06093c 89 **/
DavidEGrayson 28:4374035df5e0 90 }
DavidEGrayson 28:4374035df5e0 91
DavidEGrayson 12:835a4d24ae3b 92 void updateReckonerFromEncoders()
DavidEGrayson 12:835a4d24ae3b 93 {
DavidEGrayson 12:835a4d24ae3b 94 while(encoderBuffer.hasEvents())
DavidEGrayson 12:835a4d24ae3b 95 {
DavidEGrayson 12:835a4d24ae3b 96 PololuEncoderEvent event = encoderBuffer.readEvent();
DavidEGrayson 12:835a4d24ae3b 97 switch(event)
DavidEGrayson 12:835a4d24ae3b 98 {
DavidEGrayson 17:2df9861f53ee 99 case ENCODER_LEFT | POLOLU_ENCODER_EVENT_INC:
DavidEGrayson 17:2df9861f53ee 100 reckoner.handleTickLeftForward();
DavidEGrayson 17:2df9861f53ee 101 break;
DavidEGrayson 17:2df9861f53ee 102 case ENCODER_LEFT | POLOLU_ENCODER_EVENT_DEC:
DavidEGrayson 17:2df9861f53ee 103 reckoner.handleTickLeftBackward();
DavidEGrayson 17:2df9861f53ee 104 break;
DavidEGrayson 17:2df9861f53ee 105 case ENCODER_RIGHT | POLOLU_ENCODER_EVENT_INC:
DavidEGrayson 17:2df9861f53ee 106 reckoner.handleTickRightForward();
DavidEGrayson 17:2df9861f53ee 107 break;
DavidEGrayson 17:2df9861f53ee 108 case ENCODER_RIGHT | POLOLU_ENCODER_EVENT_DEC:
DavidEGrayson 17:2df9861f53ee 109 reckoner.handleTickRightBackward();
DavidEGrayson 17:2df9861f53ee 110 break;
DavidEGrayson 12:835a4d24ae3b 111 }
DavidEGrayson 12:835a4d24ae3b 112 }
DavidEGrayson 12:835a4d24ae3b 113 }
DavidEGrayson 17:2df9861f53ee 114
DavidEGrayson 19:a11ffc903774 115 float magnitude()
DavidEGrayson 19:a11ffc903774 116 {
DavidEGrayson 19:a11ffc903774 117 return sqrt((float)reckoner.x * reckoner.x + (float)reckoner.y * reckoner.y);
DavidEGrayson 19:a11ffc903774 118 }
DavidEGrayson 19:a11ffc903774 119
DavidEGrayson 20:dbec34f0e76b 120 float dotProduct()
DavidEGrayson 20:dbec34f0e76b 121 {
DavidEGrayson 20:dbec34f0e76b 122 float s = (float)reckoner.sin / (1 << 30);
DavidEGrayson 20:dbec34f0e76b 123 float c = (float)reckoner.cos / (1 << 30);
DavidEGrayson 20:dbec34f0e76b 124 float magn = magnitude();
DavidEGrayson 20:dbec34f0e76b 125 if (magn == 0){ return 0; }
DavidEGrayson 20:dbec34f0e76b 126 return ((float)reckoner.x * c + (float)reckoner.y * s) / magn;
DavidEGrayson 20:dbec34f0e76b 127 }
DavidEGrayson 20:dbec34f0e76b 128
DavidEGrayson 18:b65fbb795396 129 // The closer this is to zero, the closer we are to pointing towards the home position.
DavidEGrayson 18:b65fbb795396 130 // It is basically a cross product of the two vectors (x, y) and (cos, sin).
DavidEGrayson 19:a11ffc903774 131 float determinant()
DavidEGrayson 18:b65fbb795396 132 {
DavidEGrayson 18:b65fbb795396 133 // TODO: get rid of the magic numbers here (i.e. 30)
DavidEGrayson 18:b65fbb795396 134 float s = (float)reckoner.sin / (1 << 30);
DavidEGrayson 18:b65fbb795396 135 float c = (float)reckoner.cos / (1 << 30);
DavidEGrayson 19:a11ffc903774 136 return (reckoner.x * s - reckoner.y * c) / magnitude();
DavidEGrayson 19:a11ffc903774 137 }
DavidEGrayson 19:a11ffc903774 138
DavidEGrayson 21:c279c6a83671 139 int16_t reduceSpeed(int16_t speed, int32_t reduction)
DavidEGrayson 19:a11ffc903774 140 {
DavidEGrayson 19:a11ffc903774 141 if (reduction > speed)
DavidEGrayson 19:a11ffc903774 142 {
DavidEGrayson 19:a11ffc903774 143 return 0;
DavidEGrayson 19:a11ffc903774 144 }
DavidEGrayson 19:a11ffc903774 145 else
DavidEGrayson 19:a11ffc903774 146 {
DavidEGrayson 19:a11ffc903774 147 return speed - reduction;
DavidEGrayson 19:a11ffc903774 148 }
DavidEGrayson 18:b65fbb795396 149 }
DavidEGrayson 18:b65fbb795396 150
DavidEGrayson 21:c279c6a83671 151 void waitForSignalToStart()
DavidEGrayson 21:c279c6a83671 152 {
DavidEGrayson 21:c279c6a83671 153 while(!button1DefinitelyPressed())
DavidEGrayson 21:c279c6a83671 154 {
DavidEGrayson 21:c279c6a83671 155 updateReckonerFromEncoders();
DavidEGrayson 21:c279c6a83671 156 }
DavidEGrayson 21:c279c6a83671 157 reckoner.reset();
DavidEGrayson 21:c279c6a83671 158 }
DavidEGrayson 21:c279c6a83671 159
DavidEGrayson 24:fc01d9125d3b 160 // Keep the robot pointing the in the right direction (1, 0).
DavidEGrayson 24:fc01d9125d3b 161 // This should basically keep it going straight.
DavidEGrayson 24:fc01d9125d3b 162 void updateMotorsToDriveStraight()
DavidEGrayson 24:fc01d9125d3b 163 {
DavidEGrayson 24:fc01d9125d3b 164 const int32_t straightDriveStrength = 1000;
DavidEGrayson 24:fc01d9125d3b 165 int16_t speedLeft = drivingSpeed;
DavidEGrayson 24:fc01d9125d3b 166 int16_t speedRight = drivingSpeed;
DavidEGrayson 24:fc01d9125d3b 167 int32_t reduction = reckoner.sin / (1<<15) * straightDriveStrength / (1 << 15);
DavidEGrayson 24:fc01d9125d3b 168 if (reduction > 0)
DavidEGrayson 24:fc01d9125d3b 169 {
DavidEGrayson 24:fc01d9125d3b 170 speedRight = reduceSpeed(speedRight, reduction);
DavidEGrayson 24:fc01d9125d3b 171 }
DavidEGrayson 24:fc01d9125d3b 172 else
DavidEGrayson 24:fc01d9125d3b 173 {
DavidEGrayson 24:fc01d9125d3b 174 speedLeft = reduceSpeed(speedLeft, -reduction);
DavidEGrayson 24:fc01d9125d3b 175 }
DavidEGrayson 24:fc01d9125d3b 176 motorsSpeedSet(speedLeft, speedRight);
DavidEGrayson 24:fc01d9125d3b 177 }
DavidEGrayson 24:fc01d9125d3b 178
DavidEGrayson 28:4374035df5e0 179 void updateMotorsToFollowLine()
DavidEGrayson 24:fc01d9125d3b 180 {
DavidEGrayson 28:4374035df5e0 181 const int followLineStrength = 300;
DavidEGrayson 28:4374035df5e0 182
DavidEGrayson 28:4374035df5e0 183 int16_t speedLeft = drivingSpeed;
DavidEGrayson 28:4374035df5e0 184 int16_t speedRight = drivingSpeed;
DavidEGrayson 28:4374035df5e0 185 int16_t reduction = (lineTracker.getLinePosition() - 1000) * followLineStrength / 1000;
DavidEGrayson 28:4374035df5e0 186 if(reduction < 0)
DavidEGrayson 28:4374035df5e0 187 {
DavidEGrayson 28:4374035df5e0 188 speedLeft = reduceSpeed(speedLeft, -reduction);
DavidEGrayson 28:4374035df5e0 189 }
DavidEGrayson 28:4374035df5e0 190 else
DavidEGrayson 28:4374035df5e0 191 {
DavidEGrayson 28:4374035df5e0 192 speedRight = reduceSpeed(speedRight, reduction);
DavidEGrayson 28:4374035df5e0 193 }
DavidEGrayson 24:fc01d9125d3b 194
DavidEGrayson 28:4374035df5e0 195 motorsSpeedSet(speedLeft, speedRight);
DavidEGrayson 28:4374035df5e0 196 }
DavidEGrayson 28:4374035df5e0 197
DavidEGrayson 28:4374035df5e0 198 void findLine()
DavidEGrayson 28:4374035df5e0 199 {
DavidEGrayson 24:fc01d9125d3b 200 GeneralDebouncer lineStatus(10000);
DavidEGrayson 24:fc01d9125d3b 201 while(1)
DavidEGrayson 24:fc01d9125d3b 202 {
DavidEGrayson 24:fc01d9125d3b 203 lineTracker.read();
DavidEGrayson 24:fc01d9125d3b 204 lineTracker.updateCalibration();
DavidEGrayson 24:fc01d9125d3b 205 updateReckonerFromEncoders();
DavidEGrayson 24:fc01d9125d3b 206 updateMotorsToDriveStraight();
DavidEGrayson 24:fc01d9125d3b 207 lineStatus.update(lineTracker.getLineVisible());
DavidEGrayson 24:fc01d9125d3b 208
DavidEGrayson 26:7e7c376a7446 209 if(lineStatus.getState() == true && lineStatus.getTimeInCurrentStateMicroseconds() > 100000)
DavidEGrayson 24:fc01d9125d3b 210 {
DavidEGrayson 26:7e7c376a7446 211 break;
DavidEGrayson 24:fc01d9125d3b 212 }
DavidEGrayson 24:fc01d9125d3b 213 }
DavidEGrayson 24:fc01d9125d3b 214 }
DavidEGrayson 20:dbec34f0e76b 215
DavidEGrayson 21:c279c6a83671 216 void turnRightToFindLine()
DavidEGrayson 21:c279c6a83671 217 {
DavidEGrayson 21:c279c6a83671 218 while(1)
DavidEGrayson 21:c279c6a83671 219 {
DavidEGrayson 21:c279c6a83671 220 lineTracker.read();
DavidEGrayson 21:c279c6a83671 221 lineTracker.updateCalibration();
DavidEGrayson 21:c279c6a83671 222 updateReckonerFromEncoders();
DavidEGrayson 21:c279c6a83671 223
DavidEGrayson 21:c279c6a83671 224 if(lineTracker.getLineVisible())
DavidEGrayson 21:c279c6a83671 225 {
DavidEGrayson 21:c279c6a83671 226 break;
DavidEGrayson 21:c279c6a83671 227 }
DavidEGrayson 21:c279c6a83671 228
DavidEGrayson 21:c279c6a83671 229 motorsSpeedSet(300, 100);
DavidEGrayson 21:c279c6a83671 230 }
DavidEGrayson 21:c279c6a83671 231 }
DavidEGrayson 21:c279c6a83671 232
DavidEGrayson 21:c279c6a83671 233 void followLineToEnd()
DavidEGrayson 21:c279c6a83671 234 {
DavidEGrayson 26:7e7c376a7446 235 Timer timer;
DavidEGrayson 26:7e7c376a7446 236 timer.start();
DavidEGrayson 26:7e7c376a7446 237
DavidEGrayson 21:c279c6a83671 238 GeneralDebouncer lineStatus(10000);
DavidEGrayson 21:c279c6a83671 239 const uint32_t lineDebounceTime = 100000;
DavidEGrayson 21:c279c6a83671 240
DavidEGrayson 21:c279c6a83671 241 while(1)
DavidEGrayson 21:c279c6a83671 242 {
DavidEGrayson 21:c279c6a83671 243 lineTracker.read();
DavidEGrayson 21:c279c6a83671 244 updateReckonerFromEncoders();
DavidEGrayson 21:c279c6a83671 245
DavidEGrayson 21:c279c6a83671 246 lineStatus.update(lineTracker.getLineVisible());
DavidEGrayson 21:c279c6a83671 247
DavidEGrayson 21:c279c6a83671 248 bool lostLine = lineStatus.getState() == false &&
DavidEGrayson 26:7e7c376a7446 249 lineStatus.getTimeInCurrentStateMicroseconds() > lineDebounceTime;
DavidEGrayson 28:4374035df5e0 250 if(lostLine && timer.read_us() >= 2000000)
DavidEGrayson 21:c279c6a83671 251 {
DavidEGrayson 21:c279c6a83671 252 break;
DavidEGrayson 21:c279c6a83671 253 }
DavidEGrayson 21:c279c6a83671 254
DavidEGrayson 28:4374035df5e0 255 updateMotorsToFollowLine();
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 27:2456f68be679 303 const int16_t settleSpeed = 300;
DavidEGrayson 27:2456f68be679 304 const int16_t settleModificationStrength = 150;
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 27:2456f68be679 313 Pacer reportPacer(200000);
DavidEGrayson 27:2456f68be679 314 Pacer motorUpdatePacer(10000);
DavidEGrayson 21:c279c6a83671 315
DavidEGrayson 27:2456f68be679 316 float integral = 0;
DavidEGrayson 27:2456f68be679 317
DavidEGrayson 27:2456f68be679 318 motorsSpeedSet(-settleSpeed, settleSpeed); // avoid waiting 10 ms before we start settling
DavidEGrayson 27:2456f68be679 319
DavidEGrayson 19:a11ffc903774 320 while(1)
DavidEGrayson 19:a11ffc903774 321 {
DavidEGrayson 27:2456f68be679 322 led1 = (state == 1);
DavidEGrayson 27:2456f68be679 323
DavidEGrayson 20:dbec34f0e76b 324 updateReckonerFromEncoders();
DavidEGrayson 20:dbec34f0e76b 325
DavidEGrayson 20:dbec34f0e76b 326 float dot = dotProduct();
DavidEGrayson 20:dbec34f0e76b 327 int16_t speedModification = -dot * settleModificationStrength;
DavidEGrayson 20:dbec34f0e76b 328 if (speedModification > settleModificationStrength)
DavidEGrayson 20:dbec34f0e76b 329 {
DavidEGrayson 20:dbec34f0e76b 330 speedModification = settleModificationStrength;
DavidEGrayson 20:dbec34f0e76b 331 }
DavidEGrayson 20:dbec34f0e76b 332 else if (speedModification < -settleModificationStrength)
DavidEGrayson 20:dbec34f0e76b 333 {
DavidEGrayson 20:dbec34f0e76b 334 speedModification = -settleModificationStrength;
DavidEGrayson 20:dbec34f0e76b 335 }
DavidEGrayson 20:dbec34f0e76b 336
DavidEGrayson 21:c279c6a83671 337 if (state == 0 && timer.read_ms() >= 2000 && reckoner.cos > (1 << 29))
DavidEGrayson 19:a11ffc903774 338 {
DavidEGrayson 21:c279c6a83671 339 // Stop turning and start trying to maintain the right position.
DavidEGrayson 20:dbec34f0e76b 340 state = 1;
DavidEGrayson 20:dbec34f0e76b 341 }
DavidEGrayson 20:dbec34f0e76b 342
DavidEGrayson 21:c279c6a83671 343 if (state == 1 && timer.read_ms() >= 5000)
DavidEGrayson 21:c279c6a83671 344 {
DavidEGrayson 21:c279c6a83671 345 // Stop moving.
DavidEGrayson 21:c279c6a83671 346 break;
DavidEGrayson 21:c279c6a83671 347 }
DavidEGrayson 21:c279c6a83671 348
DavidEGrayson 27:2456f68be679 349 if (motorUpdatePacer.pace())
DavidEGrayson 20:dbec34f0e76b 350 {
DavidEGrayson 27:2456f68be679 351 int16_t rotationSpeed;
DavidEGrayson 27:2456f68be679 352 if (state == 1)
DavidEGrayson 27:2456f68be679 353 {
DavidEGrayson 27:2456f68be679 354 float s = (float)reckoner.sin / (1 << 30);
DavidEGrayson 27:2456f68be679 355 integral += s;
DavidEGrayson 27:2456f68be679 356 rotationSpeed = -(s * 2400 + integral * 20);
DavidEGrayson 27:2456f68be679 357
DavidEGrayson 27:2456f68be679 358 if (rotationSpeed > 300)
DavidEGrayson 27:2456f68be679 359 {
DavidEGrayson 27:2456f68be679 360 rotationSpeed = 300;
DavidEGrayson 27:2456f68be679 361 }
DavidEGrayson 27:2456f68be679 362 if (rotationSpeed < -300)
DavidEGrayson 27:2456f68be679 363 {
DavidEGrayson 27:2456f68be679 364 rotationSpeed = -300;
DavidEGrayson 27:2456f68be679 365 }
DavidEGrayson 27:2456f68be679 366 }
DavidEGrayson 27:2456f68be679 367 else
DavidEGrayson 27:2456f68be679 368 {
DavidEGrayson 27:2456f68be679 369 rotationSpeed = settleSpeed;
DavidEGrayson 27:2456f68be679 370 }
DavidEGrayson 27:2456f68be679 371
DavidEGrayson 27:2456f68be679 372 int16_t speedLeft = -rotationSpeed + speedModification;
DavidEGrayson 27:2456f68be679 373 int16_t speedRight = rotationSpeed + speedModification;
DavidEGrayson 27:2456f68be679 374 motorsSpeedSet(speedLeft, speedRight);
DavidEGrayson 20:dbec34f0e76b 375 }
DavidEGrayson 21:c279c6a83671 376
DavidEGrayson 21:c279c6a83671 377 if (state == 1 && reportPacer.pace())
DavidEGrayson 21:c279c6a83671 378 {
DavidEGrayson 21:c279c6a83671 379 pc.printf("%11d %11d %11d %11d | %11f %11f\r\n",
DavidEGrayson 21:c279c6a83671 380 reckoner.cos, reckoner.sin, reckoner.x, reckoner.y,
DavidEGrayson 21:c279c6a83671 381 determinant(), dotProduct());
DavidEGrayson 21:c279c6a83671 382 }
DavidEGrayson 19:a11ffc903774 383 }
DavidEGrayson 20:dbec34f0e76b 384
DavidEGrayson 21:c279c6a83671 385 // Done! Stop moving.
DavidEGrayson 20:dbec34f0e76b 386 motorsSpeedSet(0, 0);
DavidEGrayson 18:b65fbb795396 387 }