David's line following code from the LVBots competition, 2015.
Dependencies: GeneralDebouncer Pacer PololuEncoder mbed
Fork of DeadReckoning by
test.cpp
- Committer:
- DavidEGrayson
- Date:
- 2014-02-27
- Revision:
- 20:dbec34f0e76b
- Parent:
- 19:a11ffc903774
- Child:
- 21:c279c6a83671
File content as of revision 20:dbec34f0e76b:
// A file for testing routines that will not be used in the final firmware. #include <mbed.h> #include "motors.h" #include <Pacer.h> #include "test.h" #include "leds.h" #include "encoders.h" #include "pc_serial.h" #include "line_sensors.h" #include "reckoner.h" #include "buttons.h" void __attribute__((noreturn)) infiniteReckonerReportLoop(); void printBar(const char * name, uint16_t adcResult); void testDriveHome() { led1 = 1; while(!button1DefinitelyPressed()) { updateReckonerFromEncoders(); } driveHome(); infiniteReckonerReportLoop(); } void testFinalSettleIn() { led1 = 1; while(!button1DefinitelyPressed()) { updateReckonerFromEncoders(); } finalSettleIn(); infiniteReckonerReportLoop(); } void testButtons() { led1 = 1; while(!button1DefinitelyReleased()); while(!button1DefinitelyPressed()); led2 = 1; while(!button1DefinitelyReleased()); while(!button1DefinitelyPressed()); led3 = 1; while(!button1DefinitelyReleased()); while(!button1DefinitelyPressed()); led4 = 1; while(1){}; } void testReckoner() { Pacer reportPacer(100000); while(1) { updateReckonerFromEncoders(); led1 = (reckoner.cos > 0); led2 = (reckoner.sin > 0); led3 = (reckoner.x > 0); led4 = (reckoner.y > 0); if (reportPacer.pace()) { pc.printf("%11d %11d %11d %11d | %8d %8d %10f\r\n", reckoner.cos, reckoner.sin, reckoner.x, reckoner.y, encoderLeft.getCount(), encoderRight.getCount(), determinant()); } } } void testLineSensors() { led1 = 1; Pacer reportPacer(100000); bool const printBarGraph = true; while (1) { if (reportPacer.pace()) { uint16_t left = lineSensorsAnalog[0].read_u16(); uint16_t middle = lineSensorsAnalog[1].read_u16(); uint16_t right = lineSensorsAnalog[2].read_u16(); if (printBarGraph) { pc.printf("\x1B[0;0H"); // VT100 command for "go to 0,0" printBar("L", left); printBar("M", middle); printBar("R", right); } else { pc.printf("%8d %8d %8d\n", left, middle, right); } } } } void testEncoders() { Pacer reportPacer(500000); led1 = 1; while(1) { while(encoderBuffer.hasEvents()) { PololuEncoderEvent event = encoderBuffer.readEvent(); } if(reportPacer.pace()) { led2 = 1; pc.printf("%8d %8d\n", encoderLeft.getCount(), encoderRight.getCount()); led2 = 0; } } } void testMotors() { led1 = 1; led2 = 0; led3 = 0; while(1) { motorsSpeedSet(0, 0); led2 = 0; led3 = 0; wait(2); motorsSpeedSet(300, 300); wait(2); motorsSpeedSet(-300, 300); wait(2); motorsSpeedSet(0, 0); led2 = 1; wait(2); motorsSpeedSet(600, 600); wait(2); motorsSpeedSet(0, 0); led3 = 1; wait(2); motorsSpeedSet(1200, 1200); wait(2); } } void infiniteReckonerReportLoop() { Pacer reportPacer(200000); while(1) { if(reportPacer.pace()) { led4 = 1; pc.printf("%11d %11d %11d %11d | %11f %11f\r\n", reckoner.cos, reckoner.sin, reckoner.x, reckoner.y, determinant(), dotProduct()); led4 = 0; } } } void printBar(const char * name, uint16_t adcResult) { pc.printf("%-2s %5d |", name, adcResult); uint8_t width = adcResult >> 10; uint8_t i; for(i = 0; i < width; i++){ pc.putc('#'); } for(; i < 63; i++){ pc.putc(' '); } pc.putc('|'); pc.putc('\n'); }