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-03-04
- Revision:
- 31:739b91331f31
- Parent:
- 30:84be2d602dc0
- Child:
- 32:83a13b06093c
File content as of revision 31:739b91331f31:
// A file for testing routines that will not be used in the final firmware. #include <mbed.h> #include "motors.h" #include <Pacer.h> #include "main.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); uint16_t readPin18() { // Set PCADC bit in PCONP register. (Table 46). LPC_SC->PCONP |= (1 << 12); // Select PCLK_ADC in PCLKSEL0 register. (Table 40, Table 531). LPC_SC->PCLKSEL0 |= (3 << 24); // PCLK for ADC = CCLK/8 // Enable ADC0 through PINSEL registers. (Section 8.5). LPC_PINCON->PINSEL1 = (LPC_PINCON->PINSEL1 & ~(3 << 20)) | (1 << 20); // Pin 18: P0.26/AD0.3/AOUT/RXD3 // 7:0 Bitmap to select what channel to use // 15:8 Select clock. // 16 BURST = 0 // 20:17 Reserved // 21 PDN = 1, A/D converter is operational // 26:24 START = 001 to start conversion now LPC_ADC->ADCR = (1 << 3) | (0xFF << 8) | (1 << 21); LPC_ADC->ADCR |= (1 << 24); while(!(LPC_ADC->ADGDR >> 31 & 1)) // while not done { } //return 2; return LPC_ADC->ADGDR & 0xFFFF; } uint16_t readP10() { DigitalInOut pin(p10); pin.mode(PullNone); pin.output(); pin = 1; wait_us(20); uint16_t value = 1000; Timer timer; timer.start(); pin.input(); while(timer.read_us() < 1000) { if(pin.read() == 0) { return timer.read_us(); } } return value; } void testSensorGlitches() { AnalogIn testInput(p18); Pacer reportPacer(1000000); uint32_t badCount = 0, goodCount = 0; pc.printf("hi\r\n"); //uint16_t riseCount = 0; uint16_t reading = 0xFF; while(1) { /** This digital filtering did not work { wait(0.01); uint16_t raw = testInput.read_u16(); if (raw < reading) { riseCount = 0; reading = raw; } else { riseCount++; if (riseCount == 10) { riseCount = 0; reading = raw; } } } **/ reading = readP10(); if(reading > 100) { badCount += 1; //pc.printf("f %5d %11d %11d\r\n", reading, badCount, goodCount); } else { goodCount += 1; } if (reportPacer.pace()) { pc.printf("h %5d %11d %11d\r\n", reading, badCount, goodCount); } } } void testAnalog() { AnalogIn testInput(p18); DigitalOut pin20(p20); DigitalOut pin19(p19); //DigitalOut pin18(p18); DigitalOut pin17(p17); DigitalOut pin16(p16); DigitalOut pin15(p15); pin20 = 0; pin19 = 0; //pin18 = 0; pin17 = 0; pin16 = 0; pin15 = 0; uint32_t badCount = 0, goodCount = 0; Pacer reportPacer(1000000); while(1) { uint16_t reading = testInput.read_u16(); if(reading > 100) { badCount += 1; pc.printf("%5d %11d %11d\r\n", reading, badCount, goodCount); } else { goodCount += 1; } if (reportPacer.pace()) { pc.printf("Hello\r\n"); } } } // This also tests the LineTracker by printing out a lot of data from it. void testLineFollowing() { led1 = 1; while(!button1DefinitelyPressed()) { updateReckonerFromEncoders(); } led2 = 1; Pacer reportPacer(200000); loadCalibration(); uint16_t loopCount = 0; while(1) { updateReckonerFromEncoders(); bool lineVisiblePrevious = lineTracker.getLineVisible(); lineTracker.read(); updateMotorsToFollowLine(); loopCount += 1; if (lineVisiblePrevious != lineTracker.getLineVisible()) { pc.printf("%5d ! %1d %4d | %4d %4d %4d | %5d %5d %5d\r\n", loopCount, lineTracker.getLineVisible(), lineTracker.getLinePosition(), lineTracker.calibratedValues[0], lineTracker.calibratedValues[1], lineTracker.calibratedValues[2], lineTracker.rawValues[0], lineTracker.rawValues[1], lineTracker.rawValues[2] ); } if (reportPacer.pace()) { pc.printf("%5d %1d %4d | %4d %4d %4d\r\n", loopCount, lineTracker.getLineVisible(), lineTracker.getLinePosition(), lineTracker.calibratedValues[0], lineTracker.calibratedValues[1], lineTracker.calibratedValues[2] ); } } } void testDriveHome() { led1 = 1; while(!button1DefinitelyPressed()) { updateReckonerFromEncoders(); } driveHomeAlmost(); finalSettleIn(); 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); Pacer clearStatsPacer(2000000); uint16_t min[LINE_SENSOR_COUNT]; uint16_t max[LINE_SENSOR_COUNT]; bool const printBarGraph = true; while (1) { if (clearStatsPacer.pace()) { for(uint8_t i = 0; i < LINE_SENSOR_COUNT; i++) { min[i] = 0xFFFF; max[i] = 0; } } //values[0] = lineSensorsAnalog[0].read_u16(); //values[1] = lineSensorsAnalog[1].read_u16(); //values[2] = lineSensorsAnalog[2].read_u16(); uint16_t values[3]; readSensors(values); for(uint8_t i = 0; i < LINE_SENSOR_COUNT; i++) { if (values[i] > max[i]){ max[i] = values[i]; } if (values[i] < min[i]){ min[i] = values[i]; } } if (reportPacer.pace()) { if (printBarGraph) { pc.printf("\x1B[0;0H"); // VT100 command for "go to 0,0" printBar("L", values[0]); printBar("M", values[1]); printBar("R", values[2]); pc.printf("%4d %4d \r\n", min[0], max[0]); pc.printf("%4d %4d \r\n", min[1], max[1]); pc.printf("%4d %4d \r\n", min[2], max[2]); } else { pc.printf("%8d %8d %8d\r\n", values[0], values[1], values[2]); } } } } // Values from David's office Values from dev lab, // in the day time, 2014-02-27: 2014-02-27: // # calmin calmax // 0 34872 59726 0 40617 60222 // 1 29335 60110 1 36937 61198 // 2 23845 58446 2 33848 58862 void testCalibrate() { Timer timer; timer.start(); Pacer reportPacer(200000); bool doneCalibrating = false; led1 = 1; while(1) { lineTracker.read(); if(!doneCalibrating) { lineTracker.updateCalibration(); } led2 = calibrationLooksGood(); led3 = doneCalibrating; led4 = lineTracker.getLineVisible(); if (button1DefinitelyPressed()) { doneCalibrating = true; } if (reportPacer.pace()) { pc.printf("\x1B[0;0H"); // VT100 command for "go to 0,0" for(uint8_t s = 0; s < LINE_SENSOR_COUNT; s++) { pc.printf("%-2d %5d %5d %5d\r\n", s, lineTracker.calibratedMinimum[s], lineTracker.rawValues[s], lineTracker.calibratedMaximum[s]); } if (calibrationLooksGood()) { pc.puts("Good. \r\n"); } else { pc.puts("Not good yet.\r\n"); } } } } 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\r\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; } } } // with should be between 0 and 63 void printBar(const char * name, uint16_t result) { pc.printf("%-2s %5d |", name, result); uint16_t width = result >> 4; if (width > 63) { width = 63; } uint8_t i; for(i = 0; i < width; i++){ pc.putc('#'); } for(; i < 63; i++){ pc.putc(' '); } pc.putc('|'); pc.putc('\r'); pc.putc('\n'); }