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@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?

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;