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: main.cpp
- Revision:
- 40:6fa672be85ec
- Parent:
- 39:b19dfc5d4d4b
- Child:
- 41:3ead1dd2cc3a
diff -r b19dfc5d4d4b -r 6fa672be85ec main.cpp --- a/main.cpp Thu Jul 25 02:11:25 2019 +0000 +++ b/main.cpp Thu Jul 25 02:53:34 2019 +0000 @@ -12,11 +12,17 @@ #include "reckoner.h" #include "buttons.h" #include "line_tracker.h" +#include "l3g.h" +#include "turn_sensor.h" Reckoner reckoner; LineTracker lineTracker; +TurnSensor turnSensor; Logger logger; -Pacer loggerPacer(50000); + +uint32_t totalEncoderCounts = 0; +uint32_t nextLogEncoderCount = 0; +const uint32_t logSpacing = 100; const int16_t drivingSpeed = 400; @@ -32,6 +38,13 @@ { pc.baud(115200); + if (l3gInit()) + { + // Error initializing the gyro. + setLeds(0, 0, 1, 1); + while(1); + } + // Enable pull-ups on encoder pins and give them a chance to settle. encodersInit(); motorsInit(); @@ -42,6 +55,8 @@ //testEncoders(); //testMotorSpeed(); //testLineSensors(); + //testL3g(); + testTurnSensor(); //testReckoner(); //testButtons(); //testDriveHome(); @@ -82,9 +97,15 @@ void loggerService() { - if (loggerPacer.pace()) + if (totalEncoderCounts > nextLogEncoderCount) { - logger.log(); + nextLogEncoderCount += logSpacing; + + struct LogEntry entry; + entry.turnAngle = turnSensor.getAngle() >> 16; + entry.x = reckoner.x >> 16; + entry.y = reckoner.y >> 16; + logger.log(&entry); } } @@ -129,15 +150,19 @@ { case ENCODER_LEFT | POLOLU_ENCODER_EVENT_INC: reckoner.handleTickLeftForward(); + totalEncoderCounts++; break; case ENCODER_LEFT | POLOLU_ENCODER_EVENT_DEC: reckoner.handleTickLeftBackward(); + totalEncoderCounts--; break; case ENCODER_RIGHT | POLOLU_ENCODER_EVENT_INC: reckoner.handleTickRightForward(); + totalEncoderCounts++; break; case ENCODER_RIGHT | POLOLU_ENCODER_EVENT_DEC: reckoner.handleTickRightBackward(); + totalEncoderCounts--; break; } }