David's line following code from the LVBots competition, 2015.
Dependencies: GeneralDebouncer Pacer PololuEncoder mbed
Fork of DeadReckoning by
turn_sensor.cpp
- Committer:
- DavidEGrayson
- Date:
- 2015-04-15
- Revision:
- 45:e16e74bbbf8c
- Parent:
- 44:edcacba44760
- Child:
- 46:f11cb4f93aac
File content as of revision 45:e16e74bbbf8c:
#include "turn_sensor.h" #include "l3g.h" void TurnSensor::start() { timer.start(); rate = 0; angleUnsigned = 0; gyroLastUpdate = timer.read_us(); } void TurnSensor::update() { if (l3gZAvailable() == 1) { int32_t gz = l3gZRead(); if (gz < -500000) { // error return; } rate = gz; // First figure out how much time has passed since the last update (dt) uint16_t m = timer.read_us(); uint16_t dt = m - gyroLastUpdate; gyroLastUpdate = m; // Multiply dt by turnRate in order to get an estimation of how // much the robot has turned since the last update. // (angular change = angular velocity * time) int32_t d = (int32_t)rate * dt; // The units of d are gyro digits times microseconds. We need // to convert those to the units of turnAngle, where 2^29 units // represents 45 degrees. The conversion from gyro digits to // degrees per second (dps) is determined by the sensitivity of // the gyro: 0.07 degrees per second per digit. // // (0.07 dps/digit) * (1/1000000 s/us) * (2^29/45 unit/degree) // = 14680064/17578125 unit/(digit*us) angleUnsigned += (int64_t)d * 14680064 / 17578125; } }