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@39:a5e25fd52ff8, 2015-04-14 (annotated)

Committer:
DavidEGrayson
Date:
Tue Apr 14 01:06:41 2015 +0000
Revision:
39:a5e25fd52ff8
Parent:
38:5e93a479c244
Child:
40:e79cefc241f8
Made it follow the line a little faster (600 out of 1200).  Need a derivative term though, because it oscillates back and forth on the straight part.

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 37:23000a47ed2b 18 Logger logger;
DavidEGrayson 37:23000a47ed2b 19 Pacer loggerPacer(50000);
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 34:6c84680d823a 44 //testDriveHome();
DavidEGrayson 21:c279c6a83671 45 //testFinalSettleIn();
DavidEGrayson 23:aae5cbe3b924 46 //testCalibrate();
DavidEGrayson 33:58a0ab6e9ad2 47 //testLineFollowing();
DavidEGrayson 29:cfcf08d8ac79 48 //testAnalog();
DavidEGrayson 31:739b91331f31 49 //testSensorGlitches();
DavidEGrayson 33:58a0ab6e9ad2 50 //testTurnInPlace();
DavidEGrayson 33:58a0ab6e9ad2 51 //testCloseness();
DavidEGrayson 37:23000a47ed2b 52 //testLogger();
DavidEGrayson 37:23000a47ed2b 53
DavidEGrayson 2:968338353aef 54
DavidEGrayson 21:c279c6a83671 55 // Real routines for the contest.
DavidEGrayson 28:4374035df5e0 56 loadCalibration();
DavidEGrayson 28:4374035df5e0 57
DavidEGrayson 21:c279c6a83671 58 setLeds(1, 0, 0, 0);
DavidEGrayson 21:c279c6a83671 59 waitForSignalToStart();
DavidEGrayson 39:a5e25fd52ff8 60
DavidEGrayson 33:58a0ab6e9ad2 61 setLeds(0, 1, 0, 0);
DavidEGrayson 39:a5e25fd52ff8 62 followLineFast();
DavidEGrayson 33:58a0ab6e9ad2 63
DavidEGrayson 21:c279c6a83671 64 setLeds(1, 1, 1, 1);
DavidEGrayson 37:23000a47ed2b 65 loggerReportLoop();
DavidEGrayson 37:23000a47ed2b 66 }
DavidEGrayson 37:23000a47ed2b 67
DavidEGrayson 37:23000a47ed2b 68 void loggerService()
DavidEGrayson 37:23000a47ed2b 69 {
DavidEGrayson 37:23000a47ed2b 70 if (loggerPacer.pace())
DavidEGrayson 37:23000a47ed2b 71 {
DavidEGrayson 37:23000a47ed2b 72 logger.log();
DavidEGrayson 37:23000a47ed2b 73 }
DavidEGrayson 0:e77a0edb9878 74 }
DavidEGrayson 12:835a4d24ae3b 75
DavidEGrayson 37:23000a47ed2b 76 void loggerReportLoop()
DavidEGrayson 37:23000a47ed2b 77 {
DavidEGrayson 37:23000a47ed2b 78 while(1)
DavidEGrayson 37:23000a47ed2b 79 {
DavidEGrayson 37:23000a47ed2b 80 if(button1DefinitelyPressed())
DavidEGrayson 37:23000a47ed2b 81 {
DavidEGrayson 37:23000a47ed2b 82 logger.dump();
DavidEGrayson 37:23000a47ed2b 83 }
DavidEGrayson 37:23000a47ed2b 84 }
DavidEGrayson 37:23000a47ed2b 85 }
DavidEGrayson 37:23000a47ed2b 86
DavidEGrayson 37:23000a47ed2b 87
DavidEGrayson 28:4374035df5e0 88 void loadCalibration()
DavidEGrayson 28:4374035df5e0 89 {
DavidEGrayson 32:83a13b06093c 90 /** QTR-3RC **/
DavidEGrayson 39:a5e25fd52ff8 91 lineTracker.calibratedMinimum[0] = 137;
DavidEGrayson 39:a5e25fd52ff8 92 lineTracker.calibratedMinimum[1] = 132;
DavidEGrayson 39:a5e25fd52ff8 93 lineTracker.calibratedMinimum[2] = 154;
DavidEGrayson 39:a5e25fd52ff8 94 lineTracker.calibratedMaximum[0] = 644;
DavidEGrayson 39:a5e25fd52ff8 95 lineTracker.calibratedMaximum[1] = 779;
DavidEGrayson 32:83a13b06093c 96 lineTracker.calibratedMaximum[2] = 1000;
DavidEGrayson 32:83a13b06093c 97
DavidEGrayson 32:83a13b06093c 98 /** QTR-3A
DavidEGrayson 28:4374035df5e0 99 lineTracker.calibratedMinimum[0] = 34872;
DavidEGrayson 28:4374035df5e0 100 lineTracker.calibratedMinimum[1] = 29335;
DavidEGrayson 28:4374035df5e0 101 lineTracker.calibratedMinimum[2] = 23845;
DavidEGrayson 28:4374035df5e0 102 lineTracker.calibratedMaximum[0] = 59726;
DavidEGrayson 28:4374035df5e0 103 lineTracker.calibratedMaximum[1] = 60110;
DavidEGrayson 32:83a13b06093c 104 lineTracker.calibratedMaximum[2] = 58446;
DavidEGrayson 32:83a13b06093c 105 **/
DavidEGrayson 28:4374035df5e0 106 }
DavidEGrayson 28:4374035df5e0 107
DavidEGrayson 12:835a4d24ae3b 108 void updateReckonerFromEncoders()
DavidEGrayson 12:835a4d24ae3b 109 {
DavidEGrayson 12:835a4d24ae3b 110 while(encoderBuffer.hasEvents())
DavidEGrayson 12:835a4d24ae3b 111 {
DavidEGrayson 12:835a4d24ae3b 112 PololuEncoderEvent event = encoderBuffer.readEvent();
DavidEGrayson 12:835a4d24ae3b 113 switch(event)
DavidEGrayson 12:835a4d24ae3b 114 {
DavidEGrayson 17:2df9861f53ee 115 case ENCODER_LEFT | POLOLU_ENCODER_EVENT_INC:
DavidEGrayson 17:2df9861f53ee 116 reckoner.handleTickLeftForward();
DavidEGrayson 17:2df9861f53ee 117 break;
DavidEGrayson 17:2df9861f53ee 118 case ENCODER_LEFT | POLOLU_ENCODER_EVENT_DEC:
DavidEGrayson 17:2df9861f53ee 119 reckoner.handleTickLeftBackward();
DavidEGrayson 17:2df9861f53ee 120 break;
DavidEGrayson 17:2df9861f53ee 121 case ENCODER_RIGHT | POLOLU_ENCODER_EVENT_INC:
DavidEGrayson 17:2df9861f53ee 122 reckoner.handleTickRightForward();
DavidEGrayson 17:2df9861f53ee 123 break;
DavidEGrayson 17:2df9861f53ee 124 case ENCODER_RIGHT | POLOLU_ENCODER_EVENT_DEC:
DavidEGrayson 17:2df9861f53ee 125 reckoner.handleTickRightBackward();
DavidEGrayson 17:2df9861f53ee 126 break;
DavidEGrayson 12:835a4d24ae3b 127 }
DavidEGrayson 12:835a4d24ae3b 128 }
DavidEGrayson 12:835a4d24ae3b 129 }
DavidEGrayson 17:2df9861f53ee 130
DavidEGrayson 19:a11ffc903774 131 float magnitude()
DavidEGrayson 19:a11ffc903774 132 {
DavidEGrayson 19:a11ffc903774 133 return sqrt((float)reckoner.x * reckoner.x + (float)reckoner.y * reckoner.y);
DavidEGrayson 19:a11ffc903774 134 }
DavidEGrayson 19:a11ffc903774 135
DavidEGrayson 20:dbec34f0e76b 136 float dotProduct()
DavidEGrayson 20:dbec34f0e76b 137 {
DavidEGrayson 20:dbec34f0e76b 138 float s = (float)reckoner.sin / (1 << 30);
DavidEGrayson 20:dbec34f0e76b 139 float c = (float)reckoner.cos / (1 << 30);
DavidEGrayson 20:dbec34f0e76b 140 float magn = magnitude();
DavidEGrayson 20:dbec34f0e76b 141 if (magn == 0){ return 0; }
DavidEGrayson 20:dbec34f0e76b 142 return ((float)reckoner.x * c + (float)reckoner.y * s) / magn;
DavidEGrayson 20:dbec34f0e76b 143 }
DavidEGrayson 20:dbec34f0e76b 144
DavidEGrayson 18:b65fbb795396 145 // The closer this is to zero, the closer we are to pointing towards the home position.
DavidEGrayson 18:b65fbb795396 146 // It is basically a cross product of the two vectors (x, y) and (cos, sin).
DavidEGrayson 19:a11ffc903774 147 float determinant()
DavidEGrayson 18:b65fbb795396 148 {
DavidEGrayson 18:b65fbb795396 149 // TODO: get rid of the magic numbers here (i.e. 30)
DavidEGrayson 18:b65fbb795396 150 float s = (float)reckoner.sin / (1 << 30);
DavidEGrayson 18:b65fbb795396 151 float c = (float)reckoner.cos / (1 << 30);
DavidEGrayson 19:a11ffc903774 152 return (reckoner.x * s - reckoner.y * c) / magnitude();
DavidEGrayson 19:a11ffc903774 153 }
DavidEGrayson 19:a11ffc903774 154
DavidEGrayson 21:c279c6a83671 155 int16_t reduceSpeed(int16_t speed, int32_t reduction)
DavidEGrayson 19:a11ffc903774 156 {
DavidEGrayson 19:a11ffc903774 157 if (reduction > speed)
DavidEGrayson 19:a11ffc903774 158 {
DavidEGrayson 19:a11ffc903774 159 return 0;
DavidEGrayson 19:a11ffc903774 160 }
DavidEGrayson 19:a11ffc903774 161 else
DavidEGrayson 19:a11ffc903774 162 {
DavidEGrayson 19:a11ffc903774 163 return speed - reduction;
DavidEGrayson 19:a11ffc903774 164 }
DavidEGrayson 18:b65fbb795396 165 }
DavidEGrayson 18:b65fbb795396 166
DavidEGrayson 21:c279c6a83671 167 void waitForSignalToStart()
DavidEGrayson 21:c279c6a83671 168 {
DavidEGrayson 21:c279c6a83671 169 while(!button1DefinitelyPressed())
DavidEGrayson 21:c279c6a83671 170 {
DavidEGrayson 21:c279c6a83671 171 updateReckonerFromEncoders();
DavidEGrayson 38:5e93a479c244 172 }
DavidEGrayson 21:c279c6a83671 173 reckoner.reset();
DavidEGrayson 38:5e93a479c244 174 while(button1DefinitelyPressed())
DavidEGrayson 38:5e93a479c244 175 {
DavidEGrayson 38:5e93a479c244 176 updateReckonerFromEncoders();
DavidEGrayson 38:5e93a479c244 177 }
DavidEGrayson 38:5e93a479c244 178 wait(0.2);
DavidEGrayson 21:c279c6a83671 179 }
DavidEGrayson 21:c279c6a83671 180
DavidEGrayson 28:4374035df5e0 181 void updateMotorsToFollowLine()
DavidEGrayson 24:fc01d9125d3b 182 {
DavidEGrayson 39:a5e25fd52ff8 183 const int16_t drivingSpeed = 400;
DavidEGrayson 34:6c84680d823a 184 const int followLineStrength = drivingSpeed * 5 / 4;
DavidEGrayson 28:4374035df5e0 185
DavidEGrayson 28:4374035df5e0 186 int16_t speedLeft = drivingSpeed;
DavidEGrayson 28:4374035df5e0 187 int16_t speedRight = drivingSpeed;
DavidEGrayson 28:4374035df5e0 188 int16_t reduction = (lineTracker.getLinePosition() - 1000) * followLineStrength / 1000;
DavidEGrayson 28:4374035df5e0 189 if(reduction < 0)
DavidEGrayson 28:4374035df5e0 190 {
DavidEGrayson 28:4374035df5e0 191 speedLeft = reduceSpeed(speedLeft, -reduction);
DavidEGrayson 28:4374035df5e0 192 }
DavidEGrayson 28:4374035df5e0 193 else
DavidEGrayson 28:4374035df5e0 194 {
DavidEGrayson 28:4374035df5e0 195 speedRight = reduceSpeed(speedRight, reduction);
DavidEGrayson 28:4374035df5e0 196 }
DavidEGrayson 24:fc01d9125d3b 197
DavidEGrayson 39:a5e25fd52ff8 198 motorsSpeedSet(speedLeft, speedRight);
DavidEGrayson 24:fc01d9125d3b 199 }
DavidEGrayson 20:dbec34f0e76b 200
DavidEGrayson 39:a5e25fd52ff8 201 void updateMotorsToFollowLineFast()
DavidEGrayson 21:c279c6a83671 202 {
DavidEGrayson 39:a5e25fd52ff8 203 const int16_t drivingSpeed = 600;
DavidEGrayson 39:a5e25fd52ff8 204 const int followLineStrength = drivingSpeed * 5 / 4;
DavidEGrayson 39:a5e25fd52ff8 205 static int16_t lastPosition = 0;
DavidEGrayson 39:a5e25fd52ff8 206
DavidEGrayson 39:a5e25fd52ff8 207 int16_t position = lineTracker.getLinePosition();
DavidEGrayson 21:c279c6a83671 208
DavidEGrayson 39:a5e25fd52ff8 209 int16_t speedLeft = drivingSpeed;
DavidEGrayson 39:a5e25fd52ff8 210 int16_t speedRight = drivingSpeed;
DavidEGrayson 39:a5e25fd52ff8 211 int16_t reduction = (position - 1000) * followLineStrength / 1000;
DavidEGrayson 39:a5e25fd52ff8 212 if(reduction < 0)
DavidEGrayson 39:a5e25fd52ff8 213 {
DavidEGrayson 39:a5e25fd52ff8 214 speedLeft = reduceSpeed(speedLeft, -reduction);
DavidEGrayson 21:c279c6a83671 215 }
DavidEGrayson 39:a5e25fd52ff8 216 else
DavidEGrayson 39:a5e25fd52ff8 217 {
DavidEGrayson 39:a5e25fd52ff8 218 speedRight = reduceSpeed(speedRight, reduction);
DavidEGrayson 39:a5e25fd52ff8 219 }
DavidEGrayson 39:a5e25fd52ff8 220
DavidEGrayson 39:a5e25fd52ff8 221 motorsSpeedSet(speedLeft, speedRight);
DavidEGrayson 39:a5e25fd52ff8 222
DavidEGrayson 39:a5e25fd52ff8 223 lastPosition = position;
DavidEGrayson 20:dbec34f0e76b 224 }
DavidEGrayson 20:dbec34f0e76b 225
DavidEGrayson 39:a5e25fd52ff8 226 void followLineFast()
DavidEGrayson 39:a5e25fd52ff8 227 {
DavidEGrayson 39:a5e25fd52ff8 228 Pacer reportPacer(200000);
DavidEGrayson 19:a11ffc903774 229
DavidEGrayson 39:a5e25fd52ff8 230 loadCalibration();
DavidEGrayson 19:a11ffc903774 231 while(1)
DavidEGrayson 18:b65fbb795396 232 {
DavidEGrayson 18:b65fbb795396 233 updateReckonerFromEncoders();
DavidEGrayson 37:23000a47ed2b 234 loggerService();
DavidEGrayson 37:23000a47ed2b 235
DavidEGrayson 39:a5e25fd52ff8 236 lineTracker.read();
DavidEGrayson 39:a5e25fd52ff8 237 updateMotorsToFollowLineFast();
DavidEGrayson 18:b65fbb795396 238 }
DavidEGrayson 20:dbec34f0e76b 239 }
DavidEGrayson 20:dbec34f0e76b 240