Code for autonomous ground vehicle, Data Bus, 3rd place winner in 2012 Sparkfun AVC.
Dependencies: Watchdog mbed Schedule SimpleFilter LSM303DLM PinDetect DebounceIn Servo
Sensors/IncrementalEncoder/IncrementalEncoder.cpp@0:826c6171fc1b, 2012-06-20 (annotated)
- Committer:
- shimniok
- Date:
- Wed Jun 20 14:57:48 2012 +0000
- Revision:
- 0:826c6171fc1b
Updated documentation
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
shimniok | 0:826c6171fc1b | 1 | #include "IncrementalEncoder.h" |
shimniok | 0:826c6171fc1b | 2 | |
shimniok | 0:826c6171fc1b | 3 | IncrementalEncoder::IncrementalEncoder(PinName pin): _lastTicks(0), _ticks(0), _new(false), _interrupt(pin) { |
shimniok | 0:826c6171fc1b | 4 | _interrupt.mode(PullNone); // default is pulldown but my encoder board uses a pull-up and that just don't work |
shimniok | 0:826c6171fc1b | 5 | _interrupt.rise(this, &IncrementalEncoder::_incRise); |
shimniok | 0:826c6171fc1b | 6 | _interrupt.fall(this, &IncrementalEncoder::_incFall); |
shimniok | 0:826c6171fc1b | 7 | _t.start(); |
shimniok | 0:826c6171fc1b | 8 | _t.reset(); |
shimniok | 0:826c6171fc1b | 9 | _lastTime = _t.read_us(); |
shimniok | 0:826c6171fc1b | 10 | } |
shimniok | 0:826c6171fc1b | 11 | |
shimniok | 0:826c6171fc1b | 12 | unsigned int IncrementalEncoder::read() { |
shimniok | 0:826c6171fc1b | 13 | // disable interrupts? |
shimniok | 0:826c6171fc1b | 14 | unsigned int ticks = _ticks - _lastTicks; |
shimniok | 0:826c6171fc1b | 15 | _lastTicks = _ticks; |
shimniok | 0:826c6171fc1b | 16 | _new=false; |
shimniok | 0:826c6171fc1b | 17 | return ticks; |
shimniok | 0:826c6171fc1b | 18 | } |
shimniok | 0:826c6171fc1b | 19 | |
shimniok | 0:826c6171fc1b | 20 | unsigned int IncrementalEncoder::readTotal() { |
shimniok | 0:826c6171fc1b | 21 | _new=false; |
shimniok | 0:826c6171fc1b | 22 | return _ticks; |
shimniok | 0:826c6171fc1b | 23 | } |
shimniok | 0:826c6171fc1b | 24 | |
shimniok | 0:826c6171fc1b | 25 | unsigned int IncrementalEncoder::readRise() { |
shimniok | 0:826c6171fc1b | 26 | _new=false; |
shimniok | 0:826c6171fc1b | 27 | return _rise; |
shimniok | 0:826c6171fc1b | 28 | } |
shimniok | 0:826c6171fc1b | 29 | |
shimniok | 0:826c6171fc1b | 30 | unsigned int IncrementalEncoder::readFall() { |
shimniok | 0:826c6171fc1b | 31 | _new=false; |
shimniok | 0:826c6171fc1b | 32 | return _fall; |
shimniok | 0:826c6171fc1b | 33 | } |
shimniok | 0:826c6171fc1b | 34 | |
shimniok | 0:826c6171fc1b | 35 | unsigned int IncrementalEncoder::readTime() { |
shimniok | 0:826c6171fc1b | 36 | return _time; |
shimniok | 0:826c6171fc1b | 37 | } |
shimniok | 0:826c6171fc1b | 38 | |
shimniok | 0:826c6171fc1b | 39 | void IncrementalEncoder::reset() { |
shimniok | 0:826c6171fc1b | 40 | _ticks = _lastTicks = 0; |
shimniok | 0:826c6171fc1b | 41 | } |
shimniok | 0:826c6171fc1b | 42 | |
shimniok | 0:826c6171fc1b | 43 | void IncrementalEncoder::_increment() { |
shimniok | 0:826c6171fc1b | 44 | _ticks++; |
shimniok | 0:826c6171fc1b | 45 | } |
shimniok | 0:826c6171fc1b | 46 | |
shimniok | 0:826c6171fc1b | 47 | #define A 0.3 // mix in how much from previous readings? |
shimniok | 0:826c6171fc1b | 48 | void IncrementalEncoder::_incRise() { |
shimniok | 0:826c6171fc1b | 49 | _rise++; |
shimniok | 0:826c6171fc1b | 50 | _ticks++; |
shimniok | 0:826c6171fc1b | 51 | _new=true; |
shimniok | 0:826c6171fc1b | 52 | // compute time between ticks; only do this for rise |
shimniok | 0:826c6171fc1b | 53 | // to eliminate jitter |
shimniok | 0:826c6171fc1b | 54 | int now = _t.read_us(); |
shimniok | 0:826c6171fc1b | 55 | _time = A*_time + (1-A)*(now - _lastTime); |
shimniok | 0:826c6171fc1b | 56 | // Dead band: if _time > xxxx then turn off and wait until next tick |
shimniok | 0:826c6171fc1b | 57 | // to start up again |
shimniok | 0:826c6171fc1b | 58 | _lastTime = now; |
shimniok | 0:826c6171fc1b | 59 | } |
shimniok | 0:826c6171fc1b | 60 | |
shimniok | 0:826c6171fc1b | 61 | void IncrementalEncoder::_incFall() { |
shimniok | 0:826c6171fc1b | 62 | _fall++; |
shimniok | 0:826c6171fc1b | 63 | _ticks++; |
shimniok | 0:826c6171fc1b | 64 | _new=true; |
shimniok | 0:826c6171fc1b | 65 | } |