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@43:0e985a58f174, 2019-07-27 (annotated)

Committer:
DavidEGrayson
Date:
Sat Jul 27 22:52:19 2019 +0000
Revision:
43:0e985a58f174
Parent:
42:96671b71aac5 
Changed reckoner to use readings from turnSensor (Gyro) to get its direction vector instead of encoder ticks.

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