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@46:f11cb4f93aac, 2015-04-15 (annotated)

Committer:
DavidEGrayson
Date:
Wed Apr 15 21:19:22 2015 +0000
Revision:
46:f11cb4f93aac
Parent:
45:e16e74bbbf8c
Child:
48:c84b7b3ab0e8
got dead reckoning working nicely with the gyro;

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 37:23000a47ed2b 20 Logger logger;
DavidEGrayson 37:23000a47ed2b 21 Pacer loggerPacer(50000);
DavidEGrayson 21:c279c6a83671 22
DavidEGrayson 21:c279c6a83671 23 void setLeds(bool v1, bool v2, bool v3, bool v4)
DavidEGrayson 21:c279c6a83671 24 {
DavidEGrayson 21:c279c6a83671 25 led1 = v1;
DavidEGrayson 21:c279c6a83671 26 led2 = v2;
DavidEGrayson 21:c279c6a83671 27 led3 = v3;
DavidEGrayson 21:c279c6a83671 28 led4 = v4;
DavidEGrayson 21:c279c6a83671 29 }
DavidEGrayson 0:e77a0edb9878 30
DavidEGrayson 10:e4dd36148539 31 int __attribute__((noreturn)) main()
DavidEGrayson 2:968338353aef 32 {
DavidEGrayson 2:968338353aef 33 pc.baud(115200);
DavidEGrayson 2:968338353aef 34
DavidEGrayson 2:968338353aef 35 // Enable pull-ups on encoder pins and give them a chance to settle.
DavidEGrayson 44:edcacba44760 36 if (l3gInit())
DavidEGrayson 44:edcacba44760 37 {
DavidEGrayson 44:edcacba44760 38 // Error initializing the gyro.
DavidEGrayson 44:edcacba44760 39 setLeds(0, 0, 1, 1);
DavidEGrayson 44:edcacba44760 40 while(1);
DavidEGrayson 44:edcacba44760 41 }
DavidEGrayson 44:edcacba44760 42
DavidEGrayson 9:9734347b5756 43 encodersInit();
DavidEGrayson 9:9734347b5756 44 motorsInit();
DavidEGrayson 16:8eaa5bc2bdb1 45 buttonsInit();
DavidEGrayson 4:1b20a11765c8 46
DavidEGrayson 8:78b1ff957cba 47 // Test routines
DavidEGrayson 9:9734347b5756 48 //testMotors();
DavidEGrayson 10:e4dd36148539 49 //testEncoders();
DavidEGrayson 32:83a13b06093c 50 //testLineSensors();
DavidEGrayson 16:8eaa5bc2bdb1 51 //testReckoner();
DavidEGrayson 17:2df9861f53ee 52 //testButtons();
DavidEGrayson 34:6c84680d823a 53 //testDriveHome();
DavidEGrayson 21:c279c6a83671 54 //testFinalSettleIn();
DavidEGrayson 23:aae5cbe3b924 55 //testCalibrate();
DavidEGrayson 33:58a0ab6e9ad2 56 //testLineFollowing();
DavidEGrayson 29:cfcf08d8ac79 57 //testAnalog();
DavidEGrayson 31:739b91331f31 58 //testSensorGlitches();
DavidEGrayson 33:58a0ab6e9ad2 59 //testTurnInPlace();
DavidEGrayson 33:58a0ab6e9ad2 60 //testCloseness();
DavidEGrayson 37:23000a47ed2b 61 //testLogger();
DavidEGrayson 45:e16e74bbbf8c 62 //testL3g();
DavidEGrayson 46:f11cb4f93aac 63 //testTurnSensor();
DavidEGrayson 46:f11cb4f93aac 64 //testReckoningWithGyro();
DavidEGrayson 2:968338353aef 65
DavidEGrayson 21:c279c6a83671 66 // Real routines for the contest.
DavidEGrayson 28:4374035df5e0 67 loadCalibration();
DavidEGrayson 28:4374035df5e0 68
DavidEGrayson 21:c279c6a83671 69 setLeds(1, 0, 0, 0);
DavidEGrayson 21:c279c6a83671 70 waitForSignalToStart();
DavidEGrayson 39:a5e25fd52ff8 71
DavidEGrayson 40:e79cefc241f8 72 setLeds(0, 1, 0, 0);
DavidEGrayson 39:a5e25fd52ff8 73 followLineFast();
DavidEGrayson 33:58a0ab6e9ad2 74
DavidEGrayson 21:c279c6a83671 75 setLeds(1, 1, 1, 1);
DavidEGrayson 37:23000a47ed2b 76 loggerReportLoop();
DavidEGrayson 37:23000a47ed2b 77 }
DavidEGrayson 37:23000a47ed2b 78
DavidEGrayson 37:23000a47ed2b 79 void loggerService()
DavidEGrayson 37:23000a47ed2b 80 {
DavidEGrayson 37:23000a47ed2b 81 if (loggerPacer.pace())
DavidEGrayson 37:23000a47ed2b 82 {
DavidEGrayson 37:23000a47ed2b 83 logger.log();
DavidEGrayson 37:23000a47ed2b 84 }
DavidEGrayson 0:e77a0edb9878 85 }
DavidEGrayson 12:835a4d24ae3b 86
DavidEGrayson 37:23000a47ed2b 87 void loggerReportLoop()
DavidEGrayson 37:23000a47ed2b 88 {
DavidEGrayson 37:23000a47ed2b 89 while(1)
DavidEGrayson 37:23000a47ed2b 90 {
DavidEGrayson 37:23000a47ed2b 91 if(button1DefinitelyPressed())
DavidEGrayson 37:23000a47ed2b 92 {
DavidEGrayson 37:23000a47ed2b 93 logger.dump();
DavidEGrayson 37:23000a47ed2b 94 }
DavidEGrayson 37:23000a47ed2b 95 }
DavidEGrayson 37:23000a47ed2b 96 }
DavidEGrayson 37:23000a47ed2b 97
DavidEGrayson 37:23000a47ed2b 98
DavidEGrayson 28:4374035df5e0 99 void loadCalibration()
DavidEGrayson 28:4374035df5e0 100 {
DavidEGrayson 32:83a13b06093c 101 /** QTR-3RC **/
DavidEGrayson 39:a5e25fd52ff8 102 lineTracker.calibratedMinimum[0] = 137;
DavidEGrayson 39:a5e25fd52ff8 103 lineTracker.calibratedMinimum[1] = 132;
DavidEGrayson 39:a5e25fd52ff8 104 lineTracker.calibratedMinimum[2] = 154;
DavidEGrayson 39:a5e25fd52ff8 105 lineTracker.calibratedMaximum[0] = 644;
DavidEGrayson 39:a5e25fd52ff8 106 lineTracker.calibratedMaximum[1] = 779;
DavidEGrayson 32:83a13b06093c 107 lineTracker.calibratedMaximum[2] = 1000;
DavidEGrayson 32:83a13b06093c 108
DavidEGrayson 32:83a13b06093c 109 /** QTR-3A
DavidEGrayson 28:4374035df5e0 110 lineTracker.calibratedMinimum[0] = 34872;
DavidEGrayson 28:4374035df5e0 111 lineTracker.calibratedMinimum[1] = 29335;
DavidEGrayson 28:4374035df5e0 112 lineTracker.calibratedMinimum[2] = 23845;
DavidEGrayson 28:4374035df5e0 113 lineTracker.calibratedMaximum[0] = 59726;
DavidEGrayson 28:4374035df5e0 114 lineTracker.calibratedMaximum[1] = 60110;
DavidEGrayson 32:83a13b06093c 115 lineTracker.calibratedMaximum[2] = 58446;
DavidEGrayson 32:83a13b06093c 116 **/
DavidEGrayson 28:4374035df5e0 117 }
DavidEGrayson 28:4374035df5e0 118
DavidEGrayson 12:835a4d24ae3b 119 void updateReckonerFromEncoders()
DavidEGrayson 12:835a4d24ae3b 120 {
DavidEGrayson 12:835a4d24ae3b 121 while(encoderBuffer.hasEvents())
DavidEGrayson 12:835a4d24ae3b 122 {
DavidEGrayson 12:835a4d24ae3b 123 PololuEncoderEvent event = encoderBuffer.readEvent();
DavidEGrayson 12:835a4d24ae3b 124 switch(event)
DavidEGrayson 12:835a4d24ae3b 125 {
DavidEGrayson 17:2df9861f53ee 126 case ENCODER_LEFT | POLOLU_ENCODER_EVENT_INC:
DavidEGrayson 17:2df9861f53ee 127 reckoner.handleTickLeftForward();
DavidEGrayson 17:2df9861f53ee 128 break;
DavidEGrayson 17:2df9861f53ee 129 case ENCODER_LEFT | POLOLU_ENCODER_EVENT_DEC:
DavidEGrayson 17:2df9861f53ee 130 reckoner.handleTickLeftBackward();
DavidEGrayson 17:2df9861f53ee 131 break;
DavidEGrayson 17:2df9861f53ee 132 case ENCODER_RIGHT | POLOLU_ENCODER_EVENT_INC:
DavidEGrayson 17:2df9861f53ee 133 reckoner.handleTickRightForward();
DavidEGrayson 17:2df9861f53ee 134 break;
DavidEGrayson 17:2df9861f53ee 135 case ENCODER_RIGHT | POLOLU_ENCODER_EVENT_DEC:
DavidEGrayson 17:2df9861f53ee 136 reckoner.handleTickRightBackward();
DavidEGrayson 17:2df9861f53ee 137 break;
DavidEGrayson 12:835a4d24ae3b 138 }
DavidEGrayson 12:835a4d24ae3b 139 }
DavidEGrayson 12:835a4d24ae3b 140 }
DavidEGrayson 17:2df9861f53ee 141
DavidEGrayson 46:f11cb4f93aac 142 void updateReckoner(TurnSensor & turnSensor)
DavidEGrayson 46:f11cb4f93aac 143 {
DavidEGrayson 46:f11cb4f93aac 144 if (!encoderBuffer.hasEvents())
DavidEGrayson 46:f11cb4f93aac 145 {
DavidEGrayson 46:f11cb4f93aac 146 return;
DavidEGrayson 46:f11cb4f93aac 147 }
DavidEGrayson 46:f11cb4f93aac 148
DavidEGrayson 46:f11cb4f93aac 149 reckoner.setTurnAngle(turnSensor.getAngle());
DavidEGrayson 46:f11cb4f93aac 150
DavidEGrayson 46:f11cb4f93aac 151 while(encoderBuffer.hasEvents())
DavidEGrayson 46:f11cb4f93aac 152 {
DavidEGrayson 46:f11cb4f93aac 153 PololuEncoderEvent event = encoderBuffer.readEvent();
DavidEGrayson 46:f11cb4f93aac 154 switch(event)
DavidEGrayson 46:f11cb4f93aac 155 {
DavidEGrayson 46:f11cb4f93aac 156 case ENCODER_LEFT | POLOLU_ENCODER_EVENT_INC:
DavidEGrayson 46:f11cb4f93aac 157 case ENCODER_RIGHT | POLOLU_ENCODER_EVENT_INC:
DavidEGrayson 46:f11cb4f93aac 158 reckoner.handleForward();
DavidEGrayson 46:f11cb4f93aac 159 break;
DavidEGrayson 46:f11cb4f93aac 160 case ENCODER_LEFT | POLOLU_ENCODER_EVENT_DEC:
DavidEGrayson 46:f11cb4f93aac 161 case ENCODER_RIGHT | POLOLU_ENCODER_EVENT_DEC:
DavidEGrayson 46:f11cb4f93aac 162 reckoner.handleBackward();
DavidEGrayson 46:f11cb4f93aac 163 break;
DavidEGrayson 46:f11cb4f93aac 164 }
DavidEGrayson 46:f11cb4f93aac 165 }
DavidEGrayson 46:f11cb4f93aac 166 }
DavidEGrayson 46:f11cb4f93aac 167
DavidEGrayson 19:a11ffc903774 168 float magnitude()
DavidEGrayson 19:a11ffc903774 169 {
DavidEGrayson 19:a11ffc903774 170 return sqrt((float)reckoner.x * reckoner.x + (float)reckoner.y * reckoner.y);
DavidEGrayson 19:a11ffc903774 171 }
DavidEGrayson 19:a11ffc903774 172
DavidEGrayson 20:dbec34f0e76b 173 float dotProduct()
DavidEGrayson 20:dbec34f0e76b 174 {
DavidEGrayson 46:f11cb4f93aac 175 float s = (float)reckoner.sinv / (1 << 30);
DavidEGrayson 46:f11cb4f93aac 176 float c = (float)reckoner.cosv / (1 << 30);
DavidEGrayson 20:dbec34f0e76b 177 float magn = magnitude();
DavidEGrayson 20:dbec34f0e76b 178 if (magn == 0){ return 0; }
DavidEGrayson 20:dbec34f0e76b 179 return ((float)reckoner.x * c + (float)reckoner.y * s) / magn;
DavidEGrayson 20:dbec34f0e76b 180 }
DavidEGrayson 20:dbec34f0e76b 181
DavidEGrayson 18:b65fbb795396 182 // The closer this is to zero, the closer we are to pointing towards the home position.
DavidEGrayson 18:b65fbb795396 183 // It is basically a cross product of the two vectors (x, y) and (cos, sin).
DavidEGrayson 19:a11ffc903774 184 float determinant()
DavidEGrayson 18:b65fbb795396 185 {
DavidEGrayson 18:b65fbb795396 186 // TODO: get rid of the magic numbers here (i.e. 30)
DavidEGrayson 46:f11cb4f93aac 187 float s = (float)reckoner.sinv / (1 << 30);
DavidEGrayson 46:f11cb4f93aac 188 float c = (float)reckoner.cosv / (1 << 30);
DavidEGrayson 19:a11ffc903774 189 return (reckoner.x * s - reckoner.y * c) / magnitude();
DavidEGrayson 19:a11ffc903774 190 }
DavidEGrayson 19:a11ffc903774 191
DavidEGrayson 21:c279c6a83671 192 int16_t reduceSpeed(int16_t speed, int32_t reduction)
DavidEGrayson 19:a11ffc903774 193 {
DavidEGrayson 19:a11ffc903774 194 if (reduction > speed)
DavidEGrayson 19:a11ffc903774 195 {
DavidEGrayson 19:a11ffc903774 196 return 0;
DavidEGrayson 19:a11ffc903774 197 }
DavidEGrayson 19:a11ffc903774 198 else
DavidEGrayson 19:a11ffc903774 199 {
DavidEGrayson 19:a11ffc903774 200 return speed - reduction;
DavidEGrayson 19:a11ffc903774 201 }
DavidEGrayson 18:b65fbb795396 202 }
DavidEGrayson 18:b65fbb795396 203
DavidEGrayson 21:c279c6a83671 204 void waitForSignalToStart()
DavidEGrayson 21:c279c6a83671 205 {
DavidEGrayson 21:c279c6a83671 206 while(!button1DefinitelyPressed())
DavidEGrayson 21:c279c6a83671 207 {
DavidEGrayson 21:c279c6a83671 208 updateReckonerFromEncoders();
DavidEGrayson 38:5e93a479c244 209 }
DavidEGrayson 21:c279c6a83671 210 reckoner.reset();
DavidEGrayson 38:5e93a479c244 211 while(button1DefinitelyPressed())
DavidEGrayson 38:5e93a479c244 212 {
DavidEGrayson 38:5e93a479c244 213 updateReckonerFromEncoders();
DavidEGrayson 38:5e93a479c244 214 }
DavidEGrayson 38:5e93a479c244 215 wait(0.2);
DavidEGrayson 21:c279c6a83671 216 }
DavidEGrayson 21:c279c6a83671 217
DavidEGrayson 28:4374035df5e0 218 void updateMotorsToFollowLine()
DavidEGrayson 24:fc01d9125d3b 219 {
DavidEGrayson 39:a5e25fd52ff8 220 const int16_t drivingSpeed = 400;
DavidEGrayson 40:e79cefc241f8 221 const int32_t followLineStrength = drivingSpeed * 5 / 4;
DavidEGrayson 28:4374035df5e0 222
DavidEGrayson 28:4374035df5e0 223 int16_t speedLeft = drivingSpeed;
DavidEGrayson 28:4374035df5e0 224 int16_t speedRight = drivingSpeed;
DavidEGrayson 28:4374035df5e0 225 int16_t reduction = (lineTracker.getLinePosition() - 1000) * followLineStrength / 1000;
DavidEGrayson 28:4374035df5e0 226 if(reduction < 0)
DavidEGrayson 28:4374035df5e0 227 {
DavidEGrayson 28:4374035df5e0 228 speedLeft = reduceSpeed(speedLeft, -reduction);
DavidEGrayson 28:4374035df5e0 229 }
DavidEGrayson 28:4374035df5e0 230 else
DavidEGrayson 28:4374035df5e0 231 {
DavidEGrayson 28:4374035df5e0 232 speedRight = reduceSpeed(speedRight, reduction);
DavidEGrayson 28:4374035df5e0 233 }
DavidEGrayson 24:fc01d9125d3b 234
DavidEGrayson 39:a5e25fd52ff8 235 motorsSpeedSet(speedLeft, speedRight);
DavidEGrayson 24:fc01d9125d3b 236 }
DavidEGrayson 20:dbec34f0e76b 237
DavidEGrayson 39:a5e25fd52ff8 238 void updateMotorsToFollowLineFast()
DavidEGrayson 21:c279c6a83671 239 {
DavidEGrayson 43:6cb32548c1b4 240 const int16_t drivingSpeed = 1100;
DavidEGrayson 40:e79cefc241f8 241 const int32_t followLineStrength = drivingSpeed * 5 / 4;
DavidEGrayson 40:e79cefc241f8 242 static int16_t lastPosition = 1000;
DavidEGrayson 39:a5e25fd52ff8 243
DavidEGrayson 39:a5e25fd52ff8 244 int16_t position = lineTracker.getLinePosition();
DavidEGrayson 21:c279c6a83671 245
DavidEGrayson 39:a5e25fd52ff8 246 int16_t speedLeft = drivingSpeed;
DavidEGrayson 39:a5e25fd52ff8 247 int16_t speedRight = drivingSpeed;
DavidEGrayson 40:e79cefc241f8 248 int32_t veer = (position - 1000) * followLineStrength / 1000 + (position - lastPosition) * 200;
DavidEGrayson 40:e79cefc241f8 249 if(veer > 0)
DavidEGrayson 39:a5e25fd52ff8 250 {
DavidEGrayson 40:e79cefc241f8 251 speedRight = reduceSpeed(speedRight, veer);
DavidEGrayson 21:c279c6a83671 252 }
DavidEGrayson 39:a5e25fd52ff8 253 else
DavidEGrayson 39:a5e25fd52ff8 254 {
DavidEGrayson 40:e79cefc241f8 255 speedLeft = reduceSpeed(speedLeft, -veer);
DavidEGrayson 39:a5e25fd52ff8 256 }
DavidEGrayson 39:a5e25fd52ff8 257
DavidEGrayson 39:a5e25fd52ff8 258 motorsSpeedSet(speedLeft, speedRight);
DavidEGrayson 39:a5e25fd52ff8 259
DavidEGrayson 39:a5e25fd52ff8 260 lastPosition = position;
DavidEGrayson 20:dbec34f0e76b 261 }
DavidEGrayson 20:dbec34f0e76b 262
DavidEGrayson 39:a5e25fd52ff8 263 void followLineFast()
DavidEGrayson 39:a5e25fd52ff8 264 {
DavidEGrayson 39:a5e25fd52ff8 265 Pacer reportPacer(200000);
DavidEGrayson 19:a11ffc903774 266
DavidEGrayson 39:a5e25fd52ff8 267 loadCalibration();
DavidEGrayson 40:e79cefc241f8 268 uint32_t loopCount = 0;
DavidEGrayson 40:e79cefc241f8 269 Timer timer;
DavidEGrayson 40:e79cefc241f8 270 timer.start();
DavidEGrayson 46:f11cb4f93aac 271 TurnSensor turnSensor;
DavidEGrayson 46:f11cb4f93aac 272 turnSensor.start();
DavidEGrayson 19:a11ffc903774 273 while(1)
DavidEGrayson 18:b65fbb795396 274 {
DavidEGrayson 40:e79cefc241f8 275 loopCount += 1;
DavidEGrayson 46:f11cb4f93aac 276 turnSensor.update();
DavidEGrayson 46:f11cb4f93aac 277 updateReckoner(turnSensor);
DavidEGrayson 37:23000a47ed2b 278 loggerService();
DavidEGrayson 40:e79cefc241f8 279
DavidEGrayson 40:e79cefc241f8 280 if ((loopCount % 256) == 0)
DavidEGrayson 40:e79cefc241f8 281 {
DavidEGrayson 40:e79cefc241f8 282 pc.printf("%d\r\n", lineTracker.getLinePosition());
DavidEGrayson 40:e79cefc241f8 283 }
DavidEGrayson 40:e79cefc241f8 284
DavidEGrayson 39:a5e25fd52ff8 285 lineTracker.read();
DavidEGrayson 44:edcacba44760 286 updateMotorsToFollowLineFast();
DavidEGrayson 44:edcacba44760 287
DavidEGrayson 44:edcacba44760 288 if (button1DefinitelyPressed())
DavidEGrayson 44:edcacba44760 289 {
DavidEGrayson 44:edcacba44760 290 break;
DavidEGrayson 44:edcacba44760 291 }
DavidEGrayson 18:b65fbb795396 292 }
DavidEGrayson 44:edcacba44760 293 motorsSpeedSet(0, 0);
DavidEGrayson 20:dbec34f0e76b 294 }
DavidEGrayson 20:dbec34f0e76b 295