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

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers main.cpp Source File

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 }