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@57:99bec7fab454, 2015-04-16 (annotated)

Committer:
DavidEGrayson
Date:
Thu Apr 16 22:00:15 2015 +0000
Revision:
57:99bec7fab454
Parent:
56:55b1473f9e3b 
Doubled the encoder counts for indicating the end of the course because we might have to start a little bit back from the finish 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 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 57:99bec7fab454 411 if (lapsCompleted == 3 && totalEncoderCounts > 4000)
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