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

Committer:
DavidEGrayson
Date:
Wed Apr 15 21:42:52 2015 +0000
Revision:
48:c84b7b3ab0e8
Parent:
46:f11cb4f93aac
Child:
49:eaa6fd514f4f
made it record entries every 200 encoder counts, instead of every 200 ms;

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 48:c84b7b3ab0e8 26 const uint32_t logSpacing = 200;
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 40:e79cefc241f8 77 setLeds(0, 1, 0, 0);
DavidEGrayson 39:a5e25fd52ff8 78 followLineFast();
DavidEGrayson 33:58a0ab6e9ad2 79
DavidEGrayson 21:c279c6a83671 80 setLeds(1, 1, 1, 1);
DavidEGrayson 37:23000a47ed2b 81 loggerReportLoop();
DavidEGrayson 37:23000a47ed2b 82 }
DavidEGrayson 37:23000a47ed2b 83
DavidEGrayson 37:23000a47ed2b 84 void loggerService()
DavidEGrayson 37:23000a47ed2b 85 {
DavidEGrayson 48:c84b7b3ab0e8 86 // loggerPacer.pace()
DavidEGrayson 48:c84b7b3ab0e8 87 if (totalEncoderCounts > nextLogEncoderCount)
DavidEGrayson 37:23000a47ed2b 88 {
DavidEGrayson 48:c84b7b3ab0e8 89 nextLogEncoderCount += logSpacing;
DavidEGrayson 48:c84b7b3ab0e8 90
DavidEGrayson 48:c84b7b3ab0e8 91 struct LogEntry entry;
DavidEGrayson 48:c84b7b3ab0e8 92 entry.turnAngle = turnSensor.getAngle() >> 16;
DavidEGrayson 48:c84b7b3ab0e8 93 entry.x = reckoner.x >> 16;
DavidEGrayson 48:c84b7b3ab0e8 94 entry.y = reckoner.y >> 16;
DavidEGrayson 48:c84b7b3ab0e8 95 logger.log(&entry);
DavidEGrayson 37:23000a47ed2b 96 }
DavidEGrayson 0:e77a0edb9878 97 }
DavidEGrayson 12:835a4d24ae3b 98
DavidEGrayson 37:23000a47ed2b 99 void loggerReportLoop()
DavidEGrayson 37:23000a47ed2b 100 {
DavidEGrayson 37:23000a47ed2b 101 while(1)
DavidEGrayson 37:23000a47ed2b 102 {
DavidEGrayson 37:23000a47ed2b 103 if(button1DefinitelyPressed())
DavidEGrayson 37:23000a47ed2b 104 {
DavidEGrayson 37:23000a47ed2b 105 logger.dump();
DavidEGrayson 37:23000a47ed2b 106 }
DavidEGrayson 37:23000a47ed2b 107 }
DavidEGrayson 37:23000a47ed2b 108 }
DavidEGrayson 37:23000a47ed2b 109
DavidEGrayson 37:23000a47ed2b 110
DavidEGrayson 28:4374035df5e0 111 void loadCalibration()
DavidEGrayson 28:4374035df5e0 112 {
DavidEGrayson 32:83a13b06093c 113 /** QTR-3RC **/
DavidEGrayson 39:a5e25fd52ff8 114 lineTracker.calibratedMinimum[0] = 137;
DavidEGrayson 39:a5e25fd52ff8 115 lineTracker.calibratedMinimum[1] = 132;
DavidEGrayson 39:a5e25fd52ff8 116 lineTracker.calibratedMinimum[2] = 154;
DavidEGrayson 39:a5e25fd52ff8 117 lineTracker.calibratedMaximum[0] = 644;
DavidEGrayson 39:a5e25fd52ff8 118 lineTracker.calibratedMaximum[1] = 779;
DavidEGrayson 32:83a13b06093c 119 lineTracker.calibratedMaximum[2] = 1000;
DavidEGrayson 32:83a13b06093c 120
DavidEGrayson 32:83a13b06093c 121 /** QTR-3A
DavidEGrayson 28:4374035df5e0 122 lineTracker.calibratedMinimum[0] = 34872;
DavidEGrayson 28:4374035df5e0 123 lineTracker.calibratedMinimum[1] = 29335;
DavidEGrayson 28:4374035df5e0 124 lineTracker.calibratedMinimum[2] = 23845;
DavidEGrayson 28:4374035df5e0 125 lineTracker.calibratedMaximum[0] = 59726;
DavidEGrayson 28:4374035df5e0 126 lineTracker.calibratedMaximum[1] = 60110;
DavidEGrayson 32:83a13b06093c 127 lineTracker.calibratedMaximum[2] = 58446;
DavidEGrayson 32:83a13b06093c 128 **/
DavidEGrayson 28:4374035df5e0 129 }
DavidEGrayson 28:4374035df5e0 130
DavidEGrayson 12:835a4d24ae3b 131 void updateReckonerFromEncoders()
DavidEGrayson 12:835a4d24ae3b 132 {
DavidEGrayson 12:835a4d24ae3b 133 while(encoderBuffer.hasEvents())
DavidEGrayson 12:835a4d24ae3b 134 {
DavidEGrayson 12:835a4d24ae3b 135 PololuEncoderEvent event = encoderBuffer.readEvent();
DavidEGrayson 12:835a4d24ae3b 136 switch(event)
DavidEGrayson 12:835a4d24ae3b 137 {
DavidEGrayson 17:2df9861f53ee 138 case ENCODER_LEFT | POLOLU_ENCODER_EVENT_INC:
DavidEGrayson 17:2df9861f53ee 139 reckoner.handleTickLeftForward();
DavidEGrayson 48:c84b7b3ab0e8 140 totalEncoderCounts++;
DavidEGrayson 17:2df9861f53ee 141 break;
DavidEGrayson 17:2df9861f53ee 142 case ENCODER_LEFT | POLOLU_ENCODER_EVENT_DEC:
DavidEGrayson 17:2df9861f53ee 143 reckoner.handleTickLeftBackward();
DavidEGrayson 48:c84b7b3ab0e8 144 totalEncoderCounts--;
DavidEGrayson 17:2df9861f53ee 145 break;
DavidEGrayson 17:2df9861f53ee 146 case ENCODER_RIGHT | POLOLU_ENCODER_EVENT_INC:
DavidEGrayson 17:2df9861f53ee 147 reckoner.handleTickRightForward();
DavidEGrayson 48:c84b7b3ab0e8 148 totalEncoderCounts++;
DavidEGrayson 17:2df9861f53ee 149 break;
DavidEGrayson 17:2df9861f53ee 150 case ENCODER_RIGHT | POLOLU_ENCODER_EVENT_DEC:
DavidEGrayson 17:2df9861f53ee 151 reckoner.handleTickRightBackward();
DavidEGrayson 48:c84b7b3ab0e8 152 totalEncoderCounts--;
DavidEGrayson 17:2df9861f53ee 153 break;
DavidEGrayson 12:835a4d24ae3b 154 }
DavidEGrayson 12:835a4d24ae3b 155 }
DavidEGrayson 12:835a4d24ae3b 156 }
DavidEGrayson 17:2df9861f53ee 157
DavidEGrayson 46:f11cb4f93aac 158 void updateReckoner(TurnSensor & turnSensor)
DavidEGrayson 46:f11cb4f93aac 159 {
DavidEGrayson 46:f11cb4f93aac 160 if (!encoderBuffer.hasEvents())
DavidEGrayson 46:f11cb4f93aac 161 {
DavidEGrayson 46:f11cb4f93aac 162 return;
DavidEGrayson 46:f11cb4f93aac 163 }
DavidEGrayson 46:f11cb4f93aac 164
DavidEGrayson 46:f11cb4f93aac 165 reckoner.setTurnAngle(turnSensor.getAngle());
DavidEGrayson 46:f11cb4f93aac 166
DavidEGrayson 46:f11cb4f93aac 167 while(encoderBuffer.hasEvents())
DavidEGrayson 46:f11cb4f93aac 168 {
DavidEGrayson 46:f11cb4f93aac 169 PololuEncoderEvent event = encoderBuffer.readEvent();
DavidEGrayson 46:f11cb4f93aac 170 switch(event)
DavidEGrayson 46:f11cb4f93aac 171 {
DavidEGrayson 46:f11cb4f93aac 172 case ENCODER_LEFT | POLOLU_ENCODER_EVENT_INC:
DavidEGrayson 46:f11cb4f93aac 173 case ENCODER_RIGHT | POLOLU_ENCODER_EVENT_INC:
DavidEGrayson 48:c84b7b3ab0e8 174 totalEncoderCounts++;
DavidEGrayson 46:f11cb4f93aac 175 reckoner.handleForward();
DavidEGrayson 46:f11cb4f93aac 176 break;
DavidEGrayson 46:f11cb4f93aac 177 case ENCODER_LEFT | POLOLU_ENCODER_EVENT_DEC:
DavidEGrayson 46:f11cb4f93aac 178 case ENCODER_RIGHT | POLOLU_ENCODER_EVENT_DEC:
DavidEGrayson 46:f11cb4f93aac 179 reckoner.handleBackward();
DavidEGrayson 48:c84b7b3ab0e8 180 totalEncoderCounts--;
DavidEGrayson 46:f11cb4f93aac 181 break;
DavidEGrayson 46:f11cb4f93aac 182 }
DavidEGrayson 46:f11cb4f93aac 183 }
DavidEGrayson 46:f11cb4f93aac 184 }
DavidEGrayson 46:f11cb4f93aac 185
DavidEGrayson 19:a11ffc903774 186 float magnitude()
DavidEGrayson 19:a11ffc903774 187 {
DavidEGrayson 19:a11ffc903774 188 return sqrt((float)reckoner.x * reckoner.x + (float)reckoner.y * reckoner.y);
DavidEGrayson 19:a11ffc903774 189 }
DavidEGrayson 19:a11ffc903774 190
DavidEGrayson 20:dbec34f0e76b 191 float dotProduct()
DavidEGrayson 20:dbec34f0e76b 192 {
DavidEGrayson 46:f11cb4f93aac 193 float s = (float)reckoner.sinv / (1 << 30);
DavidEGrayson 46:f11cb4f93aac 194 float c = (float)reckoner.cosv / (1 << 30);
DavidEGrayson 20:dbec34f0e76b 195 float magn = magnitude();
DavidEGrayson 20:dbec34f0e76b 196 if (magn == 0){ return 0; }
DavidEGrayson 20:dbec34f0e76b 197 return ((float)reckoner.x * c + (float)reckoner.y * s) / magn;
DavidEGrayson 20:dbec34f0e76b 198 }
DavidEGrayson 20:dbec34f0e76b 199
DavidEGrayson 18:b65fbb795396 200 // The closer this is to zero, the closer we are to pointing towards the home position.
DavidEGrayson 18:b65fbb795396 201 // It is basically a cross product of the two vectors (x, y) and (cos, sin).
DavidEGrayson 19:a11ffc903774 202 float determinant()
DavidEGrayson 18:b65fbb795396 203 {
DavidEGrayson 18:b65fbb795396 204 // TODO: get rid of the magic numbers here (i.e. 30)
DavidEGrayson 46:f11cb4f93aac 205 float s = (float)reckoner.sinv / (1 << 30);
DavidEGrayson 46:f11cb4f93aac 206 float c = (float)reckoner.cosv / (1 << 30);
DavidEGrayson 19:a11ffc903774 207 return (reckoner.x * s - reckoner.y * c) / magnitude();
DavidEGrayson 19:a11ffc903774 208 }
DavidEGrayson 19:a11ffc903774 209
DavidEGrayson 21:c279c6a83671 210 int16_t reduceSpeed(int16_t speed, int32_t reduction)
DavidEGrayson 19:a11ffc903774 211 {
DavidEGrayson 19:a11ffc903774 212 if (reduction > speed)
DavidEGrayson 19:a11ffc903774 213 {
DavidEGrayson 19:a11ffc903774 214 return 0;
DavidEGrayson 19:a11ffc903774 215 }
DavidEGrayson 19:a11ffc903774 216 else
DavidEGrayson 19:a11ffc903774 217 {
DavidEGrayson 19:a11ffc903774 218 return speed - reduction;
DavidEGrayson 19:a11ffc903774 219 }
DavidEGrayson 18:b65fbb795396 220 }
DavidEGrayson 18:b65fbb795396 221
DavidEGrayson 21:c279c6a83671 222 void waitForSignalToStart()
DavidEGrayson 21:c279c6a83671 223 {
DavidEGrayson 21:c279c6a83671 224 while(!button1DefinitelyPressed())
DavidEGrayson 21:c279c6a83671 225 {
DavidEGrayson 21:c279c6a83671 226 updateReckonerFromEncoders();
DavidEGrayson 38:5e93a479c244 227 }
DavidEGrayson 21:c279c6a83671 228 reckoner.reset();
DavidEGrayson 38:5e93a479c244 229 while(button1DefinitelyPressed())
DavidEGrayson 38:5e93a479c244 230 {
DavidEGrayson 38:5e93a479c244 231 updateReckonerFromEncoders();
DavidEGrayson 38:5e93a479c244 232 }
DavidEGrayson 38:5e93a479c244 233 wait(0.2);
DavidEGrayson 21:c279c6a83671 234 }
DavidEGrayson 21:c279c6a83671 235
DavidEGrayson 28:4374035df5e0 236 void updateMotorsToFollowLine()
DavidEGrayson 24:fc01d9125d3b 237 {
DavidEGrayson 39:a5e25fd52ff8 238 const int16_t drivingSpeed = 400;
DavidEGrayson 40:e79cefc241f8 239 const int32_t followLineStrength = drivingSpeed * 5 / 4;
DavidEGrayson 28:4374035df5e0 240
DavidEGrayson 28:4374035df5e0 241 int16_t speedLeft = drivingSpeed;
DavidEGrayson 28:4374035df5e0 242 int16_t speedRight = drivingSpeed;
DavidEGrayson 28:4374035df5e0 243 int16_t reduction = (lineTracker.getLinePosition() - 1000) * followLineStrength / 1000;
DavidEGrayson 28:4374035df5e0 244 if(reduction < 0)
DavidEGrayson 28:4374035df5e0 245 {
DavidEGrayson 28:4374035df5e0 246 speedLeft = reduceSpeed(speedLeft, -reduction);
DavidEGrayson 28:4374035df5e0 247 }
DavidEGrayson 28:4374035df5e0 248 else
DavidEGrayson 28:4374035df5e0 249 {
DavidEGrayson 28:4374035df5e0 250 speedRight = reduceSpeed(speedRight, reduction);
DavidEGrayson 28:4374035df5e0 251 }
DavidEGrayson 24:fc01d9125d3b 252
DavidEGrayson 39:a5e25fd52ff8 253 motorsSpeedSet(speedLeft, speedRight);
DavidEGrayson 24:fc01d9125d3b 254 }
DavidEGrayson 20:dbec34f0e76b 255
DavidEGrayson 39:a5e25fd52ff8 256 void updateMotorsToFollowLineFast()
DavidEGrayson 21:c279c6a83671 257 {
DavidEGrayson 48:c84b7b3ab0e8 258 const int16_t drivingSpeed = 1000;
DavidEGrayson 40:e79cefc241f8 259 const int32_t followLineStrength = drivingSpeed * 5 / 4;
DavidEGrayson 40:e79cefc241f8 260 static int16_t lastPosition = 1000;
DavidEGrayson 39:a5e25fd52ff8 261
DavidEGrayson 39:a5e25fd52ff8 262 int16_t position = lineTracker.getLinePosition();
DavidEGrayson 21:c279c6a83671 263
DavidEGrayson 39:a5e25fd52ff8 264 int16_t speedLeft = drivingSpeed;
DavidEGrayson 39:a5e25fd52ff8 265 int16_t speedRight = drivingSpeed;
DavidEGrayson 40:e79cefc241f8 266 int32_t veer = (position - 1000) * followLineStrength / 1000 + (position - lastPosition) * 200;
DavidEGrayson 40:e79cefc241f8 267 if(veer > 0)
DavidEGrayson 39:a5e25fd52ff8 268 {
DavidEGrayson 40:e79cefc241f8 269 speedRight = reduceSpeed(speedRight, veer);
DavidEGrayson 21:c279c6a83671 270 }
DavidEGrayson 39:a5e25fd52ff8 271 else
DavidEGrayson 39:a5e25fd52ff8 272 {
DavidEGrayson 40:e79cefc241f8 273 speedLeft = reduceSpeed(speedLeft, -veer);
DavidEGrayson 39:a5e25fd52ff8 274 }
DavidEGrayson 39:a5e25fd52ff8 275
DavidEGrayson 39:a5e25fd52ff8 276 motorsSpeedSet(speedLeft, speedRight);
DavidEGrayson 39:a5e25fd52ff8 277
DavidEGrayson 39:a5e25fd52ff8 278 lastPosition = position;
DavidEGrayson 20:dbec34f0e76b 279 }
DavidEGrayson 20:dbec34f0e76b 280
DavidEGrayson 39:a5e25fd52ff8 281 void followLineFast()
DavidEGrayson 48:c84b7b3ab0e8 282 {
DavidEGrayson 48:c84b7b3ab0e8 283 totalEncoderCounts = 0;
DavidEGrayson 39:a5e25fd52ff8 284 Pacer reportPacer(200000);
DavidEGrayson 19:a11ffc903774 285
DavidEGrayson 39:a5e25fd52ff8 286 loadCalibration();
DavidEGrayson 40:e79cefc241f8 287 uint32_t loopCount = 0;
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 40:e79cefc241f8 293 loopCount += 1;
DavidEGrayson 46:f11cb4f93aac 294 turnSensor.update();
DavidEGrayson 46:f11cb4f93aac 295 updateReckoner(turnSensor);
DavidEGrayson 37:23000a47ed2b 296 loggerService();
DavidEGrayson 40:e79cefc241f8 297
DavidEGrayson 40:e79cefc241f8 298 if ((loopCount % 256) == 0)
DavidEGrayson 40:e79cefc241f8 299 {
DavidEGrayson 40:e79cefc241f8 300 pc.printf("%d\r\n", lineTracker.getLinePosition());
DavidEGrayson 40:e79cefc241f8 301 }
DavidEGrayson 40:e79cefc241f8 302
DavidEGrayson 39:a5e25fd52ff8 303 lineTracker.read();
DavidEGrayson 44:edcacba44760 304 updateMotorsToFollowLineFast();
DavidEGrayson 44:edcacba44760 305
DavidEGrayson 44:edcacba44760 306 if (button1DefinitelyPressed())
DavidEGrayson 44:edcacba44760 307 {
DavidEGrayson 44:edcacba44760 308 break;
DavidEGrayson 44:edcacba44760 309 }
DavidEGrayson 18:b65fbb795396 310 }
DavidEGrayson 44:edcacba44760 311 motorsSpeedSet(0, 0);
DavidEGrayson 20:dbec34f0e76b 312 }
DavidEGrayson 20:dbec34f0e76b 313