David's line following code from the LVBots competition, 2015.
Dependencies: GeneralDebouncer Pacer PololuEncoder mbed
Fork of DeadReckoning by
turn_sensor.cpp@47:cb5c1504c24d, 2015-04-15 (annotated)
- Committer:
- DavidEGrayson
- Date:
- Wed Apr 15 21:19:52 2015 +0000
- Revision:
- 47:cb5c1504c24d
- Parent:
- 46:f11cb4f93aac
- Child:
- 50:517c0f0e621f
fixed something in logger;
Who changed what in which revision?
User | Revision | Line number | New 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 | } |