test.cpp

00001 // A file for testing routines that will not be used in the final firmware.
00002 
00003 #include <mbed.h>
00004 #include "motors.h"
00005 #include <Pacer.h>
00006 
00007 #include "main.h"
00008 #include "test.h"
00009 #include "leds.h"
00010 #include "encoders.h"
00011 #include "pc_serial.h"
00012 #include "line_sensors.h"
00013 #include "l3g.h"
00014 #include "turn_sensor.h"
00015 #include "reckoner.h"
00016 #include "buttons.h"
00017 
00018 void __attribute__((noreturn)) infiniteReckonerReportLoop();
00019 void printBar(const char * name, uint16_t adcResult);
00020 
00021 void testLogger()
00022 {
00023     led1 = 1;
00024     while(!button1DefinitelyPressed())
00025     {
00026         led3 = logger.isFull();
00027     
00028         updateReckoner();
00029         loggerService();
00030     }
00031     led2 = 1;
00032     loggerReportLoop();
00033 }
00034 
00035 void testCloseness()
00036 {
00037     doGyroCalibration();
00038     turnSensor.start();
00039     led1 = 1;
00040     while(1)
00041     {
00042         updateReckoner();
00043         float magn = magnitude();
00044         
00045         led3 = magn < (1 << 5);
00046         led4 = magn < (1 << 7);
00047     }
00048 }
00049 
00050 void showOrientationWithLeds34()
00051 {
00052     led3 = reckoner.cosv > 0;
00053     led4 = reckoner.sinv > 0;
00054 }
00055 
00056 void testTurnInPlace()
00057 {
00058     doGyroCalibration();
00059     turnSensor.start();
00060 
00061     led1 = 1;
00062     while(!button1DefinitelyPressed())
00063     {
00064         updateReckoner();
00065         showOrientationWithLeds34();
00066     }
00067     led2 = 1;
00068     
00069     Pacer motorUpdatePacer(10000);
00070     Timer timer;
00071     timer.start();
00072     motorsSpeedSet(-300, 300);
00073     while(timer.read_ms() < 4000)
00074     {
00075         updateReckoner();
00076         showOrientationWithLeds34();
00077     }
00078     timer.reset();
00079     
00080     float integral = 0;
00081     while (timer.read_ms() < 4000)
00082     {
00083         if (motorUpdatePacer.pace())
00084         {
00085             int16_t rotationSpeed;
00086             float s = (float)reckoner.sinv / (1 << RECKONER_LOG_UNIT);
00087             integral += s;
00088             rotationSpeed = -(s * 2400 + integral * 20);
00089             
00090             if (rotationSpeed > 450)
00091             {
00092                 rotationSpeed = 450; 
00093             }
00094             if (rotationSpeed < -450)
00095             {
00096                 rotationSpeed = -450; 
00097             }
00098             
00099             int16_t speedLeft = -rotationSpeed;
00100             int16_t speedRight = rotationSpeed;
00101             motorsSpeedSet(speedLeft, speedRight);
00102         }
00103     }
00104     
00105     infiniteReckonerReportLoop();      
00106 }
00107 
00108 // This also tests the LineTracker by printing out a lot of data from it.
00109 void testLineFollowing()
00110 {
00111     loadLineCalibration();
00112     doGyroCalibration();
00113     turnSensor.start();
00114 
00115     led1 = 1;
00116     while (!button1DefinitelyPressed())
00117     {
00118         updateReckoner();
00119     }
00120     
00121     Pacer reportPacer(200000);
00122     
00123     uint16_t loopCount = 0;
00124     while(1)
00125     {
00126         updateReckoner();
00127         bool lineVisiblePrevious = lineTracker.getLineVisible();
00128         lineTracker.read();
00129         updateMotorsToFollowLine();
00130         
00131         loopCount += 1;
00132         
00133         led2 = lineTracker.calibratedValues[0] > 500;
00134         led3 = lineTracker.calibratedValues[1] > 500;
00135         led4 = lineTracker.calibratedValues[2] > 500;
00136 
00137         if (lineVisiblePrevious != lineTracker.getLineVisible())
00138         {
00139             pc.printf("%5d ! %1d %4d | %5d %5d | %4d %4d %4d\r\n",
00140                 loopCount, lineTracker.getLineVisible(), lineTracker.getLinePosition(),
00141                 motorLeftSpeed, motorRightSpeed,
00142                 lineTracker.calibratedValues[0], lineTracker.calibratedValues[1], lineTracker.calibratedValues[2]
00143                 );
00144         }
00145         
00146         if (reportPacer.pace())
00147         {
00148             pc.printf("%5d   %1d %4d | %5d %5d | %4d %4d %4d\r\n",
00149                 loopCount, lineTracker.getLineVisible(), lineTracker.getLinePosition(),
00150                 motorLeftSpeed, motorRightSpeed,
00151                 lineTracker.calibratedValues[0], lineTracker.calibratedValues[1], lineTracker.calibratedValues[2]
00152                 );
00153         }
00154     }
00155 }
00156 
00157 void testDriveHome()
00158 {
00159     doGyroCalibration();
00160     turnSensor.start();
00161     led1 = 1;   
00162     while(!button1DefinitelyPressed())
00163     {
00164         updateReckoner();
00165     }
00166     
00167     setLeds(1, 0, 1, 0);
00168     driveHomeAlmost();
00169     
00170     //setLeds(0, 1, 1, 0);
00171     //finalSettleIn();
00172     
00173     setLeds(1, 1, 1, 1);
00174     infiniteReckonerReportLoop();     
00175 }
00176 
00177 void testFinalSettleIn()
00178 {
00179     doGyroCalibration();
00180     turnSensor.start();
00181     led1 = 1;
00182     while(!button1DefinitelyPressed())
00183     {
00184         updateReckoner();
00185     }   
00186     finalSettleIn();
00187     infiniteReckonerReportLoop();     
00188 }
00189 
00190 
00191 void testButtons()
00192 {
00193     led1 = 1;
00194     
00195     while(!button1DefinitelyReleased());
00196     while(!button1DefinitelyPressed());
00197     led2 = 1;
00198 
00199     while(!button1DefinitelyReleased());
00200     while(!button1DefinitelyPressed());
00201     led3 = 1;
00202 
00203     while(!button1DefinitelyReleased());
00204     while(!button1DefinitelyPressed());
00205     led4 = 1;
00206    
00207     while(1){};
00208 }
00209 
00210 void testReckoner()
00211 {
00212     doGyroCalibration();
00213     turnSensor.start();
00214     led1 = 1;
00215     Pacer reportPacer(100000);
00216     while(1)
00217     {
00218         updateReckoner();
00219         
00220         led1 = reckoner.x > 0;
00221         led2 = reckoner.y > 0;
00222         showOrientationWithLeds34();
00223         
00224         if (reportPacer.pace())
00225         {
00226             pc.printf("%6d %6d %11d %11d | %8d %8d %10f\r\n",
00227               reckoner.cosv, reckoner.sinv, reckoner.x, reckoner.y,
00228               encoderLeft.getCount(), encoderRight.getCount(), determinant());
00229         }
00230     }
00231 }
00232 
00233 void testTurnSensor()
00234 {
00235     pc.printf("Test turn sensor\r\n");
00236     doGyroCalibration();
00237     led1 = 1;
00238     //Pacer reportPacer(200000);  // 0.2 s
00239     Pacer reportPacer(10000000);  // 10 s
00240     Timer timer;
00241     timer.start();
00242     turnSensor.start();
00243     while(1)
00244     {
00245         turnSensor.update();
00246         if (reportPacer.pace())
00247         {
00248             pc.printf("%u, %d, %d\r\n",
00249               timer.read_ms(),
00250               turnSensor.getAngleDegrees(),
00251               turnSensor.getAngleMillidegrees());
00252         }
00253     }
00254 }
00255 
00256 void testL3gAndShowAverage()
00257 {
00258     wait_ms(2000);
00259     Pacer reportPacer(750000);
00260     Pacer readingPacer(2000);
00261     int32_t total = 0;
00262     int32_t readingCount = 0;
00263     while(1)
00264     {
00265         if (readingPacer.pace())
00266         {
00267           int32_t result = l3gZAvailable();
00268           if (result == 1)
00269           {
00270               int32_t gz = l3gZRead();
00271               if (gz < -500000)
00272               {
00273                   pc.printf("l3gZRead error: %d\n", gz);
00274               }
00275               else
00276               {
00277                   total += gz;
00278                   readingCount += 1;
00279               }
00280           }
00281           else if (result != 0)
00282           {
00283               pc.printf("l3gZAvailable error: %d\n", result);
00284           }
00285         }
00286 
00287         if (reportPacer.pace())
00288         {
00289             float average = (float)total / readingCount;
00290             pc.printf("%d, %d, %f\r\n", total, readingCount, average);
00291         }
00292     }
00293     
00294     // Gyro calibration results get hardcoded into TurnSensor::update()
00295     // for now until we figure out something better.
00296 }
00297 
00298 void testL3g()
00299 {
00300     Pacer reportPacer(750000);
00301     Timer timer;
00302     timer.start();
00303     int32_t gz = 0;
00304     bool reportedReading = false;
00305     while(1)
00306     {
00307         int32_t result = l3gZAvailable();
00308         if (result == 1)
00309         {
00310             gz = l3gZRead();
00311             reportedReading = false;
00312             if (gz > 100 || gz < -100)
00313             {
00314                 pc.printf("%u, %d\r\n", timer.read_us(), gz); 
00315                 reportedReading = true;
00316             }
00317         }
00318         else if (result != 0)
00319         {
00320             pc.printf("l3gZAvailable error: %d\n", result);
00321         }
00322 
00323         if (reportPacer.pace() && !reportedReading)
00324         {
00325             pc.printf("%u, %d\r\n", timer.read_us(), gz);
00326             reportedReading = true;
00327         }
00328     }
00329 }
00330 
00331 void testLineSensorsAndCalibrate()
00332 {
00333     Pacer reportPacer(100000);
00334     
00335     const uint16_t * values = lineTracker.rawValues;
00336     const uint16_t * min = lineTracker.calibratedMinimum;
00337     const uint16_t * max = lineTracker.calibratedMaximum;
00338     const uint16_t * calValues = lineTracker.calibratedValues;
00339 
00340     // Comment this out, and hold down button 1 while exposing the line sensor
00341     // to its typical surfaces to do calibration.
00342     loadLineCalibration();
00343 
00344     const bool printBarGraph = true;
00345     while (1)
00346     {        
00347         lineTracker.read();
00348         
00349         if (button1DefinitelyPressed())
00350         {
00351             led1 = 0;
00352             lineTracker.updateCalibration();
00353         }
00354         else
00355         {
00356             led1 = 1;
00357         }
00358         
00359         led2 = calValues[0] > 500;
00360         led3 = calValues[1] > 500;
00361         led4 = calValues[2] > 500;
00362         
00363         if (reportPacer.pace())
00364         {
00365             if (printBarGraph)
00366             {
00367                 pc.printf("\x1B[0;0H");  // VT100 command for "go to 0,0"
00368                 printBar("L", values[0]);
00369                 printBar("M", values[1]);
00370                 printBar("R", values[2]);
00371                 pc.printf("%4d %4d    \r\n", min[0], max[0]);
00372                 pc.printf("%4d %4d    \r\n", min[1], max[1]);
00373                 pc.printf("%4d %4d    \r\n", min[2], max[2]);
00374             }
00375             else
00376             {
00377                 pc.printf("%8d %8d %8d\r\n", values[0], values[1], values[2]);
00378             }
00379         }
00380     }
00381 }
00382 
00383 void testMotorSpeed()
00384 {
00385     led1 = 1;
00386     motorsSpeedSet(400, 400);
00387     wait_ms(4000);
00388     uint32_t left = encoderLeft.getCount();
00389     uint32_t right = encoderRight.getCount();
00390     motorsSpeedSet(0, 0);
00391     Pacer reportPacer(500000);
00392     while (1)
00393     {
00394         if (reportPacer.pace())
00395         {
00396             led2 = 1;
00397             pc.printf("%8d %8d\r\n", left, right);
00398             led2 = 0;
00399         }    
00400     }
00401 }
00402 
00403 void testEncoders()
00404 {
00405     Pacer reportPacer(500000);
00406     led1 = 1;
00407     while(1)
00408     {
00409         while(encoderBuffer.hasEvents())
00410         {
00411             PololuEncoderEvent event = encoderBuffer.readEvent();
00412         }
00413         
00414         if(reportPacer.pace())
00415         {
00416             led2 = 1;
00417             pc.printf("%8d %8d\r\n", encoderLeft.getCount(), encoderRight.getCount());
00418             led2 = 0;
00419        }
00420     }
00421 }
00422 
00423 void testMotors()
00424 {
00425     led1 = 1;
00426     led2 = 0;
00427     led3 = 0;
00428     while(1)
00429     {
00430         motorsSpeedSet(0, 0);
00431         led2 = 0;
00432         led3 = 0;
00433         wait(2);
00434         
00435         motorsSpeedSet(300, 300);
00436         wait(2);
00437         
00438         motorsSpeedSet(-300, 300);
00439         wait(2);
00440         
00441         motorsSpeedSet(0, 0);
00442         led2 = 1;
00443         wait(2);
00444         
00445         motorsSpeedSet(600, 600);
00446         wait(2);
00447         
00448         motorsSpeedSet(0, 0);
00449         led3 = 1;
00450         wait(2);
00451         
00452         motorsSpeedSet(1200, 1200);
00453         wait(2);
00454     }
00455 }
00456 
00457 void infiniteReckonerReportLoop()
00458 {
00459     Pacer reportPacer(200000);
00460     while(1)
00461     {
00462         showOrientationWithLeds34();
00463         if(reportPacer.pace())
00464         {
00465             pc.printf("%6d %6d %11d %11d | %11f %11f\r\n",
00466               reckoner.cosv, reckoner.sinv, reckoner.x, reckoner.y,
00467               determinant(), dotProduct());
00468        }
00469     }
00470 }
00471 
00472 // with should be between 0 and 63
00473 void printBar(const char * name, uint16_t result)
00474 {
00475     pc.printf("%-2s %5d |", name, result);
00476     uint16_t width = result >> 4;
00477     if (width > 63) { width = 63; }
00478     uint8_t i;
00479     for(i = 0; i < width; i++){ pc.putc('#'); }
00480     for(; i < 63; i++){ pc.putc(' '); }
00481     pc.putc('|');
00482     pc.putc('\r');
00483     pc.putc('\n');
00484 }