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
turn_sensor.cpp@41:3ead1dd2cc3a, 2019-07-25 (annotated)
- Committer:
- DavidEGrayson
- Date:
- Thu Jul 25 03:20:41 2019 +0000
- Revision:
- 41:3ead1dd2cc3a
- Parent:
- 40:6fa672be85ec
- Child:
- 42:96671b71aac5
Gyro: Add a hacky offset of +6.5 in TurnSensor.cpp. The turn sensor drifts much more slowly now, like maybe 1 degree per minute.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
DavidEGrayson | 40:6fa672be85ec | 1 | #include "turn_sensor.h" |
DavidEGrayson | 40:6fa672be85ec | 2 | #include "l3g.h" |
DavidEGrayson | 40:6fa672be85ec | 3 | |
DavidEGrayson | 40:6fa672be85ec | 4 | void TurnSensor::reset() |
DavidEGrayson | 40:6fa672be85ec | 5 | { |
DavidEGrayson | 40:6fa672be85ec | 6 | angleUnsigned = 0; |
DavidEGrayson | 40:6fa672be85ec | 7 | } |
DavidEGrayson | 40:6fa672be85ec | 8 | |
DavidEGrayson | 40:6fa672be85ec | 9 | void TurnSensor::start() |
DavidEGrayson | 40:6fa672be85ec | 10 | { |
DavidEGrayson | 40:6fa672be85ec | 11 | timer.start(); |
DavidEGrayson | 40:6fa672be85ec | 12 | rate = 0; |
DavidEGrayson | 40:6fa672be85ec | 13 | angleUnsigned = 0; |
DavidEGrayson | 40:6fa672be85ec | 14 | gyroLastUpdate = timer.read_us(); |
DavidEGrayson | 40:6fa672be85ec | 15 | } |
DavidEGrayson | 40:6fa672be85ec | 16 | |
DavidEGrayson | 40:6fa672be85ec | 17 | void TurnSensor::update() |
DavidEGrayson | 40:6fa672be85ec | 18 | { |
DavidEGrayson | 40:6fa672be85ec | 19 | if (l3gZAvailable() == 1) |
DavidEGrayson | 40:6fa672be85ec | 20 | { |
DavidEGrayson | 40:6fa672be85ec | 21 | int32_t gz = l3gZRead(); |
DavidEGrayson | 40:6fa672be85ec | 22 | if (gz < -500000) |
DavidEGrayson | 40:6fa672be85ec | 23 | { |
DavidEGrayson | 40:6fa672be85ec | 24 | // error |
DavidEGrayson | 40:6fa672be85ec | 25 | return; |
DavidEGrayson | 40:6fa672be85ec | 26 | } |
DavidEGrayson | 40:6fa672be85ec | 27 | |
DavidEGrayson | 41:3ead1dd2cc3a | 28 | // The gyro zero rate on my robot, measured by testL3gAndCalibrate() on |
DavidEGrayson | 41:3ead1dd2cc3a | 29 | // 2019-07-24 is about -6.5. So let's add 6 half the time and add 7 |
DavidEGrayson | 41:3ead1dd2cc3a | 30 | // the other half of the time. This is a big hack. |
DavidEGrayson | 41:3ead1dd2cc3a | 31 | static uint8_t updateCount = 0; |
DavidEGrayson | 41:3ead1dd2cc3a | 32 | updateCount++; |
DavidEGrayson | 41:3ead1dd2cc3a | 33 | gz = gz + 6 + (updateCount & 1); |
DavidEGrayson | 41:3ead1dd2cc3a | 34 | |
DavidEGrayson | 40:6fa672be85ec | 35 | // The gyro on this robot is mounted upside down; account for that here so that |
DavidEGrayson | 40:6fa672be85ec | 36 | // we can have counter-clockwise be a positive rotation. |
DavidEGrayson | 41:3ead1dd2cc3a | 37 | gz = -gz; |
DavidEGrayson | 41:3ead1dd2cc3a | 38 | |
DavidEGrayson | 40:6fa672be85ec | 39 | rate = gz; |
DavidEGrayson | 40:6fa672be85ec | 40 | |
DavidEGrayson | 40:6fa672be85ec | 41 | // First figure out how much time has passed since the last update (dt) |
DavidEGrayson | 40:6fa672be85ec | 42 | uint16_t m = timer.read_us(); |
DavidEGrayson | 40:6fa672be85ec | 43 | uint16_t dt = m - gyroLastUpdate; |
DavidEGrayson | 40:6fa672be85ec | 44 | gyroLastUpdate = m; |
DavidEGrayson | 40:6fa672be85ec | 45 | |
DavidEGrayson | 40:6fa672be85ec | 46 | // Multiply dt by turnRate in order to get an estimation of how |
DavidEGrayson | 40:6fa672be85ec | 47 | // much the robot has turned since the last update. |
DavidEGrayson | 40:6fa672be85ec | 48 | // (angular change = angular velocity * time) |
DavidEGrayson | 40:6fa672be85ec | 49 | int32_t d = (int32_t)rate * dt; |
DavidEGrayson | 40:6fa672be85ec | 50 | |
DavidEGrayson | 40:6fa672be85ec | 51 | // The units of d are gyro digits times microseconds. We need |
DavidEGrayson | 40:6fa672be85ec | 52 | // to convert those to the units of turnAngle, where 2^29 units |
DavidEGrayson | 40:6fa672be85ec | 53 | // represents 45 degrees. The conversion from gyro digits to |
DavidEGrayson | 40:6fa672be85ec | 54 | // degrees per second (dps) is determined by the sensitivity of |
DavidEGrayson | 40:6fa672be85ec | 55 | // the gyro: 0.07 degrees per second per digit. |
DavidEGrayson | 40:6fa672be85ec | 56 | // |
DavidEGrayson | 40:6fa672be85ec | 57 | // (0.07 dps/digit) * (1/1000000 s/us) * (2^29/45 unit/degree) |
DavidEGrayson | 40:6fa672be85ec | 58 | // = 14680064/17578125 unit/(digit*us) |
DavidEGrayson | 40:6fa672be85ec | 59 | |
DavidEGrayson | 40:6fa672be85ec | 60 | angleUnsigned += (int64_t)d * 14680064 / 17578125; |
DavidEGrayson | 40:6fa672be85ec | 61 | } |
DavidEGrayson | 40:6fa672be85ec | 62 | } |