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@48:597738b77f77, 2019-08-13 (annotated)

Committer:
DavidEGrayson
Date:
Tue Aug 13 21:21:17 2019 +0000
Revision:
48:597738b77f77
Parent:
47:9773dc14c834 
Changes from before the contest, I think.

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 47:9773dc14c834 53 if (rate > 0)
DavidEGrayson 47:9773dc14c834 54 {
DavidEGrayson 48:597738b77f77 55 // Counter-clockwise scale factors from
DavidEGrayson 48:597738b77f77 56 // log_190730_2.csv and log_190801_4.csv.
DavidEGrayson 48:597738b77f77 57 const double scale = 1.012394298 * 0.998207092;
DavidEGrayson 47:9773dc14c834 58 angleUnsigned += (int64_t)d * (int32_t)(14680064 * scale) / 17578125;
DavidEGrayson 47:9773dc14c834 59 }
DavidEGrayson 47:9773dc14c834 60 else
DavidEGrayson 47:9773dc14c834 61 {
DavidEGrayson 48:597738b77f77 62 // Clockwise scale factor calculated from
DavidEGrayson 48:597738b77f77 63 // log_190730_3.csv and then log_190801_3.csv.
DavidEGrayson 48:597738b77f77 64 const double scale = 0.992376154 * 0.999892525;
DavidEGrayson 47:9773dc14c834 65 angleUnsigned += (int64_t)d * (int32_t)(14680064 * scale) / 17578125;
DavidEGrayson 47:9773dc14c834 66 }
DavidEGrayson 40:6fa672be85ec 67 }
DavidEGrayson 40:6fa672be85ec 68 }