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:
- 40:6fa672be85ec
- Child:
- 41:3ead1dd2cc3a
diff -r b19dfc5d4d4b -r 6fa672be85ec turn_sensor.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/turn_sensor.cpp Thu Jul 25 02:53:34 2019 +0000 @@ -0,0 +1,60 @@ +#include "turn_sensor.h" +#include "l3g.h" + +void TurnSensor::reset() +{ + angleUnsigned = 0; +} + +void TurnSensor::start() +{ + timer.start(); + rate = 0; + angleUnsigned = 0; + gyroLastUpdate = timer.read_us(); +} + +void TurnSensor::update() +{ + if (l3gZAvailable() == 1) + { + int32_t gz = l3gZRead(); + if (gz < -500000) + { + // error + return; + } + + // 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; + + rate = gz; + + // First figure out how much time has passed since the last update (dt) + uint16_t m = timer.read_us(); + uint16_t dt = m - gyroLastUpdate; + gyroLastUpdate = m; + + // Multiply dt by turnRate in order to get an estimation of how + // much the robot has turned since the last update. + // (angular change = angular velocity * time) + int32_t d = (int32_t)rate * dt; + + // The units of d are gyro digits times microseconds. We need + // to convert those to the units of turnAngle, where 2^29 units + // represents 45 degrees. The conversion from gyro digits to + // degrees per second (dps) is determined by the sensitivity of + // the gyro: 0.07 degrees per second per digit. + // + // (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; + } +} \ No newline at end of file