David's dead reckoning code for the LVBots competition on March 6th. Uses the mbed LPC1768, DRV8835, QTR-3RC, and two DC motors with encoders.
Dependencies: PololuEncoder Pacer mbed GeneralDebouncer
main.cpp
00001 #include <mbed.h> 00002 #include <Pacer.h> 00003 #include <GeneralDebouncer.h> 00004 #include <math.h> 00005 00006 #include "main.h" 00007 #include "motors.h" 00008 #include "encoders.h" 00009 #include "leds.h" 00010 #include "pc_serial.h" 00011 #include "test.h" 00012 #include "reckoner.h" 00013 #include "buttons.h" 00014 #include "line_tracker.h" 00015 #include "l3g.h" 00016 #include "turn_sensor.h" 00017 00018 void __attribute__((noreturn)) doDeadReckoning(); 00019 00020 Reckoner reckoner; 00021 LineTracker lineTracker; 00022 TurnSensor turnSensor; 00023 Logger logger; 00024 00025 const uint32_t timeout = 0; 00026 00027 const uint32_t logSpacing = 200; 00028 00029 const int16_t drivingSpeed = 500; 00030 00031 uint32_t totalEncoderCounts = 0; 00032 uint32_t nextLogEncoderCount = 0; 00033 00034 00035 void setLeds(bool v1, bool v2, bool v3, bool v4) 00036 { 00037 led1 = v1; 00038 led2 = v2; 00039 led3 = v3; 00040 led4 = v4; 00041 } 00042 00043 int __attribute__((noreturn)) main() 00044 { 00045 pc.baud(115200); 00046 00047 if (l3gInit()) 00048 { 00049 // Error initializing the gyro. 00050 setLeds(0, 0, 1, 1); 00051 while(1); 00052 } 00053 00054 // Enable pull-ups on encoder pins and give them a chance to settle. 00055 encodersInit(); 00056 motorsInit(); 00057 buttonsInit(); 00058 00059 // Test routines 00060 //testButtons(); 00061 //testEncoders(); 00062 //testMotors(); 00063 //testMotorSpeed(); 00064 //testL3g(); 00065 //testL3gAndShowAverage(); 00066 //testTurnSensor(); 00067 //testReckoner(); 00068 //testCloseness(); // didn't do it in 2019 00069 //testDriveHome(); 00070 //testFinalSettleIn(); // doesn't really work 00071 //testLineSensorsAndCalibrate(); 00072 //testLineFollowing(); 00073 //testTurnInPlace(); // didn't do it in 2019 00074 //testLogger(); // didn't do it in 2019 00075 00076 doDeadReckoning(); 00077 } 00078 00079 void doDeadReckoning() 00080 { 00081 loadLineCalibration(); 00082 doGyroCalibration(); 00083 turnSensor.start(); 00084 00085 setLeds(1, 0, 0, 0); 00086 waitForSignalToStart(); 00087 00088 setLeds(0, 1, 0, 0); 00089 findLine(); 00090 00091 setLeds(0, 0, 1, 0); 00092 followLineToEnd(); 00093 00094 setLeds(1, 0, 1, 0); 00095 driveHomeAlmost(); 00096 00097 //setLeds(0, 1, 1, 0); 00098 //finalSettleIn(); 00099 00100 setLeds(1, 1, 1, 1); 00101 loggerReportLoop(); 00102 } 00103 00104 void loggerService() 00105 { 00106 if (totalEncoderCounts > nextLogEncoderCount) 00107 { 00108 nextLogEncoderCount += logSpacing; 00109 00110 struct LogEntry entry; 00111 entry.turnAngle = turnSensor.getAngle() >> 16; 00112 entry.x = reckoner.x >> 16; 00113 entry.y = reckoner.y >> 16; 00114 logger.log(&entry); 00115 } 00116 } 00117 00118 void loggerReportLoop() 00119 { 00120 while(1) 00121 { 00122 if(button1DefinitelyPressed()) 00123 { 00124 logger.dump(); 00125 } 00126 } 00127 } 00128 00129 void doGyroCalibration() 00130 { 00131 wait_ms(1000); // Time for the robot to stop moving. 00132 while (!l3gCalibrateDone()) 00133 { 00134 l3gCalibrate(); 00135 wait_ms(2); 00136 } 00137 } 00138 00139 void loadLineCalibration() 00140 { 00141 // QTR-3RC calibration from contest in 2014. 00142 //lineTracker.calibratedMinimum[0] = 100; 00143 //lineTracker.calibratedMaximum[0] = 792; 00144 //lineTracker.calibratedMinimum[1] = 94; 00145 //lineTracker.calibratedMaximum[1] = 807; 00146 //lineTracker.calibratedMinimum[2] = 103; 00147 //lineTracker.calibratedMaximum[2] = 1000; 00148 00149 // QTR-3RC calibration from David's home setup, 2019-07-27. 00150 // Generated with testLineSensorsAndCalibrate(). 00151 lineTracker.calibratedMinimum[0] = 210; 00152 lineTracker.calibratedMaximum[0] = 757; 00153 lineTracker.calibratedMinimum[1] = 197; 00154 lineTracker.calibratedMaximum[1] = 1000; 00155 lineTracker.calibratedMinimum[2] = 203; 00156 lineTracker.calibratedMaximum[2] = 746; 00157 } 00158 00159 void updateReckoner() 00160 { 00161 turnSensor.update(); 00162 reckoner.setTurnAngle(turnSensor.getAngle()); 00163 00164 if (!encoderBuffer.hasEvents()) 00165 { 00166 return; 00167 } 00168 00169 while(encoderBuffer.hasEvents()) 00170 { 00171 PololuEncoderEvent event = encoderBuffer.readEvent(); 00172 switch(event) 00173 { 00174 case ENCODER_LEFT | POLOLU_ENCODER_EVENT_INC: 00175 case ENCODER_RIGHT | POLOLU_ENCODER_EVENT_INC: 00176 totalEncoderCounts++; 00177 reckoner.handleForward(); 00178 break; 00179 case ENCODER_LEFT | POLOLU_ENCODER_EVENT_DEC: 00180 case ENCODER_RIGHT | POLOLU_ENCODER_EVENT_DEC: 00181 reckoner.handleBackward(); 00182 totalEncoderCounts--; 00183 break; 00184 } 00185 } 00186 } 00187 00188 float magnitude() 00189 { 00190 return sqrt((float)reckoner.x * reckoner.x + (float)reckoner.y * reckoner.y); 00191 } 00192 00193 float dotProduct() 00194 { 00195 float s = (float)reckoner.sinv / (1 << RECKONER_LOG_UNIT); 00196 float c = (float)reckoner.cosv / (1 << RECKONER_LOG_UNIT); 00197 float magn = magnitude(); 00198 if (magn == 0){ return 0; } 00199 return ((float)reckoner.x * c + (float)reckoner.y * s) / magn; 00200 } 00201 00202 // The closer this is to zero, the closer we are to pointing towards the home position. 00203 // It is basically a cross product of the two vectors (x, y) and (cos, sin). 00204 float determinant() 00205 { 00206 float s = (float)reckoner.sinv / (1 << RECKONER_LOG_UNIT); 00207 float c = (float)reckoner.cosv / (1 << RECKONER_LOG_UNIT); 00208 return (reckoner.x * s - reckoner.y * c) / magnitude(); 00209 } 00210 00211 int16_t reduceSpeed(int16_t speed, int32_t reduction) 00212 { 00213 if (reduction > speed) 00214 { 00215 return 0; 00216 } 00217 else 00218 { 00219 return speed - reduction; 00220 } 00221 } 00222 00223 void waitForSignalToStart() 00224 { 00225 while(!button1DefinitelyPressed()) 00226 { 00227 updateReckoner(); 00228 } 00229 reckoner.reset(); 00230 turnSensor.reset(); 00231 while(button1DefinitelyPressed()) 00232 { 00233 updateReckoner(); 00234 } 00235 wait(0.2); 00236 } 00237 00238 // Keep the robot pointing the in the right direction (1, 0). 00239 // This should basically keep it going straight. 00240 void updateMotorsToDriveStraight() 00241 { 00242 const int32_t straightDriveStrength = 1000; 00243 int16_t speedLeft = drivingSpeed; 00244 int16_t speedRight = drivingSpeed; 00245 int32_t reduction = reckoner.sinv * straightDriveStrength >> RECKONER_LOG_UNIT; 00246 if (reduction > 0) 00247 { 00248 speedRight = reduceSpeed(speedRight, reduction); 00249 } 00250 else 00251 { 00252 speedLeft = reduceSpeed(speedLeft, -reduction); 00253 } 00254 motorsSpeedSet(speedLeft, speedRight); 00255 } 00256 00257 void updateMotorsToFollowLine() 00258 { 00259 const int followLineStrength = drivingSpeed * 5 / 4; 00260 00261 int16_t speedLeft = drivingSpeed; 00262 int16_t speedRight = drivingSpeed; 00263 int16_t reduction = (lineTracker.getLinePosition() - 1000) * followLineStrength / 1000; 00264 if(reduction < 0) 00265 { 00266 speedLeft = reduceSpeed(speedLeft, -reduction); 00267 } 00268 else 00269 { 00270 speedRight = reduceSpeed(speedRight, reduction); 00271 } 00272 00273 motorsSpeedSet(speedLeft, speedRight); 00274 } 00275 00276 void findLine() 00277 { 00278 GeneralDebouncer lineStatus(10000); 00279 while(1) 00280 { 00281 lineTracker.read(); 00282 lineTracker.updateCalibration(); 00283 updateReckoner(); 00284 loggerService(); 00285 updateMotorsToDriveStraight(); 00286 lineStatus.update(lineTracker.getLineVisible()); 00287 00288 if(lineStatus.getState() == true && lineStatus.getTimeInCurrentStateMicroseconds() > 20000) 00289 { 00290 break; 00291 } 00292 } 00293 } 00294 00295 void followLineToEnd() 00296 { 00297 Timer timer; 00298 timer.start(); 00299 00300 GeneralDebouncer lineStatus(10000); 00301 const uint32_t lineDebounceTime = 1000000; 00302 00303 while(1) 00304 { 00305 lineTracker.read(); 00306 updateReckoner(); 00307 loggerService(); 00308 00309 lineStatus.update(lineTracker.getLineVisible()); 00310 00311 bool lostLine = lineStatus.getState() == false && 00312 lineStatus.getTimeInCurrentStateMicroseconds() > lineDebounceTime; 00313 if (lostLine && timer.read_ms() >= 2000) 00314 { 00315 break; 00316 } 00317 00318 if (timeout && timer.read_ms() > timeout) 00319 { 00320 break; 00321 } 00322 00323 updateMotorsToFollowLine(); 00324 } 00325 } 00326 00327 void driveHomeAlmost() 00328 { 00329 Timer timer; 00330 timer.start(); 00331 00332 while(1) 00333 { 00334 updateReckoner(); 00335 loggerService(); 00336 00337 float magn = magnitude(); 00338 00339 if (magn < (1<<(14+7))) 00340 { 00341 // We are within 128 encoder ticks, so go to the next step. 00342 break; 00343 } 00344 00345 float det = determinant(); 00346 00347 int16_t speedLeft = drivingSpeed; 00348 int16_t speedRight = drivingSpeed; 00349 if (magn < (1<<(14+9))) // Within 512 encoder ticks of the origin, so slow down. 00350 { 00351 int16_t reduction = (1 - magn/(1<<(14+8))) * (drivingSpeed/2); 00352 speedLeft = reduceSpeed(speedLeft, reduction); 00353 speedRight = reduceSpeed(speedRight, reduction); 00354 } 00355 00356 // tmphax 00357 if (0) { 00358 if (det != det) 00359 { 00360 // NaN 00361 setLeds(1, 0, 0, 1); 00362 } 00363 else if (det > 0.5) 00364 { 00365 setLeds(0, 0, 1, 1); 00366 } 00367 else if (det > 0.1) 00368 { 00369 setLeds(0, 0, 0, 1); 00370 } 00371 else if (det < -0.5) 00372 { 00373 setLeds(1, 1, 0, 0); 00374 } 00375 else if (det < -0.1) 00376 { 00377 setLeds(1, 0, 0, 0); 00378 } 00379 else 00380 { 00381 // Heading basically the right direction. 00382 setLeds(1, 1, 1, 1); 00383 } 00384 speedLeft = speedRight = 0; 00385 } 00386 00387 if (det > 0) 00388 { 00389 speedLeft = reduceSpeed(speedLeft, det * 1000); 00390 } 00391 else 00392 { 00393 speedRight = reduceSpeed(speedRight, -det * 1000); 00394 } 00395 motorsSpeedSet(speedLeft, speedRight); 00396 } 00397 00398 motorsSpeedSet(0, 0); 00399 } 00400 00401 void finalSettleIn() 00402 { 00403 const int16_t settleSpeed = 300; 00404 const int16_t settleModificationStrength = 150; 00405 00406 Timer timer; 00407 timer.start(); 00408 00409 // State 0: rotating 00410 // State 1: Trying to get into final orientation: want [cos, sin] == [1<<30, 0]. 00411 uint8_t state = 0; 00412 00413 Pacer reportPacer(200000); 00414 Pacer motorUpdatePacer(10000); 00415 00416 float integral = 0; 00417 00418 motorsSpeedSet(-settleSpeed, settleSpeed); // avoid waiting 10 ms before we start settling 00419 00420 while(1) 00421 { 00422 led1 = (state == 1); 00423 00424 updateReckoner(); 00425 loggerService(); 00426 00427 float dot = dotProduct(); 00428 int16_t speedModification = -dot * settleModificationStrength; 00429 if (speedModification > settleModificationStrength) 00430 { 00431 speedModification = settleModificationStrength; 00432 } 00433 else if (speedModification < -settleModificationStrength) 00434 { 00435 speedModification = -settleModificationStrength; 00436 } 00437 00438 if (state == 0 && timer.read_ms() >= 2000 && 00439 reckoner.cosv > (1 << (RECKONER_LOG_UNIT - 1))) 00440 { 00441 // Stop turning and start trying to maintain the right position. 00442 state = 1; 00443 } 00444 00445 if (state == 1 && timer.read_ms() >= 5000) 00446 { 00447 // Stop moving. 00448 break; 00449 } 00450 00451 if (motorUpdatePacer.pace()) 00452 { 00453 int16_t rotationSpeed; 00454 if (state == 1) 00455 { 00456 float s = (float)reckoner.sinv / (1 << RECKONER_LOG_UNIT); 00457 integral += s; 00458 rotationSpeed = -(s * 2400 + integral * 20); 00459 00460 if (rotationSpeed > 300) 00461 { 00462 rotationSpeed = 300; 00463 } 00464 if (rotationSpeed < -300) 00465 { 00466 rotationSpeed = -300; 00467 } 00468 } 00469 else 00470 { 00471 rotationSpeed = settleSpeed; 00472 } 00473 00474 int16_t speedLeft = -rotationSpeed + speedModification; 00475 int16_t speedRight = rotationSpeed + speedModification; 00476 motorsSpeedSet(speedLeft, speedRight); 00477 } 00478 00479 if (state == 1 && reportPacer.pace()) 00480 { 00481 pc.printf("%5d %5d %11d %11d | %11f %11f\r\n", 00482 reckoner.cosv, reckoner.sinv, reckoner.x, reckoner.y, 00483 determinant(), dotProduct()); 00484 } 00485 } 00486 00487 // Done! Stop moving. 00488 motorsSpeedSet(0, 0); 00489 }
Generated on Sun Jul 17 2022 15:45:15 by 1.7.2