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

Committer:
DavidEGrayson
Date:
Wed Apr 15 23:01:39 2015 +0000
Revision:
51:b9f7243609d4
Parent:
50:517c0f0e621f
Child:
52:05a8e919ddb0
got it to detect the starting point better;

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