David Grayson / Mbed 2 deprecated DeadReckoning

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?

UserRevisionLine numberNew 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;