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 Reckoner reckoner;
00019 LineTracker lineTracker;
00020 TurnSensor turnSensor;
00021 Logger logger;
00022 Pacer loggerPacer(50000);
00023 
00024 uint8_t lapsCompleted = 0;
00025 uint32_t totalEncoderCounts = 0;
00026 uint32_t nextLogEncoderCount = 0;
00027 const uint32_t logSpacing = 100;
00028 
00029 void setLeds(bool v1, bool v2, bool v3, bool v4)
00030 {
00031    led1 = v1;
00032    led2 = v2;
00033    led3 = v3;
00034    led4 = v4;
00035 }
00036 
00037 int __attribute__((noreturn)) main()
00038 {
00039     pc.baud(115200);
00040     
00041     // Enable pull-ups on encoder pins and give them a chance to settle.
00042     if (l3gInit())
00043     {
00044         // Error initializing the gyro.
00045         setLeds(0, 0, 1, 1);
00046         while(1);
00047     }
00048 
00049     encodersInit();
00050     motorsInit();
00051     buttonsInit();
00052 
00053     // Test routines
00054     //testMotors();
00055     //testEncoders();
00056     //testLineSensors();
00057     //testReckoner();
00058     //testButtons();
00059     //testDriveHome();
00060     //testFinalSettleIn();
00061     //testCalibrate();
00062     //testLineFollowing();
00063     //testAnalog();
00064     //testSensorGlitches();
00065     //testTurnInPlace();
00066     //testCloseness();
00067     //testLogger();
00068     //testL3g();
00069     //testTurnSensor();
00070     //testReckoningWithGyro();
00071 
00072     // Real routines for the contest.
00073     loadCalibration();
00074     
00075     setLeds(1, 0, 0, 0);
00076     waitForSignalToStart();
00077   
00078     setLeds(0, 1, 0, 0);    // led3 and led4 get set by followLineSmart for debugging
00079     //followLineFast();
00080     followLineSmart();
00081     
00082     setLeds(1, 1, 1, 1);
00083     loggerReportLoop();
00084 }
00085 
00086 void loggerService()
00087 {
00088     // loggerPacer.pace()
00089     if (totalEncoderCounts > nextLogEncoderCount)
00090     {
00091         nextLogEncoderCount += logSpacing;
00092         
00093         struct LogEntry entry;
00094         entry.turnAngle = turnSensor.getAngle() >> 16;
00095         entry.x = reckoner.x >> 16;
00096         entry.y = reckoner.y >> 16;
00097         logger.log(&entry);
00098     }
00099 }
00100 
00101 void loggerReportLoop()
00102 {
00103     while(1)
00104     {
00105         if(button1DefinitelyPressed())
00106         {
00107             logger.dump();
00108         }
00109     }   
00110 }
00111 
00112 
00113 void loadCalibration()
00114 {
00115     /** QTR-3RC **/
00116     lineTracker.calibratedMinimum[0] =  137;
00117     lineTracker.calibratedMinimum[1] =  132;
00118     lineTracker.calibratedMinimum[2] =  154;
00119     lineTracker.calibratedMaximum[0] =  644;
00120     lineTracker.calibratedMaximum[1] =  779;
00121     lineTracker.calibratedMaximum[2] = 1000;    
00122     
00123     /** QTR-3A
00124     lineTracker.calibratedMinimum[0] = 34872;
00125     lineTracker.calibratedMinimum[1] = 29335;
00126     lineTracker.calibratedMinimum[2] = 23845;
00127     lineTracker.calibratedMaximum[0] = 59726;
00128     lineTracker.calibratedMaximum[1] = 60110;
00129     lineTracker.calibratedMaximum[2] = 58446;
00130     **/
00131 }
00132 
00133 void updateReckonerFromEncoders()
00134 {
00135     while(encoderBuffer.hasEvents())
00136     {
00137         PololuEncoderEvent event = encoderBuffer.readEvent();
00138         switch(event)
00139         {
00140         case ENCODER_LEFT | POLOLU_ENCODER_EVENT_INC:
00141             reckoner.handleTickLeftForward();
00142             totalEncoderCounts++;
00143             break;
00144         case ENCODER_LEFT | POLOLU_ENCODER_EVENT_DEC:
00145             reckoner.handleTickLeftBackward();
00146             totalEncoderCounts--;
00147             break;
00148         case ENCODER_RIGHT | POLOLU_ENCODER_EVENT_INC:
00149             reckoner.handleTickRightForward();
00150             totalEncoderCounts++;
00151             break;
00152         case ENCODER_RIGHT | POLOLU_ENCODER_EVENT_DEC:
00153             reckoner.handleTickRightBackward();
00154             totalEncoderCounts--;
00155             break;
00156         }
00157     }
00158 }
00159 
00160 void updateReckoner(TurnSensor & turnSensor)
00161 {
00162     if (!encoderBuffer.hasEvents())
00163     {
00164         return;
00165     }
00166     
00167     reckoner.setTurnAngle(turnSensor.getAngle());
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 << 30);
00196     float c = (float)reckoner.cosv / (1 << 30);
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     // TODO: get rid of the magic numbers here (i.e. 30)
00207     float s = (float)reckoner.sinv / (1 << 30);
00208     float c = (float)reckoner.cosv / (1 << 30);
00209     return (reckoner.x * s - reckoner.y * c) / magnitude();
00210 }
00211 
00212 int16_t reduceSpeed(int16_t speed, int32_t reduction)
00213 {
00214     if (reduction > speed)
00215     {
00216         return 0;   
00217     }
00218     else
00219     {
00220         return speed - reduction;
00221     }
00222 }
00223 
00224 void waitForSignalToStart()
00225 {
00226     while(!button1DefinitelyPressed())
00227     {
00228         updateReckonerFromEncoders();
00229     }
00230     reckoner.reset();
00231     while(button1DefinitelyPressed())
00232     {
00233         updateReckonerFromEncoders();
00234     }
00235     wait(0.2);
00236 }
00237 
00238 void updateMotorsToFollowLineSlow()
00239 {
00240     const int16_t drivingSpeed = 400;
00241     const int32_t followLineStrength = drivingSpeed * 5 / 4;
00242 
00243     int16_t speedLeft = drivingSpeed;
00244     int16_t speedRight = drivingSpeed;
00245     int16_t reduction = (lineTracker.getLinePosition() - 1000) * followLineStrength / 1000;
00246     if(reduction < 0)
00247     {
00248         speedLeft = reduceSpeed(speedLeft, -reduction);   
00249     }
00250     else
00251     {
00252         speedRight = reduceSpeed(speedRight, reduction);
00253     }
00254     
00255     motorsSpeedSet(speedLeft, speedRight);
00256 }
00257 
00258 void updateMotorsToFollowLineFast(int16_t drivingSpeed)
00259 {
00260     const int32_t followLineStrength = drivingSpeed * 5 / 4;
00261     static int16_t lastPosition = 1000;
00262 
00263     int16_t position = lineTracker.getLinePosition();
00264 
00265     int16_t speedLeft = drivingSpeed;
00266     int16_t speedRight = drivingSpeed;
00267     int32_t veer = (position - 1000) * followLineStrength / 1000 + (position - lastPosition) * 200;
00268     if(veer > 0)
00269     {
00270         speedRight = reduceSpeed(speedRight, veer);
00271     }
00272     else
00273     {
00274         speedLeft = reduceSpeed(speedLeft, -veer);   
00275     }
00276     
00277     motorsSpeedSet(speedLeft, speedRight);
00278     
00279     lastPosition = position;
00280 }
00281 
00282 void followLineFast()
00283 {
00284     totalEncoderCounts = 0;
00285     Pacer reportPacer(200000);
00286     
00287     loadCalibration();
00288     Timer timer;
00289     timer.start();
00290     turnSensor.start();
00291     while(1)
00292     {
00293         turnSensor.update();
00294         updateReckoner(turnSensor);
00295         loggerService();
00296         
00297         lineTracker.read();
00298         updateMotorsToFollowLineFast(1000);
00299         
00300         if (button1DefinitelyPressed())
00301         {
00302             break;
00303         }
00304     }
00305     motorsSpeedSet(0, 0);
00306 }
00307 
00308 bool foundStart()
00309 {
00310     static int32_t lastX = 0;
00311     bool result = lastX < 0 && reckoner.x >= 0 && abs(reckoner.y) < (115 << 16) &&
00312       totalEncoderCounts > 10000 && abs(turnSensor.getAngle()) < turnAngle1 * 30;
00313     lastX = reckoner.x;
00314     return result;
00315 }
00316 
00317 bool onLongStraightPart()
00318 {
00319     if (lapsCompleted == 0) { return false; }
00320     
00321     // Figure out what part of the log corresponds to our current situation.
00322     uint32_t logIndex = totalEncoderCounts / logSpacing;
00323     
00324     if (logIndex >= logger.getSize())
00325     {
00326         // Should not happen.
00327         return false;
00328     }
00329     
00330     // To improve this, we could check that turnSensor.getAngle() matches what is in the log.
00331 
00332     uint32_t angle1 = turnSensor.getAngleUnsigned();
00333 
00334     // 2000 encoder ticks
00335     const uint32_t lookAheadAmount = 3000 / logSpacing;
00336     
00337     // Figure out how far away the next turn is.
00338     uint32_t i = logIndex;
00339     while(1)
00340     {
00341         i++;
00342         
00343         if (i >= logger.getSize())
00344         {
00345             // reached the end the log
00346             return false;
00347         }
00348 
00349         if (i > logIndex + lookAheadAmount)
00350         {
00351             // looked far enough ahead that we don't think there is a turn coming up soon
00352             return true;
00353         }
00354         
00355         
00356         uint32_t angle2 = (uint16_t)logger.entries[i].turnAngle << 16;
00357         if (abs((int32_t)(angle2 - angle1)) > turnAngle45)
00358         {
00359             // detected a turn
00360             return false;
00361         }
00362     }
00363 }
00364 
00365 void followLineSmart()
00366 {
00367     lapsCompleted = 0;   
00368     totalEncoderCounts = 0;
00369 
00370     Pacer reportPacer(200000);
00371     
00372     loadCalibration();
00373     turnSensor.start();
00374     while(1)
00375     {
00376         turnSensor.update();
00377         updateReckoner(turnSensor);
00378         loggerService();
00379         
00380         lineTracker.read();
00381         
00382         // By default, choose a cautious speed of 1000 (out of 1200).
00383         int16_t speed = 1000;
00384         
00385         // Go fast if we know we are on a long straight part.
00386         if (onLongStraightPart())
00387         {
00388             speed = 1200;
00389             led4 = 1;
00390         }
00391         else
00392         {
00393             led4 = 0;
00394         }
00395         
00396         
00397         updateMotorsToFollowLineFast(speed);
00398 
00399         if (foundStart())
00400         {
00401             // Another lap has been completed!
00402             lapsCompleted += 1;
00403             led3 = lapsCompleted & 1;
00404 
00405             reckoner.reset();
00406             turnSensor.reset();
00407             totalEncoderCounts = 0;
00408             nextLogEncoderCount = 0;
00409         }
00410         
00411         if (lapsCompleted == 3 && totalEncoderCounts > 4000)
00412         {
00413             // Classy move: know when you are done with the competition and stop automatically.
00414             // (Of course, there is a risk that this will backfire!)
00415             break;
00416         }
00417         
00418         if (button1DefinitelyPressed())
00419         {
00420             break;
00421         }
00422     }
00423     motorsSpeedSet(0, 0);
00424 }
00425