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: turn_sensor.cpp
- Revision:
- 41:3ead1dd2cc3a
- Parent:
- 40:6fa672be85ec
- Child:
- 42:96671b71aac5
--- a/turn_sensor.cpp Thu Jul 25 02:53:34 2019 +0000 +++ b/turn_sensor.cpp Thu Jul 25 03:20:41 2019 +0000 @@ -25,10 +25,17 @@ return; } + // The gyro zero rate on my robot, measured by testL3gAndCalibrate() on + // 2019-07-24 is about -6.5. So let's add 6 half the time and add 7 + // the other half of the time. This is a big hack. + static uint8_t updateCount = 0; + updateCount++; + gz = gz + 6 + (updateCount & 1); + // The gyro on this robot is mounted upside down; account for that here so that // we can have counter-clockwise be a positive rotation. - gz = -gz; - + gz = -gz; + rate = gz; // First figure out how much time has passed since the last update (dt) @@ -49,11 +56,6 @@ // // (0.07 dps/digit) * (1/1000000 s/us) * (2^29/45 unit/degree) // = 14680064/17578125 unit/(digit*us) - //const float factor = (float)14680064 / 17578125; - - // Fudge factor to account for the fact that the gyro might be mounted - // at a bad angle or it might be more or less sensitive than expected. - //const float fudge = 0.98809906722; angleUnsigned += (int64_t)d * 14680064 / 17578125; }