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.h@40:6fa672be85ec, 2019-07-25 (annotated)
- Committer:
- DavidEGrayson
- Date:
- Thu Jul 25 02:53:34 2019 +0000
- Revision:
- 40:6fa672be85ec
- Child:
- 42:96671b71aac5
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 | #pragma once |
DavidEGrayson | 40:6fa672be85ec | 2 | |
DavidEGrayson | 40:6fa672be85ec | 3 | #include <mbed.h> |
DavidEGrayson | 40:6fa672be85ec | 4 | |
DavidEGrayson | 40:6fa672be85ec | 5 | class TurnSensor |
DavidEGrayson | 40:6fa672be85ec | 6 | { |
DavidEGrayson | 40:6fa672be85ec | 7 | // TODO: for production code, you would want a way to set the gyro offset |
DavidEGrayson | 40:6fa672be85ec | 8 | |
DavidEGrayson | 40:6fa672be85ec | 9 | public: |
DavidEGrayson | 40:6fa672be85ec | 10 | |
DavidEGrayson | 40:6fa672be85ec | 11 | void reset(); |
DavidEGrayson | 40:6fa672be85ec | 12 | void start(); |
DavidEGrayson | 40:6fa672be85ec | 13 | void update(); |
DavidEGrayson | 40:6fa672be85ec | 14 | |
DavidEGrayson | 40:6fa672be85ec | 15 | int32_t getAngle() |
DavidEGrayson | 40:6fa672be85ec | 16 | { |
DavidEGrayson | 40:6fa672be85ec | 17 | return (int32_t)angleUnsigned; |
DavidEGrayson | 40:6fa672be85ec | 18 | } |
DavidEGrayson | 40:6fa672be85ec | 19 | |
DavidEGrayson | 40:6fa672be85ec | 20 | uint32_t getAngleUnsigned() |
DavidEGrayson | 40:6fa672be85ec | 21 | { |
DavidEGrayson | 40:6fa672be85ec | 22 | return angleUnsigned; |
DavidEGrayson | 40:6fa672be85ec | 23 | } |
DavidEGrayson | 40:6fa672be85ec | 24 | |
DavidEGrayson | 40:6fa672be85ec | 25 | int16_t getAngleDegrees() |
DavidEGrayson | 40:6fa672be85ec | 26 | { |
DavidEGrayson | 40:6fa672be85ec | 27 | return (((int32_t)angleUnsigned >> 16) * 360) >> 16; |
DavidEGrayson | 40:6fa672be85ec | 28 | } |
DavidEGrayson | 40:6fa672be85ec | 29 | |
DavidEGrayson | 40:6fa672be85ec | 30 | int16_t getRate() |
DavidEGrayson | 40:6fa672be85ec | 31 | { |
DavidEGrayson | 40:6fa672be85ec | 32 | return rate; |
DavidEGrayson | 40:6fa672be85ec | 33 | } |
DavidEGrayson | 40:6fa672be85ec | 34 | |
DavidEGrayson | 40:6fa672be85ec | 35 | private: |
DavidEGrayson | 40:6fa672be85ec | 36 | |
DavidEGrayson | 40:6fa672be85ec | 37 | Timer timer; |
DavidEGrayson | 40:6fa672be85ec | 38 | uint32_t angleUnsigned; |
DavidEGrayson | 40:6fa672be85ec | 39 | int16_t rate; |
DavidEGrayson | 40:6fa672be85ec | 40 | uint16_t gyroLastUpdate; |
DavidEGrayson | 40:6fa672be85ec | 41 | }; |
DavidEGrayson | 40:6fa672be85ec | 42 | |
DavidEGrayson | 40:6fa672be85ec | 43 | |
DavidEGrayson | 40:6fa672be85ec | 44 | // This constant represents a turn of 45 degrees. |
DavidEGrayson | 40:6fa672be85ec | 45 | const int32_t turnAngle45 = 0x20000000; |
DavidEGrayson | 40:6fa672be85ec | 46 | |
DavidEGrayson | 40:6fa672be85ec | 47 | // This constant represents a turn of 90 degrees. |
DavidEGrayson | 40:6fa672be85ec | 48 | const int32_t turnAngle90 = turnAngle45 * 2; |
DavidEGrayson | 40:6fa672be85ec | 49 | |
DavidEGrayson | 40:6fa672be85ec | 50 | // This constant represents a turn of approximately 1 degree. |
DavidEGrayson | 40:6fa672be85ec | 51 | const int32_t turnAngle1 = (turnAngle45 + 22) / 45; |