David Grayson / Mbed 2 deprecated LineFollowing

David's line following code from the LVBots competition, 2015.

Dependencies:   GeneralDebouncer Pacer PololuEncoder mbed

Fork of DeadReckoning by David Grayson

turn_sensor.cpp@46:f11cb4f93aac, 2015-04-15 (annotated)

Committer:
DavidEGrayson
Date:
Wed Apr 15 21:19:22 2015 +0000
Revision:
46:f11cb4f93aac
Parent:
45:e16e74bbbf8c
Child:
50:517c0f0e621f
got dead reckoning working nicely with the gyro;

Who changed what in which revision?

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