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@48:597738b77f77, 2019-08-13 (annotated)
- Committer:
- DavidEGrayson
- Date:
- Tue Aug 13 21:21:17 2019 +0000
- Revision:
- 48:597738b77f77
- Parent:
- 42:96671b71aac5
Changes from before the contest, I think.
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 | 42:96671b71aac5 | 30 | int32_t getAngleMillidegrees() |
DavidEGrayson | 42:96671b71aac5 | 31 | { |
DavidEGrayson | 42:96671b71aac5 | 32 | return ((int64_t)(int32_t)angleUnsigned * 360000) >> 32; |
DavidEGrayson | 42:96671b71aac5 | 33 | } |
DavidEGrayson | 42:96671b71aac5 | 34 | |
DavidEGrayson | 40:6fa672be85ec | 35 | int16_t getRate() |
DavidEGrayson | 40:6fa672be85ec | 36 | { |
DavidEGrayson | 40:6fa672be85ec | 37 | return rate; |
DavidEGrayson | 40:6fa672be85ec | 38 | } |
DavidEGrayson | 40:6fa672be85ec | 39 | |
DavidEGrayson | 40:6fa672be85ec | 40 | private: |
DavidEGrayson | 40:6fa672be85ec | 41 | |
DavidEGrayson | 40:6fa672be85ec | 42 | Timer timer; |
DavidEGrayson | 40:6fa672be85ec | 43 | uint32_t angleUnsigned; |
DavidEGrayson | 40:6fa672be85ec | 44 | int16_t rate; |
DavidEGrayson | 40:6fa672be85ec | 45 | uint16_t gyroLastUpdate; |
DavidEGrayson | 40:6fa672be85ec | 46 | }; |
DavidEGrayson | 40:6fa672be85ec | 47 | |
DavidEGrayson | 40:6fa672be85ec | 48 | |
DavidEGrayson | 40:6fa672be85ec | 49 | // This constant represents a turn of 45 degrees. |
DavidEGrayson | 40:6fa672be85ec | 50 | const int32_t turnAngle45 = 0x20000000; |
DavidEGrayson | 40:6fa672be85ec | 51 | |
DavidEGrayson | 40:6fa672be85ec | 52 | // This constant represents a turn of 90 degrees. |
DavidEGrayson | 40:6fa672be85ec | 53 | const int32_t turnAngle90 = turnAngle45 * 2; |
DavidEGrayson | 40:6fa672be85ec | 54 | |
DavidEGrayson | 40:6fa672be85ec | 55 | // This constant represents a turn of approximately 1 degree. |
DavidEGrayson | 40:6fa672be85ec | 56 | const int32_t turnAngle1 = (turnAngle45 + 22) / 45; |