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@40:6fa672be85ec, 2019-07-25 (annotated)
- Committer:
- DavidEGrayson
- Date:
- Thu Jul 25 02:53:34 2019 +0000
- Revision:
- 40:6fa672be85ec
- Child:
- 41:3ead1dd2cc3a
Add TurnSensor and L3G code but I am not happy with how the Gyro drifts a degree every few seconds or so.
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 | 40:6fa672be85ec | 28 | // The gyro on this robot is mounted upside down; account for that here so that |
DavidEGrayson | 40:6fa672be85ec | 29 | // we can have counter-clockwise be a positive rotation. |
DavidEGrayson | 40:6fa672be85ec | 30 | gz = -gz; |
DavidEGrayson | 40:6fa672be85ec | 31 | |
DavidEGrayson | 40:6fa672be85ec | 32 | rate = gz; |
DavidEGrayson | 40:6fa672be85ec | 33 | |
DavidEGrayson | 40:6fa672be85ec | 34 | // First figure out how much time has passed since the last update (dt) |
DavidEGrayson | 40:6fa672be85ec | 35 | uint16_t m = timer.read_us(); |
DavidEGrayson | 40:6fa672be85ec | 36 | uint16_t dt = m - gyroLastUpdate; |
DavidEGrayson | 40:6fa672be85ec | 37 | gyroLastUpdate = m; |
DavidEGrayson | 40:6fa672be85ec | 38 | |
DavidEGrayson | 40:6fa672be85ec | 39 | // Multiply dt by turnRate in order to get an estimation of how |
DavidEGrayson | 40:6fa672be85ec | 40 | // much the robot has turned since the last update. |
DavidEGrayson | 40:6fa672be85ec | 41 | // (angular change = angular velocity * time) |
DavidEGrayson | 40:6fa672be85ec | 42 | int32_t d = (int32_t)rate * dt; |
DavidEGrayson | 40:6fa672be85ec | 43 | |
DavidEGrayson | 40:6fa672be85ec | 44 | // The units of d are gyro digits times microseconds. We need |
DavidEGrayson | 40:6fa672be85ec | 45 | // to convert those to the units of turnAngle, where 2^29 units |
DavidEGrayson | 40:6fa672be85ec | 46 | // represents 45 degrees. The conversion from gyro digits to |
DavidEGrayson | 40:6fa672be85ec | 47 | // degrees per second (dps) is determined by the sensitivity of |
DavidEGrayson | 40:6fa672be85ec | 48 | // the gyro: 0.07 degrees per second per digit. |
DavidEGrayson | 40:6fa672be85ec | 49 | // |
DavidEGrayson | 40:6fa672be85ec | 50 | // (0.07 dps/digit) * (1/1000000 s/us) * (2^29/45 unit/degree) |
DavidEGrayson | 40:6fa672be85ec | 51 | // = 14680064/17578125 unit/(digit*us) |
DavidEGrayson | 40:6fa672be85ec | 52 | //const float factor = (float)14680064 / 17578125; |
DavidEGrayson | 40:6fa672be85ec | 53 | |
DavidEGrayson | 40:6fa672be85ec | 54 | // Fudge factor to account for the fact that the gyro might be mounted |
DavidEGrayson | 40:6fa672be85ec | 55 | // at a bad angle or it might be more or less sensitive than expected. |
DavidEGrayson | 40:6fa672be85ec | 56 | //const float fudge = 0.98809906722; |
DavidEGrayson | 40:6fa672be85ec | 57 | |
DavidEGrayson | 40:6fa672be85ec | 58 | angleUnsigned += (int64_t)d * 14680064 / 17578125; |
DavidEGrayson | 40:6fa672be85ec | 59 | } |
DavidEGrayson | 40:6fa672be85ec | 60 | } |