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
Revision 47:9773dc14c834, committed 2019-07-31
- Comitter:
- DavidEGrayson
- Date:
- Wed Jul 31 07:14:09 2019 +0000
- Parent:
- 46:df2c2d25c070
- Child:
- 48:597738b77f77
- Commit message:
- Got some awesome results using carefully calculated scale factors for the gyro!
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
turn_sensor.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Sun Jul 28 22:20:12 2019 +0000 +++ b/main.cpp Wed Jul 31 07:14:09 2019 +0000 @@ -15,7 +15,7 @@ #include "l3g.h" #include "turn_sensor.h" -void doDeadReckoning(); +void __attribute__((noreturn)) doDeadReckoning(); Reckoner reckoner; LineTracker lineTracker; @@ -24,10 +24,13 @@ uint32_t totalEncoderCounts = 0; uint32_t nextLogEncoderCount = 0; -const uint32_t logSpacing = 100; +const uint32_t logSpacing = 250; const int16_t drivingSpeed = 400; +const uint32_t timeout = 1*60000; + + void setLeds(bool v1, bool v2, bool v3, bool v4) { led1 = v1; @@ -306,9 +309,14 @@ bool lostLine = lineStatus.getState() == false && lineStatus.getTimeInCurrentStateMicroseconds() > lineDebounceTime; - if(lostLine && timer.read_us() >= 2000000) + if (lostLine && timer.read_ms() >= 2000) { - break; + break; + } + + if (timeout && timer.read_ms() > timeout) + { + break; } updateMotorsToFollowLine();
--- a/turn_sensor.cpp Sun Jul 28 22:20:12 2019 +0000 +++ b/turn_sensor.cpp Wed Jul 31 07:14:09 2019 +0000 @@ -50,6 +50,17 @@ // (0.07 dps/digit) * (1/1000000 s/us) * (2^29/45 unit/degree) // = 14680064/17578125 unit/(digit*us) - angleUnsigned += (int64_t)d * 14680064 / 17578125; + if (rate > 0) + { + // Counter-clockwise scale factor calculated from log_190730_2.csv. + const double scale = 1.012394298; + angleUnsigned += (int64_t)d * (int32_t)(14680064 * scale) / 17578125; + } + else + { + // Clockwise scale factor calculated from log_190730_3.csv. + const double scale = 0.992376154; + angleUnsigned += (int64_t)d * (int32_t)(14680064 * scale) / 17578125; + } } } \ No newline at end of file