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@47:9773dc14c834, 2019-07-31 (annotated)
- Committer:
- DavidEGrayson
- Date:
- Wed Jul 31 07:14:09 2019 +0000
- Revision:
- 47:9773dc14c834
- Parent:
- 42:96671b71aac5
- Child:
- 48:597738b77f77
Got some awesome results using carefully calculated scale factors for the gyro!
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 | 42:96671b71aac5 | 21 | int32_t gz = l3gZReadCalibrated(); |
DavidEGrayson | 40:6fa672be85ec | 22 | if (gz < -500000) |
DavidEGrayson | 40:6fa672be85ec | 23 | { |
DavidEGrayson | 40:6fa672be85ec | 24 | // error |
DavidEGrayson | 40:6fa672be85ec | 25 | return; |
DavidEGrayson | 42:96671b71aac5 | 26 | } |
DavidEGrayson | 41:3ead1dd2cc3a | 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 | 41:3ead1dd2cc3a | 30 | gz = -gz; |
DavidEGrayson | 41:3ead1dd2cc3a | 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 | |
DavidEGrayson | 47:9773dc14c834 | 53 | if (rate > 0) |
DavidEGrayson | 47:9773dc14c834 | 54 | { |
DavidEGrayson | 47:9773dc14c834 | 55 | // Counter-clockwise scale factor calculated from log_190730_2.csv. |
DavidEGrayson | 47:9773dc14c834 | 56 | const double scale = 1.012394298; |
DavidEGrayson | 47:9773dc14c834 | 57 | angleUnsigned += (int64_t)d * (int32_t)(14680064 * scale) / 17578125; |
DavidEGrayson | 47:9773dc14c834 | 58 | } |
DavidEGrayson | 47:9773dc14c834 | 59 | else |
DavidEGrayson | 47:9773dc14c834 | 60 | { |
DavidEGrayson | 47:9773dc14c834 | 61 | // Clockwise scale factor calculated from log_190730_3.csv. |
DavidEGrayson | 47:9773dc14c834 | 62 | const double scale = 0.992376154; |
DavidEGrayson | 47:9773dc14c834 | 63 | angleUnsigned += (int64_t)d * (int32_t)(14680064 * scale) / 17578125; |
DavidEGrayson | 47:9773dc14c834 | 64 | } |
DavidEGrayson | 40:6fa672be85ec | 65 | } |
DavidEGrayson | 40:6fa672be85ec | 66 | } |