David's dead reckoning code for the LVBots competition on March 6th. Uses the mbed LPC1768, DRV8835, QTR-3RC, and two DC motors with encoders.
Dependencies: PololuEncoder Pacer mbed GeneralDebouncer
Diff: test.cpp
- Revision:
- 43:0e985a58f174
- Parent:
- 42:96671b71aac5
- Child:
- 44:b4a00fbab06b
--- a/test.cpp Sat Jul 27 20:58:46 2019 +0000 +++ b/test.cpp Sat Jul 27 22:52:19 2019 +0000 @@ -25,7 +25,7 @@ { led3 = logger.isFull(); - updateReckonerFromEncoders(); + updateReckoner(); loggerService(); } led2 = 1; @@ -34,29 +34,31 @@ void testCloseness() { + doGyroCalibration(); led1 = 1; while(1) { - updateReckonerFromEncoders(); + updateReckoner(); float magn = magnitude(); - led3 = (magn < (1<<(14+7))); - led4 = (magn < (1<<(14+9))); + led3 = magn < (1 << 5); + led4 = magn < (1 << 7); } } void showOrientationWithLeds34() { - led3 = reckoner.cos > 0; - led4 = reckoner.sin > 0; + led3 = reckoner.cosv > 0; + led4 = reckoner.sinv > 0; } void testTurnInPlace() { + doGyroCalibration(); led1 = 1; while(!button1DefinitelyPressed()) { - updateReckonerFromEncoders(); + updateReckoner(); showOrientationWithLeds34(); } led2 = 1; @@ -67,7 +69,7 @@ motorsSpeedSet(-300, 300); while(timer.read_ms() < 4000) { - updateReckonerFromEncoders(); + updateReckoner(); showOrientationWithLeds34(); } timer.reset(); @@ -78,7 +80,7 @@ if (motorUpdatePacer.pace()) { int16_t rotationSpeed; - float s = (float)reckoner.sin / (1 << 30); + float s = (float)reckoner.sinv / (1 << RECKONER_LOG_UNIT); integral += s; rotationSpeed = -(s * 2400 + integral * 20); @@ -100,109 +102,13 @@ infiniteReckonerReportLoop(); } - -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; - } - } - } - **/ - - uint16_t values[LINE_SENSOR_COUNT]; - readSensors(values); - reading = values[0]; - - 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(); + updateReckoner(); } led2 = 1; @@ -212,7 +118,7 @@ uint16_t loopCount = 0; while(1) { - updateReckonerFromEncoders(); + updateReckoner(); bool lineVisiblePrevious = lineTracker.getLineVisible(); lineTracker.read(); updateMotorsToFollowLine(); @@ -241,10 +147,11 @@ void testDriveHome() { + doGyroCalibration(); led1 = 1; while(!button1DefinitelyPressed()) { - updateReckonerFromEncoders(); + updateReckoner(); } setLeds(1, 0, 1, 0); @@ -259,10 +166,11 @@ void testFinalSettleIn() { + doGyroCalibration(); led1 = 1; while(!button1DefinitelyPressed()) { - updateReckonerFromEncoders(); + updateReckoner(); } finalSettleIn(); infiniteReckonerReportLoop(); @@ -292,19 +200,21 @@ { doGyroCalibration(); led1 = 1; + turnSensor.start(); Pacer reportPacer(100000); while(1) { - updateReckonerFromEncoders(); + turnSensor.update(); + updateReckoner(); - led1 = (reckoner.x > 0); - led2 = (reckoner.y > 0); + led1 = reckoner.x > 0; + led2 = reckoner.y > 0; showOrientationWithLeds34(); if (reportPacer.pace()) { - pc.printf("%11d %11d %11d %11d | %8d %8d %10f\r\n", - reckoner.cos, reckoner.sin, reckoner.x, reckoner.y, + pc.printf("%6d %6d %11d %11d | %8d %8d %10f\r\n", + reckoner.cosv, reckoner.sinv, reckoner.x, reckoner.y, encoderLeft.getCount(), encoderRight.getCount(), determinant()); } } @@ -313,12 +223,11 @@ void testTurnSensor() { pc.printf("Test turn sensor\r\n"); + doGyroCalibration(); led1 = 1; - doGyroCalibration(); //Pacer reportPacer(200000); // 0.2 s Pacer reportPacer(10000000); // 10 s Timer timer; - led2 = 1; timer.start(); turnSensor.start(); while(1) @@ -430,10 +339,6 @@ } } - //values[0] = lineSensorsAnalog[0].read_u16(); - //values[1] = lineSensorsAnalog[1].read_u16(); - //values[2] = lineSensorsAnalog[2].read_u16(); - uint16_t values[3]; readSensors(values); @@ -589,8 +494,8 @@ showOrientationWithLeds34(); if(reportPacer.pace()) { - pc.printf("%11d %11d %11d %11d | %11f %11f\r\n", - reckoner.cos, reckoner.sin, reckoner.x, reckoner.y, + pc.printf("%6d %6d %11d %11d | %11f %11f\r\n", + reckoner.cosv, reckoner.sinv, reckoner.x, reckoner.y, determinant(), dotProduct()); } }