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

Committer:
DavidEGrayson
Date:
Wed Apr 15 19:14:39 2015 +0000
Revision:
44:edcacba44760
Parent:
43:6cb32548c1b4
Child:
45:e16e74bbbf8c
Got the L3GD20H working, but I'm having trouble with TurnSensor.

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