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@56:55b1473f9e3b, 2015-04-16 (annotated)

Committer:
DavidEGrayson
Date:
Thu Apr 16 21:02:38 2015 +0000
Revision:
56:55b1473f9e3b
Parent:
54:1ca4e748e098
Child:
57:99bec7fab454
Made the y tolerance in foundStart less strict because sometimes the wobble from the robot makes it hard to hit that target exactly.

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 44:edcacba44760 15 #include "l3g.h"
DavidEGrayson 44:edcacba44760 16 #include "turn_sensor.h"
DavidEGrayson 21:c279c6a83671 17
DavidEGrayson 21:c279c6a83671 18 Reckoner reckoner;
DavidEGrayson 21:c279c6a83671 19 LineTracker lineTracker;
DavidEGrayson 48:c84b7b3ab0e8 20 TurnSensor turnSensor;
DavidEGrayson 37:23000a47ed2b 21 Logger logger;
DavidEGrayson 37:23000a47ed2b 22 Pacer loggerPacer(50000);
DavidEGrayson 21:c279c6a83671 23
DavidEGrayson 52:05a8e919ddb0 24 uint8_t lapsCompleted = 0;
DavidEGrayson 48:c84b7b3ab0e8 25 uint32_t totalEncoderCounts = 0;
DavidEGrayson 48:c84b7b3ab0e8 26 uint32_t nextLogEncoderCount = 0;
DavidEGrayson 49:eaa6fd514f4f 27 const uint32_t logSpacing = 100;
DavidEGrayson 48:c84b7b3ab0e8 28
DavidEGrayson 21:c279c6a83671 29 void setLeds(bool v1, bool v2, bool v3, bool v4)
DavidEGrayson 21:c279c6a83671 30 {
DavidEGrayson 21:c279c6a83671 31 led1 = v1;
DavidEGrayson 21:c279c6a83671 32 led2 = v2;
DavidEGrayson 21:c279c6a83671 33 led3 = v3;
DavidEGrayson 21:c279c6a83671 34 led4 = v4;
DavidEGrayson 21:c279c6a83671 35 }
DavidEGrayson 0:e77a0edb9878 36
DavidEGrayson 10:e4dd36148539 37 int __attribute__((noreturn)) main()
DavidEGrayson 2:968338353aef 38 {
DavidEGrayson 2:968338353aef 39 pc.baud(115200);
DavidEGrayson 2:968338353aef 40
DavidEGrayson 2:968338353aef 41 // Enable pull-ups on encoder pins and give them a chance to settle.
DavidEGrayson 44:edcacba44760 42 if (l3gInit())
DavidEGrayson 44:edcacba44760 43 {
DavidEGrayson 44:edcacba44760 44 // Error initializing the gyro.
DavidEGrayson 44:edcacba44760 45 setLeds(0, 0, 1, 1);
DavidEGrayson 44:edcacba44760 46 while(1);
DavidEGrayson 44:edcacba44760 47 }
DavidEGrayson 44:edcacba44760 48
DavidEGrayson 9:9734347b5756 49 encodersInit();
DavidEGrayson 9:9734347b5756 50 motorsInit();
DavidEGrayson 16:8eaa5bc2bdb1 51 buttonsInit();
DavidEGrayson 4:1b20a11765c8 52
DavidEGrayson 8:78b1ff957cba 53 // Test routines
DavidEGrayson 9:9734347b5756 54 //testMotors();
DavidEGrayson 10:e4dd36148539 55 //testEncoders();
DavidEGrayson 32:83a13b06093c 56 //testLineSensors();
DavidEGrayson 16:8eaa5bc2bdb1 57 //testReckoner();
DavidEGrayson 17:2df9861f53ee 58 //testButtons();
DavidEGrayson 34:6c84680d823a 59 //testDriveHome();
DavidEGrayson 21:c279c6a83671 60 //testFinalSettleIn();
DavidEGrayson 23:aae5cbe3b924 61 //testCalibrate();
DavidEGrayson 33:58a0ab6e9ad2 62 //testLineFollowing();
DavidEGrayson 29:cfcf08d8ac79 63 //testAnalog();
DavidEGrayson 31:739b91331f31 64 //testSensorGlitches();
DavidEGrayson 33:58a0ab6e9ad2 65 //testTurnInPlace();
DavidEGrayson 33:58a0ab6e9ad2 66 //testCloseness();
DavidEGrayson 37:23000a47ed2b 67 //testLogger();
DavidEGrayson 45:e16e74bbbf8c 68 //testL3g();
DavidEGrayson 46:f11cb4f93aac 69 //testTurnSensor();
DavidEGrayson 46:f11cb4f93aac 70 //testReckoningWithGyro();
DavidEGrayson 2:968338353aef 71
DavidEGrayson 21:c279c6a83671 72 // Real routines for the contest.
DavidEGrayson 28:4374035df5e0 73 loadCalibration();
DavidEGrayson 28:4374035df5e0 74
DavidEGrayson 21:c279c6a83671 75 setLeds(1, 0, 0, 0);
DavidEGrayson 21:c279c6a83671 76 waitForSignalToStart();
DavidEGrayson 52:05a8e919ddb0 77
DavidEGrayson 52:05a8e919ddb0 78 setLeds(0, 1, 0, 0); // led3 and led4 get set by followLineSmart for debugging
DavidEGrayson 50:517c0f0e621f 79 //followLineFast();
DavidEGrayson 50:517c0f0e621f 80 followLineSmart();
DavidEGrayson 33:58a0ab6e9ad2 81
DavidEGrayson 21:c279c6a83671 82 setLeds(1, 1, 1, 1);
DavidEGrayson 37:23000a47ed2b 83 loggerReportLoop();
DavidEGrayson 37:23000a47ed2b 84 }
DavidEGrayson 37:23000a47ed2b 85
DavidEGrayson 37:23000a47ed2b 86 void loggerService()
DavidEGrayson 37:23000a47ed2b 87 {
DavidEGrayson 48:c84b7b3ab0e8 88 // loggerPacer.pace()
DavidEGrayson 48:c84b7b3ab0e8 89 if (totalEncoderCounts > nextLogEncoderCount)
DavidEGrayson 37:23000a47ed2b 90 {
DavidEGrayson 48:c84b7b3ab0e8 91 nextLogEncoderCount += logSpacing;
DavidEGrayson 48:c84b7b3ab0e8 92
DavidEGrayson 48:c84b7b3ab0e8 93 struct LogEntry entry;
DavidEGrayson 48:c84b7b3ab0e8 94 entry.turnAngle = turnSensor.getAngle() >> 16;
DavidEGrayson 48:c84b7b3ab0e8 95 entry.x = reckoner.x >> 16;
DavidEGrayson 48:c84b7b3ab0e8 96 entry.y = reckoner.y >> 16;
DavidEGrayson 48:c84b7b3ab0e8 97 logger.log(&entry);
DavidEGrayson 37:23000a47ed2b 98 }
DavidEGrayson 0:e77a0edb9878 99 }
DavidEGrayson 12:835a4d24ae3b 100
DavidEGrayson 37:23000a47ed2b 101 void loggerReportLoop()
DavidEGrayson 37:23000a47ed2b 102 {
DavidEGrayson 37:23000a47ed2b 103 while(1)
DavidEGrayson 37:23000a47ed2b 104 {
DavidEGrayson 37:23000a47ed2b 105 if(button1DefinitelyPressed())
DavidEGrayson 37:23000a47ed2b 106 {
DavidEGrayson 37:23000a47ed2b 107 logger.dump();
DavidEGrayson 37:23000a47ed2b 108 }
DavidEGrayson 37:23000a47ed2b 109 }
DavidEGrayson 37:23000a47ed2b 110 }
DavidEGrayson 37:23000a47ed2b 111
DavidEGrayson 37:23000a47ed2b 112
DavidEGrayson 28:4374035df5e0 113 void loadCalibration()
DavidEGrayson 28:4374035df5e0 114 {
DavidEGrayson 32:83a13b06093c 115 /** QTR-3RC **/
DavidEGrayson 39:a5e25fd52ff8 116 lineTracker.calibratedMinimum[0] = 137;
DavidEGrayson 39:a5e25fd52ff8 117 lineTracker.calibratedMinimum[1] = 132;
DavidEGrayson 39:a5e25fd52ff8 118 lineTracker.calibratedMinimum[2] = 154;
DavidEGrayson 39:a5e25fd52ff8 119 lineTracker.calibratedMaximum[0] = 644;
DavidEGrayson 39:a5e25fd52ff8 120 lineTracker.calibratedMaximum[1] = 779;
DavidEGrayson 32:83a13b06093c 121 lineTracker.calibratedMaximum[2] = 1000;
DavidEGrayson 32:83a13b06093c 122
DavidEGrayson 32:83a13b06093c 123 /** QTR-3A
DavidEGrayson 28:4374035df5e0 124 lineTracker.calibratedMinimum[0] = 34872;
DavidEGrayson 28:4374035df5e0 125 lineTracker.calibratedMinimum[1] = 29335;
DavidEGrayson 28:4374035df5e0 126 lineTracker.calibratedMinimum[2] = 23845;
DavidEGrayson 28:4374035df5e0 127 lineTracker.calibratedMaximum[0] = 59726;
DavidEGrayson 28:4374035df5e0 128 lineTracker.calibratedMaximum[1] = 60110;
DavidEGrayson 32:83a13b06093c 129 lineTracker.calibratedMaximum[2] = 58446;
DavidEGrayson 32:83a13b06093c 130 **/
DavidEGrayson 28:4374035df5e0 131 }
DavidEGrayson 28:4374035df5e0 132
DavidEGrayson 12:835a4d24ae3b 133 void updateReckonerFromEncoders()
DavidEGrayson 12:835a4d24ae3b 134 {
DavidEGrayson 12:835a4d24ae3b 135 while(encoderBuffer.hasEvents())
DavidEGrayson 12:835a4d24ae3b 136 {
DavidEGrayson 12:835a4d24ae3b 137 PololuEncoderEvent event = encoderBuffer.readEvent();
DavidEGrayson 12:835a4d24ae3b 138 switch(event)
DavidEGrayson 12:835a4d24ae3b 139 {
DavidEGrayson 17:2df9861f53ee 140 case ENCODER_LEFT | POLOLU_ENCODER_EVENT_INC:
DavidEGrayson 17:2df9861f53ee 141 reckoner.handleTickLeftForward();
DavidEGrayson 48:c84b7b3ab0e8 142 totalEncoderCounts++;
DavidEGrayson 17:2df9861f53ee 143 break;
DavidEGrayson 17:2df9861f53ee 144 case ENCODER_LEFT | POLOLU_ENCODER_EVENT_DEC:
DavidEGrayson 17:2df9861f53ee 145 reckoner.handleTickLeftBackward();
DavidEGrayson 48:c84b7b3ab0e8 146 totalEncoderCounts--;
DavidEGrayson 17:2df9861f53ee 147 break;
DavidEGrayson 17:2df9861f53ee 148 case ENCODER_RIGHT | POLOLU_ENCODER_EVENT_INC:
DavidEGrayson 17:2df9861f53ee 149 reckoner.handleTickRightForward();
DavidEGrayson 48:c84b7b3ab0e8 150 totalEncoderCounts++;
DavidEGrayson 17:2df9861f53ee 151 break;
DavidEGrayson 17:2df9861f53ee 152 case ENCODER_RIGHT | POLOLU_ENCODER_EVENT_DEC:
DavidEGrayson 17:2df9861f53ee 153 reckoner.handleTickRightBackward();
DavidEGrayson 48:c84b7b3ab0e8 154 totalEncoderCounts--;
DavidEGrayson 17:2df9861f53ee 155 break;
DavidEGrayson 12:835a4d24ae3b 156 }
DavidEGrayson 12:835a4d24ae3b 157 }
DavidEGrayson 12:835a4d24ae3b 158 }
DavidEGrayson 17:2df9861f53ee 159
DavidEGrayson 46:f11cb4f93aac 160 void updateReckoner(TurnSensor & turnSensor)
DavidEGrayson 46:f11cb4f93aac 161 {
DavidEGrayson 46:f11cb4f93aac 162 if (!encoderBuffer.hasEvents())
DavidEGrayson 46:f11cb4f93aac 163 {
DavidEGrayson 46:f11cb4f93aac 164 return;
DavidEGrayson 46:f11cb4f93aac 165 }
DavidEGrayson 46:f11cb4f93aac 166
DavidEGrayson 46:f11cb4f93aac 167 reckoner.setTurnAngle(turnSensor.getAngle());
DavidEGrayson 46:f11cb4f93aac 168
DavidEGrayson 46:f11cb4f93aac 169 while(encoderBuffer.hasEvents())
DavidEGrayson 46:f11cb4f93aac 170 {
DavidEGrayson 46:f11cb4f93aac 171 PololuEncoderEvent event = encoderBuffer.readEvent();
DavidEGrayson 46:f11cb4f93aac 172 switch(event)
DavidEGrayson 46:f11cb4f93aac 173 {
DavidEGrayson 46:f11cb4f93aac 174 case ENCODER_LEFT | POLOLU_ENCODER_EVENT_INC:
DavidEGrayson 46:f11cb4f93aac 175 case ENCODER_RIGHT | POLOLU_ENCODER_EVENT_INC:
DavidEGrayson 48:c84b7b3ab0e8 176 totalEncoderCounts++;
DavidEGrayson 46:f11cb4f93aac 177 reckoner.handleForward();
DavidEGrayson 46:f11cb4f93aac 178 break;
DavidEGrayson 46:f11cb4f93aac 179 case ENCODER_LEFT | POLOLU_ENCODER_EVENT_DEC:
DavidEGrayson 46:f11cb4f93aac 180 case ENCODER_RIGHT | POLOLU_ENCODER_EVENT_DEC:
DavidEGrayson 46:f11cb4f93aac 181 reckoner.handleBackward();
DavidEGrayson 48:c84b7b3ab0e8 182 totalEncoderCounts--;
DavidEGrayson 46:f11cb4f93aac 183 break;
DavidEGrayson 46:f11cb4f93aac 184 }
DavidEGrayson 46:f11cb4f93aac 185 }
DavidEGrayson 46:f11cb4f93aac 186 }
DavidEGrayson 46:f11cb4f93aac 187
DavidEGrayson 19:a11ffc903774 188 float magnitude()
DavidEGrayson 19:a11ffc903774 189 {
DavidEGrayson 19:a11ffc903774 190 return sqrt((float)reckoner.x * reckoner.x + (float)reckoner.y * reckoner.y);
DavidEGrayson 19:a11ffc903774 191 }
DavidEGrayson 19:a11ffc903774 192
DavidEGrayson 20:dbec34f0e76b 193 float dotProduct()
DavidEGrayson 20:dbec34f0e76b 194 {
DavidEGrayson 46:f11cb4f93aac 195 float s = (float)reckoner.sinv / (1 << 30);
DavidEGrayson 46:f11cb4f93aac 196 float c = (float)reckoner.cosv / (1 << 30);
DavidEGrayson 20:dbec34f0e76b 197 float magn = magnitude();
DavidEGrayson 20:dbec34f0e76b 198 if (magn == 0){ return 0; }
DavidEGrayson 20:dbec34f0e76b 199 return ((float)reckoner.x * c + (float)reckoner.y * s) / magn;
DavidEGrayson 20:dbec34f0e76b 200 }
DavidEGrayson 20:dbec34f0e76b 201
DavidEGrayson 18:b65fbb795396 202 // The closer this is to zero, the closer we are to pointing towards the home position.
DavidEGrayson 18:b65fbb795396 203 // It is basically a cross product of the two vectors (x, y) and (cos, sin).
DavidEGrayson 19:a11ffc903774 204 float determinant()
DavidEGrayson 18:b65fbb795396 205 {
DavidEGrayson 18:b65fbb795396 206 // TODO: get rid of the magic numbers here (i.e. 30)
DavidEGrayson 46:f11cb4f93aac 207 float s = (float)reckoner.sinv / (1 << 30);
DavidEGrayson 46:f11cb4f93aac 208 float c = (float)reckoner.cosv / (1 << 30);
DavidEGrayson 19:a11ffc903774 209 return (reckoner.x * s - reckoner.y * c) / magnitude();
DavidEGrayson 19:a11ffc903774 210 }
DavidEGrayson 19:a11ffc903774 211
DavidEGrayson 21:c279c6a83671 212 int16_t reduceSpeed(int16_t speed, int32_t reduction)
DavidEGrayson 19:a11ffc903774 213 {
DavidEGrayson 19:a11ffc903774 214 if (reduction > speed)
DavidEGrayson 19:a11ffc903774 215 {
DavidEGrayson 19:a11ffc903774 216 return 0;
DavidEGrayson 19:a11ffc903774 217 }
DavidEGrayson 19:a11ffc903774 218 else
DavidEGrayson 19:a11ffc903774 219 {
DavidEGrayson 19:a11ffc903774 220 return speed - reduction;
DavidEGrayson 19:a11ffc903774 221 }
DavidEGrayson 18:b65fbb795396 222 }
DavidEGrayson 18:b65fbb795396 223
DavidEGrayson 21:c279c6a83671 224 void waitForSignalToStart()
DavidEGrayson 21:c279c6a83671 225 {
DavidEGrayson 21:c279c6a83671 226 while(!button1DefinitelyPressed())
DavidEGrayson 21:c279c6a83671 227 {
DavidEGrayson 21:c279c6a83671 228 updateReckonerFromEncoders();
DavidEGrayson 38:5e93a479c244 229 }
DavidEGrayson 21:c279c6a83671 230 reckoner.reset();
DavidEGrayson 38:5e93a479c244 231 while(button1DefinitelyPressed())
DavidEGrayson 38:5e93a479c244 232 {
DavidEGrayson 38:5e93a479c244 233 updateReckonerFromEncoders();
DavidEGrayson 38:5e93a479c244 234 }
DavidEGrayson 38:5e93a479c244 235 wait(0.2);
DavidEGrayson 21:c279c6a83671 236 }
DavidEGrayson 21:c279c6a83671 237
DavidEGrayson 52:05a8e919ddb0 238 void updateMotorsToFollowLineSlow()
DavidEGrayson 24:fc01d9125d3b 239 {
DavidEGrayson 39:a5e25fd52ff8 240 const int16_t drivingSpeed = 400;
DavidEGrayson 40:e79cefc241f8 241 const int32_t followLineStrength = drivingSpeed * 5 / 4;
DavidEGrayson 28:4374035df5e0 242
DavidEGrayson 28:4374035df5e0 243 int16_t speedLeft = drivingSpeed;
DavidEGrayson 28:4374035df5e0 244 int16_t speedRight = drivingSpeed;
DavidEGrayson 28:4374035df5e0 245 int16_t reduction = (lineTracker.getLinePosition() - 1000) * followLineStrength / 1000;
DavidEGrayson 28:4374035df5e0 246 if(reduction < 0)
DavidEGrayson 28:4374035df5e0 247 {
DavidEGrayson 28:4374035df5e0 248 speedLeft = reduceSpeed(speedLeft, -reduction);
DavidEGrayson 28:4374035df5e0 249 }
DavidEGrayson 28:4374035df5e0 250 else
DavidEGrayson 28:4374035df5e0 251 {
DavidEGrayson 28:4374035df5e0 252 speedRight = reduceSpeed(speedRight, reduction);
DavidEGrayson 28:4374035df5e0 253 }
DavidEGrayson 24:fc01d9125d3b 254
DavidEGrayson 39:a5e25fd52ff8 255 motorsSpeedSet(speedLeft, speedRight);
DavidEGrayson 24:fc01d9125d3b 256 }
DavidEGrayson 20:dbec34f0e76b 257
DavidEGrayson 52:05a8e919ddb0 258 void updateMotorsToFollowLineFast(int16_t drivingSpeed)
DavidEGrayson 21:c279c6a83671 259 {
DavidEGrayson 40:e79cefc241f8 260 const int32_t followLineStrength = drivingSpeed * 5 / 4;
DavidEGrayson 40:e79cefc241f8 261 static int16_t lastPosition = 1000;
DavidEGrayson 39:a5e25fd52ff8 262
DavidEGrayson 39:a5e25fd52ff8 263 int16_t position = lineTracker.getLinePosition();
DavidEGrayson 21:c279c6a83671 264
DavidEGrayson 39:a5e25fd52ff8 265 int16_t speedLeft = drivingSpeed;
DavidEGrayson 39:a5e25fd52ff8 266 int16_t speedRight = drivingSpeed;
DavidEGrayson 40:e79cefc241f8 267 int32_t veer = (position - 1000) * followLineStrength / 1000 + (position - lastPosition) * 200;
DavidEGrayson 40:e79cefc241f8 268 if(veer > 0)
DavidEGrayson 39:a5e25fd52ff8 269 {
DavidEGrayson 40:e79cefc241f8 270 speedRight = reduceSpeed(speedRight, veer);
DavidEGrayson 21:c279c6a83671 271 }
DavidEGrayson 39:a5e25fd52ff8 272 else
DavidEGrayson 39:a5e25fd52ff8 273 {
DavidEGrayson 40:e79cefc241f8 274 speedLeft = reduceSpeed(speedLeft, -veer);
DavidEGrayson 39:a5e25fd52ff8 275 }
DavidEGrayson 39:a5e25fd52ff8 276
DavidEGrayson 39:a5e25fd52ff8 277 motorsSpeedSet(speedLeft, speedRight);
DavidEGrayson 39:a5e25fd52ff8 278
DavidEGrayson 39:a5e25fd52ff8 279 lastPosition = position;
DavidEGrayson 20:dbec34f0e76b 280 }
DavidEGrayson 20:dbec34f0e76b 281
DavidEGrayson 39:a5e25fd52ff8 282 void followLineFast()
DavidEGrayson 48:c84b7b3ab0e8 283 {
DavidEGrayson 48:c84b7b3ab0e8 284 totalEncoderCounts = 0;
DavidEGrayson 39:a5e25fd52ff8 285 Pacer reportPacer(200000);
DavidEGrayson 19:a11ffc903774 286
DavidEGrayson 39:a5e25fd52ff8 287 loadCalibration();
DavidEGrayson 40:e79cefc241f8 288 Timer timer;
DavidEGrayson 40:e79cefc241f8 289 timer.start();
DavidEGrayson 46:f11cb4f93aac 290 turnSensor.start();
DavidEGrayson 19:a11ffc903774 291 while(1)
DavidEGrayson 18:b65fbb795396 292 {
DavidEGrayson 46:f11cb4f93aac 293 turnSensor.update();
DavidEGrayson 46:f11cb4f93aac 294 updateReckoner(turnSensor);
DavidEGrayson 37:23000a47ed2b 295 loggerService();
DavidEGrayson 40:e79cefc241f8 296
DavidEGrayson 39:a5e25fd52ff8 297 lineTracker.read();
DavidEGrayson 52:05a8e919ddb0 298 updateMotorsToFollowLineFast(1000);
DavidEGrayson 44:edcacba44760 299
DavidEGrayson 44:edcacba44760 300 if (button1DefinitelyPressed())
DavidEGrayson 44:edcacba44760 301 {
DavidEGrayson 44:edcacba44760 302 break;
DavidEGrayson 44:edcacba44760 303 }
DavidEGrayson 18:b65fbb795396 304 }
DavidEGrayson 44:edcacba44760 305 motorsSpeedSet(0, 0);
DavidEGrayson 20:dbec34f0e76b 306 }
DavidEGrayson 20:dbec34f0e76b 307
DavidEGrayson 50:517c0f0e621f 308 bool foundStart()
DavidEGrayson 50:517c0f0e621f 309 {
DavidEGrayson 51:b9f7243609d4 310 static int32_t lastX = 0;
DavidEGrayson 56:55b1473f9e3b 311 bool result = lastX < 0 && reckoner.x >= 0 && abs(reckoner.y) < (115 << 16) &&
DavidEGrayson 50:517c0f0e621f 312 totalEncoderCounts > 10000 && abs(turnSensor.getAngle()) < turnAngle1 * 30;
DavidEGrayson 50:517c0f0e621f 313 lastX = reckoner.x;
DavidEGrayson 51:b9f7243609d4 314 return result;
DavidEGrayson 50:517c0f0e621f 315 }
DavidEGrayson 50:517c0f0e621f 316
DavidEGrayson 52:05a8e919ddb0 317 bool onLongStraightPart()
DavidEGrayson 52:05a8e919ddb0 318 {
DavidEGrayson 52:05a8e919ddb0 319 if (lapsCompleted == 0) { return false; }
DavidEGrayson 52:05a8e919ddb0 320
DavidEGrayson 52:05a8e919ddb0 321 // Figure out what part of the log corresponds to our current situation.
DavidEGrayson 52:05a8e919ddb0 322 uint32_t logIndex = totalEncoderCounts / logSpacing;
DavidEGrayson 52:05a8e919ddb0 323
DavidEGrayson 52:05a8e919ddb0 324 if (logIndex >= logger.getSize())
DavidEGrayson 52:05a8e919ddb0 325 {
DavidEGrayson 52:05a8e919ddb0 326 // Should not happen.
DavidEGrayson 52:05a8e919ddb0 327 return false;
DavidEGrayson 52:05a8e919ddb0 328 }
DavidEGrayson 52:05a8e919ddb0 329
DavidEGrayson 52:05a8e919ddb0 330 // To improve this, we could check that turnSensor.getAngle() matches what is in the log.
DavidEGrayson 52:05a8e919ddb0 331
DavidEGrayson 52:05a8e919ddb0 332 uint32_t angle1 = turnSensor.getAngleUnsigned();
DavidEGrayson 52:05a8e919ddb0 333
DavidEGrayson 54:1ca4e748e098 334 // 2000 encoder ticks
DavidEGrayson 54:1ca4e748e098 335 const uint32_t lookAheadAmount = 3000 / logSpacing;
DavidEGrayson 52:05a8e919ddb0 336
DavidEGrayson 52:05a8e919ddb0 337 // Figure out how far away the next turn is.
DavidEGrayson 52:05a8e919ddb0 338 uint32_t i = logIndex;
DavidEGrayson 52:05a8e919ddb0 339 while(1)
DavidEGrayson 52:05a8e919ddb0 340 {
DavidEGrayson 52:05a8e919ddb0 341 i++;
DavidEGrayson 52:05a8e919ddb0 342
DavidEGrayson 52:05a8e919ddb0 343 if (i >= logger.getSize())
DavidEGrayson 52:05a8e919ddb0 344 {
DavidEGrayson 52:05a8e919ddb0 345 // reached the end the log
DavidEGrayson 52:05a8e919ddb0 346 return false;
DavidEGrayson 52:05a8e919ddb0 347 }
DavidEGrayson 52:05a8e919ddb0 348
DavidEGrayson 52:05a8e919ddb0 349 if (i > logIndex + lookAheadAmount)
DavidEGrayson 52:05a8e919ddb0 350 {
DavidEGrayson 52:05a8e919ddb0 351 // looked far enough ahead that we don't think there is a turn coming up soon
DavidEGrayson 52:05a8e919ddb0 352 return true;
DavidEGrayson 52:05a8e919ddb0 353 }
DavidEGrayson 52:05a8e919ddb0 354
DavidEGrayson 52:05a8e919ddb0 355
DavidEGrayson 52:05a8e919ddb0 356 uint32_t angle2 = (uint16_t)logger.entries[i].turnAngle << 16;
DavidEGrayson 52:05a8e919ddb0 357 if (abs((int32_t)(angle2 - angle1)) > turnAngle45)
DavidEGrayson 52:05a8e919ddb0 358 {
DavidEGrayson 52:05a8e919ddb0 359 // detected a turn
DavidEGrayson 52:05a8e919ddb0 360 return false;
DavidEGrayson 52:05a8e919ddb0 361 }
DavidEGrayson 52:05a8e919ddb0 362 }
DavidEGrayson 52:05a8e919ddb0 363 }
DavidEGrayson 52:05a8e919ddb0 364
DavidEGrayson 50:517c0f0e621f 365 void followLineSmart()
DavidEGrayson 50:517c0f0e621f 366 {
DavidEGrayson 52:05a8e919ddb0 367 lapsCompleted = 0;
DavidEGrayson 50:517c0f0e621f 368 totalEncoderCounts = 0;
DavidEGrayson 52:05a8e919ddb0 369
DavidEGrayson 50:517c0f0e621f 370 Pacer reportPacer(200000);
DavidEGrayson 50:517c0f0e621f 371
DavidEGrayson 50:517c0f0e621f 372 loadCalibration();
DavidEGrayson 50:517c0f0e621f 373 turnSensor.start();
DavidEGrayson 50:517c0f0e621f 374 while(1)
DavidEGrayson 50:517c0f0e621f 375 {
DavidEGrayson 50:517c0f0e621f 376 turnSensor.update();
DavidEGrayson 50:517c0f0e621f 377 updateReckoner(turnSensor);
DavidEGrayson 50:517c0f0e621f 378 loggerService();
DavidEGrayson 50:517c0f0e621f 379
DavidEGrayson 50:517c0f0e621f 380 lineTracker.read();
DavidEGrayson 52:05a8e919ddb0 381
DavidEGrayson 52:05a8e919ddb0 382 // By default, choose a cautious speed of 1000 (out of 1200).
DavidEGrayson 52:05a8e919ddb0 383 int16_t speed = 1000;
DavidEGrayson 52:05a8e919ddb0 384
DavidEGrayson 52:05a8e919ddb0 385 // Go fast if we know we are on a long straight part.
DavidEGrayson 52:05a8e919ddb0 386 if (onLongStraightPart())
DavidEGrayson 52:05a8e919ddb0 387 {
DavidEGrayson 52:05a8e919ddb0 388 speed = 1200;
DavidEGrayson 53:73565a3ef5c5 389 led4 = 1;
DavidEGrayson 52:05a8e919ddb0 390 }
DavidEGrayson 52:05a8e919ddb0 391 else
DavidEGrayson 52:05a8e919ddb0 392 {
DavidEGrayson 53:73565a3ef5c5 393 led4 = 0;
DavidEGrayson 52:05a8e919ddb0 394 }
DavidEGrayson 52:05a8e919ddb0 395
DavidEGrayson 52:05a8e919ddb0 396
DavidEGrayson 52:05a8e919ddb0 397 updateMotorsToFollowLineFast(speed);
DavidEGrayson 50:517c0f0e621f 398
DavidEGrayson 50:517c0f0e621f 399 if (foundStart())
DavidEGrayson 50:517c0f0e621f 400 {
DavidEGrayson 52:05a8e919ddb0 401 // Another lap has been completed!
DavidEGrayson 53:73565a3ef5c5 402 lapsCompleted += 1;
DavidEGrayson 53:73565a3ef5c5 403 led3 = lapsCompleted & 1;
DavidEGrayson 52:05a8e919ddb0 404
DavidEGrayson 50:517c0f0e621f 405 reckoner.reset();
DavidEGrayson 50:517c0f0e621f 406 turnSensor.reset();
DavidEGrayson 50:517c0f0e621f 407 totalEncoderCounts = 0;
DavidEGrayson 50:517c0f0e621f 408 nextLogEncoderCount = 0;
DavidEGrayson 52:05a8e919ddb0 409 }
DavidEGrayson 52:05a8e919ddb0 410
DavidEGrayson 52:05a8e919ddb0 411 if (lapsCompleted == 3 && totalEncoderCounts > 2000)
DavidEGrayson 52:05a8e919ddb0 412 {
DavidEGrayson 52:05a8e919ddb0 413 // Classy move: know when you are done with the competition and stop automatically.
DavidEGrayson 52:05a8e919ddb0 414 // (Of course, there is a risk that this will backfire!)
DavidEGrayson 52:05a8e919ddb0 415 break;
DavidEGrayson 52:05a8e919ddb0 416 }
DavidEGrayson 50:517c0f0e621f 417
DavidEGrayson 50:517c0f0e621f 418 if (button1DefinitelyPressed())
DavidEGrayson 50:517c0f0e621f 419 {
DavidEGrayson 50:517c0f0e621f 420 break;
DavidEGrayson 50:517c0f0e621f 421 }
DavidEGrayson 50:517c0f0e621f 422 }
DavidEGrayson 50:517c0f0e621f 423 motorsSpeedSet(0, 0);
DavidEGrayson 50:517c0f0e621f 424 }
DavidEGrayson 50:517c0f0e621f 425