zhouhang shao
/
test
test
encoder.cpp@3:34a763e93423, 2017-05-07 (annotated)
- Committer:
- szh66
- Date:
- Sun May 07 01:31:52 2017 +0000
- Revision:
- 3:34a763e93423
- Parent:
- 2:b78dfa2afe92
- Child:
- 6:9f698d1b2996
motor circuit test
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
szh66 | 3:34a763e93423 | 1 | #include "encoder.h" |
szh66 | 3:34a763e93423 | 2 | /* |
szh66 | 3:34a763e93423 | 3 | Reset both encoders |
szh66 | 3:34a763e93423 | 4 | */ |
szh66 | 3:34a763e93423 | 5 | void resetEncoders(){ |
szh66 | 3:34a763e93423 | 6 | leftEncoder.reset(); |
szh66 | 3:34a763e93423 | 7 | } |
szh66 | 3:34a763e93423 | 8 | |
szh66 | 3:34a763e93423 | 9 | /* |
szh66 | 3:34a763e93423 | 10 | Returns the average number of pulses across both encoders since last reset. Unit is encoder pulses; intended for straight driving only. |
szh66 | 3:34a763e93423 | 11 | */ |
szh66 | 3:34a763e93423 | 12 | int getEncoderDistance(){ |
szh66 | 3:34a763e93423 | 13 | return (leftEncoder) >> 1; |
szh66 | 3:34a763e93423 | 14 | } |
szh66 | 3:34a763e93423 | 15 | |
szh66 | 3:34a763e93423 | 16 | /* |
szh66 | 3:34a763e93423 | 17 | * Represents a quadrature motor encoder. Modified from mbed QEI |
szh66 | 3:34a763e93423 | 18 | */ |
szh66 | 3:34a763e93423 | 19 | Encoder::Encoder(PinName channelA, PinName channelB) : |
szh66 | 3:34a763e93423 | 20 | channelA_(channelA), |
szh66 | 3:34a763e93423 | 21 | channelB_(channelB){ |
szh66 | 3:34a763e93423 | 22 | pulses = 0; |
szh66 | 3:34a763e93423 | 23 | |
szh66 | 3:34a763e93423 | 24 | //Workout what the current state is. |
szh66 | 3:34a763e93423 | 25 | int chanA = channelA_.read(); |
szh66 | 3:34a763e93423 | 26 | int chanB = channelB_.read(); |
szh66 | 3:34a763e93423 | 27 | |
szh66 | 3:34a763e93423 | 28 | //2-bit state. |
szh66 | 3:34a763e93423 | 29 | currState_ = (chanA << 1) | (chanB); |
szh66 | 3:34a763e93423 | 30 | prevState_ = currState_; |
szh66 | 3:34a763e93423 | 31 | |
szh66 | 3:34a763e93423 | 32 | channelA_.rise(this, &Encoder::encode); |
szh66 | 3:34a763e93423 | 33 | channelA_.fall(this, &Encoder::encode); |
szh66 | 3:34a763e93423 | 34 | } |
szh66 | 3:34a763e93423 | 35 | |
szh66 | 3:34a763e93423 | 36 | //Internally updates the pulse count for the encoder. |
szh66 | 3:34a763e93423 | 37 | void Encoder::encode(void) { |
szh66 | 3:34a763e93423 | 38 | int chanA = channelA_.read(); |
szh66 | 3:34a763e93423 | 39 | int chanB = channelB_.read(); |
szh66 | 3:34a763e93423 | 40 | |
szh66 | 3:34a763e93423 | 41 | //2-bit state. |
szh66 | 3:34a763e93423 | 42 | currState_ = (chanA << 1) | (chanB); |
szh66 | 3:34a763e93423 | 43 | |
szh66 | 3:34a763e93423 | 44 | //11->00->11->00 is counter clockwise rotation or "forward". |
szh66 | 3:34a763e93423 | 45 | if ((prevState_ == 0x3 && currState_ == 0x0) || |
szh66 | 3:34a763e93423 | 46 | (prevState_ == 0x0 && currState_ == 0x3)) { |
szh66 | 3:34a763e93423 | 47 | pulses+=2; |
szh66 | 3:34a763e93423 | 48 | } |
szh66 | 3:34a763e93423 | 49 | //10->01->10->01 is clockwise rotation or "backward". |
szh66 | 3:34a763e93423 | 50 | else if ((prevState_ == 0x2 && currState_ == 0x1) || |
szh66 | 3:34a763e93423 | 51 | (prevState_ == 0x1 && currState_ == 0x2)) { |
szh66 | 3:34a763e93423 | 52 | pulses-=2; |
szh66 | 3:34a763e93423 | 53 | } |
szh66 | 3:34a763e93423 | 54 | |
szh66 | 3:34a763e93423 | 55 | prevState_ = currState_; |
szh66 | 3:34a763e93423 | 56 | } |
szh66 | 3:34a763e93423 | 57 | |
szh66 | 3:34a763e93423 | 58 | //Resets the encoder |
szh66 | 3:34a763e93423 | 59 | void Encoder::reset(void) { |
szh66 | 3:34a763e93423 | 60 | pulses = 0; |
szh66 | 3:34a763e93423 | 61 | } |