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 "reckoner.h"
00014 #include "buttons.h"
00015 #include "l3g.h"
00016 #include "turn_sensor.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         updateReckonerFromEncoders();
00029         loggerService();
00030     }
00031     led2 = 1;
00032     loggerReportLoop();
00033 }
00034 
00035 void testCloseness()
00036 {
00037     led1 = 1;
00038     while(1)
00039     {
00040         updateReckonerFromEncoders();
00041         float magn = magnitude();
00042         
00043         led3 = (magn < (1<<(14+7)));
00044         led4 = (magn < (1<<(14+9)));
00045     }
00046 }
00047 
00048 void showOrientationWithLeds34()
00049 {
00050     led3 = reckoner.cosv > 0;
00051     led4 = reckoner.sinv > 0;
00052 }
00053 
00054 void testTurnInPlace()
00055 {
00056     led1 = 1;
00057     while(!button1DefinitelyPressed())
00058     {
00059         updateReckonerFromEncoders();
00060         showOrientationWithLeds34();
00061     }
00062     led2 = 1;
00063     
00064     Pacer motorUpdatePacer(10000);
00065     Timer timer;
00066     timer.start();
00067     motorsSpeedSet(-300, 300);
00068     while(timer.read_ms() < 4000)
00069     {
00070         updateReckonerFromEncoders();
00071         showOrientationWithLeds34();
00072     }
00073     timer.reset();
00074     
00075     float integral = 0;
00076     while (timer.read_ms() < 4000)
00077     {
00078         if (motorUpdatePacer.pace())
00079         {
00080             int16_t rotationSpeed;
00081             float s = (float)reckoner.sinv / (1 << 30);
00082             integral += s;
00083             rotationSpeed = -(s * 2400 + integral * 20);
00084             
00085             if (rotationSpeed > 450)
00086             {
00087                 rotationSpeed = 450; 
00088             }
00089             if (rotationSpeed < -450)
00090             {
00091                 rotationSpeed = -450; 
00092             }
00093             
00094             int16_t speedLeft = -rotationSpeed;
00095             int16_t speedRight = rotationSpeed;
00096             motorsSpeedSet(speedLeft, speedRight);
00097         }
00098     }
00099     
00100     infiniteReckonerReportLoop();      
00101 }
00102 
00103 
00104 void testSensorGlitches()
00105 {
00106     AnalogIn testInput(p18);
00107     Pacer reportPacer(1000000);
00108     uint32_t badCount = 0, goodCount = 0;
00109     pc.printf("hi\r\n");
00110     
00111     //uint16_t riseCount = 0;
00112     uint16_t reading = 0xFF;
00113     
00114     while(1)
00115     {
00116         /** This digital filtering did not work
00117         {
00118             wait(0.01);
00119             uint16_t raw = testInput.read_u16();
00120             if (raw < reading)
00121             {
00122                 riseCount = 0;
00123                 reading = raw;
00124             }
00125             else
00126             {
00127                 riseCount++;
00128                 if (riseCount == 10)
00129                 {
00130                     riseCount = 0;
00131                     reading = raw;   
00132                 }
00133             }
00134         }
00135         **/
00136 
00137         uint16_t values[LINE_SENSOR_COUNT];        
00138         readSensors(values);
00139         reading = values[0];
00140         
00141         if(reading > 100)
00142         {
00143             badCount += 1;
00144             //pc.printf("f %5d %11d %11d\r\n", reading, badCount, goodCount);
00145         }
00146         else
00147         {
00148             goodCount += 1;
00149         }
00150         
00151         if (reportPacer.pace())
00152         {
00153             pc.printf("h %5d %11d %11d\r\n", reading, badCount, goodCount);
00154         }
00155     }
00156 }
00157 
00158 void testAnalog()
00159 {
00160     AnalogIn testInput(p18);
00161     
00162     DigitalOut pin20(p20);
00163     DigitalOut pin19(p19);
00164     //DigitalOut pin18(p18);
00165     DigitalOut pin17(p17);
00166     DigitalOut pin16(p16);
00167     DigitalOut pin15(p15);
00168     
00169     pin20 = 0;
00170     pin19 = 0;
00171     //pin18 = 0;
00172     pin17 = 0;
00173     pin16 = 0;
00174     pin15 = 0;
00175     
00176     uint32_t badCount = 0, goodCount = 0;
00177     
00178     Pacer reportPacer(1000000);
00179     while(1)
00180     {
00181         uint16_t reading = testInput.read_u16();
00182         if(reading > 100)
00183         {
00184             badCount += 1;
00185             pc.printf("%5d %11d %11d\r\n", reading, badCount, goodCount);   
00186         }
00187         else
00188         {
00189             goodCount += 1;   
00190         }
00191         
00192         if (reportPacer.pace())
00193         {
00194             pc.printf("Hello\r\n");   
00195         }
00196     }
00197 }
00198 
00199 // This also tests the LineTracker by printing out a lot of data from it.
00200 void testLineFollowing()
00201 {
00202     led1 = 1;   
00203     while(!button1DefinitelyPressed())
00204     {
00205         updateReckonerFromEncoders();
00206     }
00207     led2 = 1;
00208     
00209     Pacer reportPacer(200000);
00210     
00211     loadCalibration();
00212     uint16_t loopCount = 0;
00213     while(1)
00214     {
00215         updateReckonerFromEncoders();
00216         bool lineVisiblePrevious = lineTracker.getLineVisible();
00217         lineTracker.read();
00218         updateMotorsToFollowLineSlow();
00219         loopCount += 1;
00220         
00221         if (lineVisiblePrevious != lineTracker.getLineVisible())
00222         {
00223             pc.printf("%5d ! %1d %4d | %5d %5d | %4d %4d %4d\r\n",
00224                 loopCount, lineTracker.getLineVisible(), lineTracker.getLinePosition(),
00225                 motorLeftSpeed, motorRightSpeed,
00226                 lineTracker.calibratedValues[0], lineTracker.calibratedValues[1], lineTracker.calibratedValues[2]
00227                 );
00228         }
00229         
00230         if (reportPacer.pace())
00231         {
00232             pc.printf("%5d   %1d %4d | %5d %5d | %4d %4d %4d\r\n",
00233                 loopCount, lineTracker.getLineVisible(), lineTracker.getLinePosition(),
00234                 motorLeftSpeed, motorRightSpeed,
00235                 lineTracker.calibratedValues[0], lineTracker.calibratedValues[1], lineTracker.calibratedValues[2]
00236                 );
00237         }
00238     }
00239 }
00240 
00241 void testButtons()
00242 {
00243     led1 = 1;
00244     
00245     while(!button1DefinitelyReleased());
00246     while(!button1DefinitelyPressed());
00247     led2 = 1;
00248 
00249     while(!button1DefinitelyReleased());
00250     while(!button1DefinitelyPressed());
00251     led3 = 1;
00252 
00253     while(!button1DefinitelyReleased());
00254     while(!button1DefinitelyPressed());
00255     led4 = 1;
00256    
00257     while(1){};
00258 }
00259 
00260 void testReckoner()
00261 {
00262     Pacer reportPacer(100000);
00263     while(1)
00264     {
00265         updateReckonerFromEncoders();
00266         
00267         led1 = (reckoner.x > 0);
00268         led2 = (reckoner.y > 0);
00269         showOrientationWithLeds34();
00270         
00271         if (reportPacer.pace())
00272         {
00273             pc.printf("%11d %11d %11d %11d | %8d %8d %10f\r\n",
00274               reckoner.cosv, reckoner.sinv, reckoner.x, reckoner.y,
00275               encoderLeft.getCount(), encoderRight.getCount(), determinant());
00276         }
00277     }
00278 }
00279 
00280 void testLineSensors()
00281 {
00282     led1 = 1;
00283     Pacer reportPacer(100000);
00284     Pacer clearStatsPacer(2000000);
00285     
00286     uint16_t min[LINE_SENSOR_COUNT];
00287     uint16_t max[LINE_SENSOR_COUNT];
00288     
00289     bool const printBarGraph = true;
00290     while (1)
00291     {
00292         if (clearStatsPacer.pace())
00293         {
00294             for(uint8_t i = 0; i < LINE_SENSOR_COUNT; i++)
00295             {
00296                 min[i] = 0xFFFF;
00297                 max[i] = 0;
00298             }
00299         }
00300         
00301         //values[0] = lineSensorsAnalog[0].read_u16();
00302         //values[1] = lineSensorsAnalog[1].read_u16();
00303         //values[2] = lineSensorsAnalog[2].read_u16();
00304 
00305         uint16_t values[3];
00306         readSensors(values);
00307         
00308         for(uint8_t i = 0; i < LINE_SENSOR_COUNT; i++)
00309         {
00310             if (values[i] > max[i]){ max[i] = values[i]; }
00311             if (values[i] < min[i]){ min[i] = values[i]; }
00312         }
00313         
00314         if (reportPacer.pace())
00315         {
00316             if (printBarGraph)
00317             {
00318                 pc.printf("\x1B[0;0H");  // VT100 command for "go to 0,0"
00319                 printBar("L", values[0]);
00320                 printBar("M", values[1]);
00321                 printBar("R", values[2]);
00322                 pc.printf("%4d %4d    \r\n", min[0], max[0]);
00323                 pc.printf("%4d %4d    \r\n", min[1], max[1]);
00324                 pc.printf("%4d %4d    \r\n", min[2], max[2]);
00325             }
00326             else
00327             {
00328                 pc.printf("%8d %8d %8d\r\n", values[0], values[1], values[2]);
00329             }
00330         }
00331     }
00332 }
00333 
00334 // Values from David's office     Values from dev lab,
00335 // in the day time, 2014-02-27:   2014-02-27:
00336 // #  calmin calmax
00337 // 0  34872 59726                 0  40617 60222
00338 // 1  29335 60110                 1  36937 61198
00339 // 2  23845 58446                 2  33848 58862
00340 void testCalibrate()
00341 {
00342     Timer timer;
00343     timer.start();
00344     
00345     Pacer reportPacer(200000);
00346     
00347     bool doneCalibrating = false;
00348     
00349     led1 = 1;
00350     
00351     while(1)
00352     {
00353         lineTracker.read();
00354         if(!doneCalibrating)
00355         {
00356             lineTracker.updateCalibration();
00357         }
00358 
00359         led3 = doneCalibrating;
00360         led4 = lineTracker.getLineVisible();
00361         
00362         if (button1DefinitelyPressed())
00363         {
00364             doneCalibrating = true;
00365         }
00366         
00367         if (reportPacer.pace())
00368         {
00369             pc.printf("\x1B[0;0H");  // VT100 command for "go to 0,0"
00370             for(uint8_t s = 0; s < LINE_SENSOR_COUNT; s++)
00371             {
00372                 pc.printf("%-2d %5d %5d %5d\r\n", s, lineTracker.calibratedMinimum[s], lineTracker.rawValues[s], lineTracker.calibratedMaximum[s]);
00373             }
00374         }
00375     }
00376 }
00377 
00378 void testEncoders()
00379 {
00380     Pacer reportPacer(500000);
00381     led1 = 1;
00382     while(1)
00383     {
00384         while(encoderBuffer.hasEvents())
00385         {
00386             PololuEncoderEvent event = encoderBuffer.readEvent();
00387         }
00388         
00389         if(reportPacer.pace())
00390         {
00391             led2 = 1;
00392             pc.printf("%8d %8d\r\n", encoderLeft.getCount(), encoderRight.getCount());
00393             led2 = 0;
00394        }
00395     }
00396 }
00397 
00398 void testMotors()
00399 {
00400     led1 = 1;
00401     led2 = 0;
00402     led3 = 0;
00403     while(1)
00404     {
00405         motorsSpeedSet(0, 0);
00406         led2 = 0;
00407         led3 = 0;
00408         wait(2);
00409         
00410         motorsSpeedSet(300, 300);
00411         wait(2);
00412         
00413         motorsSpeedSet(-300, 300);
00414         wait(2);
00415         
00416         motorsSpeedSet(0, 0);
00417         led2 = 1;
00418         wait(2);
00419         
00420         motorsSpeedSet(600, 600);
00421         wait(1);
00422         
00423         motorsSpeedSet(0, 0);
00424         led3 = 1;
00425         wait(2);
00426         
00427         motorsSpeedSet(1200, 1200);
00428         wait(1);
00429     }
00430 }
00431 
00432 void infiniteReckonerReportLoop()
00433 {
00434     Pacer reportPacer(200000);
00435     while(1)
00436     {
00437         showOrientationWithLeds34();
00438         if(reportPacer.pace())
00439         {
00440             pc.printf("%11d %11d %11d %11d | %11f %11f\r\n",
00441               reckoner.cosv, reckoner.sinv, reckoner.x, reckoner.y,
00442               determinant(), dotProduct());
00443        }
00444     }
00445    
00446 }
00447 
00448 // with should be between 0 and 63
00449 void printBar(const char * name, uint16_t result)
00450 {
00451     pc.printf("%-2s %5d |", name, result);
00452     uint16_t width = result >> 4;
00453     if (width > 63) { width = 63; }
00454     uint8_t i;
00455     for(i = 0; i < width; i++){ pc.putc('#'); }
00456     for(; i < 63; i++){ pc.putc(' '); }
00457     pc.putc('|');
00458     pc.putc('\r');
00459     pc.putc('\n');
00460 }
00461 
00462 void testL3g()
00463 {
00464     Pacer reportPacer(750000);
00465     Timer timer;
00466     timer.start();
00467     int32_t gz = 0;
00468     bool reportedReading = false;
00469     while(1)
00470     {
00471         int32_t result = l3gZAvailable();
00472         if (result == 1)
00473         {
00474             gz = l3gZRead();
00475             reportedReading = false;
00476             if (gz > 100 || gz < -100)
00477             {
00478                 pc.printf("%d, %d\r\n", timer.read_us(), gz); 
00479                 reportedReading = true;
00480             }
00481         }
00482         else if (result != 0)
00483         {
00484             pc.printf("l3gZAvailable error: %d\n", result);
00485         }
00486 
00487         if (reportPacer.pace() && !reportedReading)
00488         {
00489             pc.printf("%d, %d\r\n", timer.read_us(), gz);
00490             reportedReading = true;
00491         }
00492     }
00493 }
00494 
00495 void testTurnSensor()
00496 {
00497     pc.printf("Test turn sensor\r\n");
00498     Pacer reportPacer(200000);
00499     TurnSensor turnSensor;
00500     turnSensor.start();
00501     while(1)
00502     {
00503         turnSensor.update();
00504         if (reportPacer.pace())
00505         {
00506             pc.printf("%d\r\n", turnSensor.getAngleDegrees());
00507         }
00508     }
00509 }
00510 
00511 void testReckoningWithGyro()
00512 {   
00513     setLeds(0, 0, 0, 1);
00514     waitForSignalToStart();
00515     setLeds(1, 0, 0, 1);
00516 
00517     loadCalibration();
00518     Timer timer;
00519     timer.start();
00520     TurnSensor turnSensor;
00521     turnSensor.start();
00522     while(1)
00523     {
00524         turnSensor.update();
00525         updateReckoner(turnSensor);
00526         loggerService();
00527         
00528         if (button1DefinitelyPressed())
00529         {
00530             break;
00531         }
00532     }
00533     motorsSpeedSet(0, 0);
00534     
00535     setLeds(1, 1, 1, 1);
00536     loggerReportLoop();
00537 }