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

Committer:
DavidEGrayson
Date:
Sat Jul 27 22:52:19 2019 +0000
Revision:
43:0e985a58f174
Parent:
42:96671b71aac5
Child:
47:9773dc14c834
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 #include "turn_sensor.h"
DavidEGrayson 40:6fa672be85ec 2 #include "l3g.h"
DavidEGrayson 40:6fa672be85ec 3
DavidEGrayson 40:6fa672be85ec 4 void TurnSensor::reset()
DavidEGrayson 40:6fa672be85ec 5 {
DavidEGrayson 40:6fa672be85ec 6 angleUnsigned = 0;
DavidEGrayson 40:6fa672be85ec 7 }
DavidEGrayson 40:6fa672be85ec 8
DavidEGrayson 40:6fa672be85ec 9 void TurnSensor::start()
DavidEGrayson 40:6fa672be85ec 10 {
DavidEGrayson 40:6fa672be85ec 11 timer.start();
DavidEGrayson 40:6fa672be85ec 12 rate = 0;
DavidEGrayson 40:6fa672be85ec 13 angleUnsigned = 0;
DavidEGrayson 40:6fa672be85ec 14 gyroLastUpdate = timer.read_us();
DavidEGrayson 40:6fa672be85ec 15 }
DavidEGrayson 40:6fa672be85ec 16
DavidEGrayson 40:6fa672be85ec 17 void TurnSensor::update()
DavidEGrayson 40:6fa672be85ec 18 {
DavidEGrayson 40:6fa672be85ec 19 if (l3gZAvailable() == 1)
DavidEGrayson 40:6fa672be85ec 20 {
DavidEGrayson 42:96671b71aac5 21 int32_t gz = l3gZReadCalibrated();
DavidEGrayson 40:6fa672be85ec 22 if (gz < -500000)
DavidEGrayson 40:6fa672be85ec 23 {
DavidEGrayson 40:6fa672be85ec 24 // error
DavidEGrayson 40:6fa672be85ec 25 return;
DavidEGrayson 42:96671b71aac5 26 }
DavidEGrayson 41:3ead1dd2cc3a 27
DavidEGrayson 40:6fa672be85ec 28 // The gyro on this robot is mounted upside down; account for that here so that
DavidEGrayson 40:6fa672be85ec 29 // we can have counter-clockwise be a positive rotation.
DavidEGrayson 41:3ead1dd2cc3a 30 gz = -gz;
DavidEGrayson 41:3ead1dd2cc3a 31
DavidEGrayson 40:6fa672be85ec 32 rate = gz;
DavidEGrayson 40:6fa672be85ec 33
DavidEGrayson 40:6fa672be85ec 34 // First figure out how much time has passed since the last update (dt)
DavidEGrayson 40:6fa672be85ec 35 uint16_t m = timer.read_us();
DavidEGrayson 40:6fa672be85ec 36 uint16_t dt = m - gyroLastUpdate;
DavidEGrayson 40:6fa672be85ec 37 gyroLastUpdate = m;
DavidEGrayson 40:6fa672be85ec 38
DavidEGrayson 40:6fa672be85ec 39 // Multiply dt by turnRate in order to get an estimation of how
DavidEGrayson 40:6fa672be85ec 40 // much the robot has turned since the last update.
DavidEGrayson 40:6fa672be85ec 41 // (angular change = angular velocity * time)
DavidEGrayson 40:6fa672be85ec 42 int32_t d = (int32_t)rate * dt;
DavidEGrayson 40:6fa672be85ec 43
DavidEGrayson 40:6fa672be85ec 44 // The units of d are gyro digits times microseconds. We need
DavidEGrayson 40:6fa672be85ec 45 // to convert those to the units of turnAngle, where 2^29 units
DavidEGrayson 40:6fa672be85ec 46 // represents 45 degrees. The conversion from gyro digits to
DavidEGrayson 40:6fa672be85ec 47 // degrees per second (dps) is determined by the sensitivity of
DavidEGrayson 40:6fa672be85ec 48 // the gyro: 0.07 degrees per second per digit.
DavidEGrayson 40:6fa672be85ec 49 //
DavidEGrayson 40:6fa672be85ec 50 // (0.07 dps/digit) * (1/1000000 s/us) * (2^29/45 unit/degree)
DavidEGrayson 40:6fa672be85ec 51 // = 14680064/17578125 unit/(digit*us)
DavidEGrayson 40:6fa672be85ec 52
DavidEGrayson 40:6fa672be85ec 53 angleUnsigned += (int64_t)d * 14680064 / 17578125;
DavidEGrayson 40:6fa672be85ec 54 }
DavidEGrayson 40:6fa672be85ec 55 }