QEIその2
Dependents: Nucleo_Motor Nucleo_Motor mbed_test_enc mbed_touteki_MR1 ... more
Fork of QEI2 by
QEIver2です。 前回のQEIよりハードウェアの節約を実現しました。 基本的にメソッドの追加などはないですが、コンストラクタに引数を追加しました。 第五引数にTimerクラスのアドレスを突っ込むことでTimerクラスの共有ができます。 また従来のQEIクラスの非効率的なTimerクラスの使い方をなくすことで、複数のQEIを使うことができました
QEI.cpp@4:dd8e607964d2, 2015-09-23 (annotated)
- Committer:
- kikoaac
- Date:
- Wed Sep 23 02:13:01 2015 +0000
- Revision:
- 4:dd8e607964d2
- Parent:
- 3:f285adb565b1
??????
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
kikoaac | 1:1f4e8614d0ed | 1 | #include "QEI.h" |
kikoaac | 3:f285adb565b1 | 2 | QEI::QEI(const QEI& q) : channelA_(q.Pin[0]), channelB_(q.Pin[1]),index_(q.Pin[2]) |
kikoaac | 3:f285adb565b1 | 3 | { |
kikoaac | 3:f285adb565b1 | 4 | pulses_ = 0; |
kikoaac | 3:f285adb565b1 | 5 | revolutions_ = 0; |
kikoaac | 3:f285adb565b1 | 6 | pulsesPerRev_ = q.pulsesPerRev_; |
kikoaac | 3:f285adb565b1 | 7 | encoding_ = q.encoding_; |
kikoaac | 3:f285adb565b1 | 8 | timer=q.timer; |
kikoaac | 3:f285adb565b1 | 9 | //Workout what the current state is. |
kikoaac | 3:f285adb565b1 | 10 | int chanA = channelA_.read(); |
kikoaac | 3:f285adb565b1 | 11 | int chanB = channelB_.read(); |
kikoaac | 1:1f4e8614d0ed | 12 | |
kikoaac | 3:f285adb565b1 | 13 | //2-bit state. |
kikoaac | 3:f285adb565b1 | 14 | currState_ = (chanA << 1) | (chanB); |
kikoaac | 3:f285adb565b1 | 15 | prevState_ = currState_; |
kikoaac | 3:f285adb565b1 | 16 | |
kikoaac | 3:f285adb565b1 | 17 | channelA_.rise(this, &QEI::encode); |
kikoaac | 3:f285adb565b1 | 18 | channelA_.fall(this, &QEI::encode); |
kikoaac | 3:f285adb565b1 | 19 | |
kikoaac | 3:f285adb565b1 | 20 | |
kikoaac | 3:f285adb565b1 | 21 | if (q.encoding_ == X4_ENCODING) { |
kikoaac | 3:f285adb565b1 | 22 | channelB_.rise(this, &QEI::encode); |
kikoaac | 3:f285adb565b1 | 23 | channelB_.fall(this, &QEI::encode); |
kikoaac | 3:f285adb565b1 | 24 | } |
kikoaac | 3:f285adb565b1 | 25 | if (Pin[2] != NC) { |
kikoaac | 3:f285adb565b1 | 26 | index_.rise(this, &QEI::index); |
kikoaac | 3:f285adb565b1 | 27 | } |
kikoaac | 3:f285adb565b1 | 28 | } |
kikoaac | 1:1f4e8614d0ed | 29 | QEI::QEI(PinName channelA, |
kikoaac | 1:1f4e8614d0ed | 30 | PinName channelB, |
kikoaac | 1:1f4e8614d0ed | 31 | PinName index, |
kikoaac | 1:1f4e8614d0ed | 32 | int pulsesPerRev, |
kikoaac | 3:f285adb565b1 | 33 | Timer *T, |
kikoaac | 1:1f4e8614d0ed | 34 | Encoding encoding |
kikoaac | 3:f285adb565b1 | 35 | ) : channelA_(channelA), channelB_(channelB), |
kikoaac | 3:f285adb565b1 | 36 | index_(index) |
kikoaac | 3:f285adb565b1 | 37 | { |
kikoaac | 3:f285adb565b1 | 38 | timer=T; |
kikoaac | 4:dd8e607964d2 | 39 | timer->start(); |
kikoaac | 3:f285adb565b1 | 40 | Pin[0] = channelA; |
kikoaac | 3:f285adb565b1 | 41 | Pin[1] = channelB; |
kikoaac | 3:f285adb565b1 | 42 | Pin[2] = index; |
kikoaac | 1:1f4e8614d0ed | 43 | pulses_ = 0; |
kikoaac | 1:1f4e8614d0ed | 44 | revolutions_ = 0; |
kikoaac | 1:1f4e8614d0ed | 45 | pulsesPerRev_ = pulsesPerRev; |
kikoaac | 1:1f4e8614d0ed | 46 | encoding_ = encoding; |
kikoaac | 3:f285adb565b1 | 47 | |
kikoaac | 1:1f4e8614d0ed | 48 | //Workout what the current state is. |
kikoaac | 1:1f4e8614d0ed | 49 | int chanA = channelA_.read(); |
kikoaac | 1:1f4e8614d0ed | 50 | int chanB = channelB_.read(); |
kikoaac | 1:1f4e8614d0ed | 51 | |
kikoaac | 1:1f4e8614d0ed | 52 | //2-bit state. |
kikoaac | 1:1f4e8614d0ed | 53 | currState_ = (chanA << 1) | (chanB); |
kikoaac | 1:1f4e8614d0ed | 54 | prevState_ = currState_; |
kikoaac | 1:1f4e8614d0ed | 55 | |
kikoaac | 1:1f4e8614d0ed | 56 | channelA_.rise(this, &QEI::encode); |
kikoaac | 1:1f4e8614d0ed | 57 | channelA_.fall(this, &QEI::encode); |
kikoaac | 1:1f4e8614d0ed | 58 | |
kikoaac | 1:1f4e8614d0ed | 59 | |
kikoaac | 1:1f4e8614d0ed | 60 | if (encoding == X4_ENCODING) { |
kikoaac | 1:1f4e8614d0ed | 61 | channelB_.rise(this, &QEI::encode); |
kikoaac | 1:1f4e8614d0ed | 62 | channelB_.fall(this, &QEI::encode); |
kikoaac | 1:1f4e8614d0ed | 63 | } |
kikoaac | 1:1f4e8614d0ed | 64 | if (index != NC) { |
kikoaac | 1:1f4e8614d0ed | 65 | index_.rise(this, &QEI::index); |
kikoaac | 1:1f4e8614d0ed | 66 | } |
kikoaac | 1:1f4e8614d0ed | 67 | |
kikoaac | 1:1f4e8614d0ed | 68 | } |
kikoaac | 1:1f4e8614d0ed | 69 | void QEI::state(int i) |
kikoaac | 1:1f4e8614d0ed | 70 | { |
kikoaac | 3:f285adb565b1 | 71 | if(i==1) { |
kikoaac | 3:f285adb565b1 | 72 | channelA_.disable_irq(); |
kikoaac | 3:f285adb565b1 | 73 | channelB_.disable_irq(); |
kikoaac | 3:f285adb565b1 | 74 | } else if(i==0) { |
kikoaac | 3:f285adb565b1 | 75 | channelA_.enable_irq(); |
kikoaac | 3:f285adb565b1 | 76 | channelB_.enable_irq(); |
kikoaac | 1:1f4e8614d0ed | 77 | } |
kikoaac | 1:1f4e8614d0ed | 78 | } |
kikoaac | 3:f285adb565b1 | 79 | void QEI::qei_reset(void) |
kikoaac | 3:f285adb565b1 | 80 | { |
kikoaac | 1:1f4e8614d0ed | 81 | |
kikoaac | 1:1f4e8614d0ed | 82 | pulses_ = 0; |
kikoaac | 1:1f4e8614d0ed | 83 | revolutions_ = 0; |
kikoaac | 1:1f4e8614d0ed | 84 | round_rev = 0; |
kikoaac | 1:1f4e8614d0ed | 85 | sumangle = angle_ =0; |
kikoaac | 1:1f4e8614d0ed | 86 | } |
kikoaac | 3:f285adb565b1 | 87 | void QEI::set(int pul , int rev) |
kikoaac | 3:f285adb565b1 | 88 | { |
kikoaac | 1:1f4e8614d0ed | 89 | |
kikoaac | 1:1f4e8614d0ed | 90 | pulses_ = pul; |
kikoaac | 1:1f4e8614d0ed | 91 | revolutions_ = rev; |
kikoaac | 1:1f4e8614d0ed | 92 | |
kikoaac | 1:1f4e8614d0ed | 93 | } |
kikoaac | 3:f285adb565b1 | 94 | int QEI::getCurrentState(void) |
kikoaac | 3:f285adb565b1 | 95 | { |
kikoaac | 1:1f4e8614d0ed | 96 | |
kikoaac | 1:1f4e8614d0ed | 97 | return currState_; |
kikoaac | 1:1f4e8614d0ed | 98 | |
kikoaac | 1:1f4e8614d0ed | 99 | } |
kikoaac | 1:1f4e8614d0ed | 100 | |
kikoaac | 3:f285adb565b1 | 101 | int QEI::getPulses(void) |
kikoaac | 3:f285adb565b1 | 102 | { |
kikoaac | 1:1f4e8614d0ed | 103 | |
kikoaac | 1:1f4e8614d0ed | 104 | return pulses_; |
kikoaac | 1:1f4e8614d0ed | 105 | |
kikoaac | 1:1f4e8614d0ed | 106 | } |
kikoaac | 1:1f4e8614d0ed | 107 | |
kikoaac | 3:f285adb565b1 | 108 | int QEI::getRevolutions(void) |
kikoaac | 3:f285adb565b1 | 109 | { |
kikoaac | 1:1f4e8614d0ed | 110 | |
kikoaac | 1:1f4e8614d0ed | 111 | return revolutions_; |
kikoaac | 1:1f4e8614d0ed | 112 | |
kikoaac | 1:1f4e8614d0ed | 113 | } |
kikoaac | 1:1f4e8614d0ed | 114 | double QEI::getAngle() |
kikoaac | 1:1f4e8614d0ed | 115 | { |
kikoaac | 1:1f4e8614d0ed | 116 | return angle_; |
kikoaac | 1:1f4e8614d0ed | 117 | } |
kikoaac | 1:1f4e8614d0ed | 118 | int QEI::getAng_rev() |
kikoaac | 1:1f4e8614d0ed | 119 | { |
kikoaac | 1:1f4e8614d0ed | 120 | return round_rev; |
kikoaac | 1:1f4e8614d0ed | 121 | } |
kikoaac | 1:1f4e8614d0ed | 122 | double QEI::getSumangle() |
kikoaac | 1:1f4e8614d0ed | 123 | { |
kikoaac | 1:1f4e8614d0ed | 124 | return sumangle; |
kikoaac | 1:1f4e8614d0ed | 125 | } |
kikoaac | 0:a24686ca50ab | 126 | |
kikoaac | 1:1f4e8614d0ed | 127 | double QEI::getRPM() |
kikoaac | 1:1f4e8614d0ed | 128 | { |
kikoaac | 3:f285adb565b1 | 129 | |
kikoaac | 4:dd8e607964d2 | 130 | float data = (sumangle - prev_angle) / gettime() * 60.0 / 360; |
kikoaac | 3:f285adb565b1 | 131 | |
kikoaac | 3:f285adb565b1 | 132 | prev_angle = sumangle; |
kikoaac | 4:dd8e607964d2 | 133 | RPM = data; |
kikoaac | 4:dd8e607964d2 | 134 | //printf("%f %f\r\n",getSumangle(),prev_angle); |
kikoaac | 1:1f4e8614d0ed | 135 | return RPM; |
kikoaac | 1:1f4e8614d0ed | 136 | } |
kikoaac | 1:1f4e8614d0ed | 137 | double QEI::getRPS() |
kikoaac | 1:1f4e8614d0ed | 138 | { |
kikoaac | 1:1f4e8614d0ed | 139 | static double prev_angle; |
kikoaac | 3:f285adb565b1 | 140 | |
kikoaac | 3:f285adb565b1 | 141 | RPS = (sumangle - prev_angle) / gettime() / 360; |
kikoaac | 3:f285adb565b1 | 142 | prev_angle = sumangle; |
kikoaac | 1:1f4e8614d0ed | 143 | return RPS; |
kikoaac | 1:1f4e8614d0ed | 144 | } |
kikoaac | 1:1f4e8614d0ed | 145 | double QEI::getRPMS() |
kikoaac | 3:f285adb565b1 | 146 | { |
kikoaac | 3:f285adb565b1 | 147 | static double prev_angle; |
kikoaac | 3:f285adb565b1 | 148 | |
kikoaac | 3:f285adb565b1 | 149 | RPMS = (sumangle - prev_angle) / gettime()*1000/ 360; |
kikoaac | 3:f285adb565b1 | 150 | prev_angle = sumangle; |
kikoaac | 1:1f4e8614d0ed | 151 | return RPMS; |
kikoaac | 1:1f4e8614d0ed | 152 | } |
kikoaac | 1:1f4e8614d0ed | 153 | double QEI::getRPUS() |
kikoaac | 3:f285adb565b1 | 154 | { |
kikoaac | 3:f285adb565b1 | 155 | static double prev_angle; |
kikoaac | 3:f285adb565b1 | 156 | RPUS =(sumangle - prev_angle) / gettime()*1000*1000/ 360; |
kikoaac | 3:f285adb565b1 | 157 | prev_angle = sumangle; |
kikoaac | 3:f285adb565b1 | 158 | prev_angle = sumangle; |
kikoaac | 1:1f4e8614d0ed | 159 | return RPUS; |
kikoaac | 1:1f4e8614d0ed | 160 | } |
kikoaac | 3:f285adb565b1 | 161 | void QEI::encode(void) |
kikoaac | 3:f285adb565b1 | 162 | { |
kikoaac | 1:1f4e8614d0ed | 163 | int change = 0; |
kikoaac | 1:1f4e8614d0ed | 164 | int chanA = channelA_.read(); |
kikoaac | 1:1f4e8614d0ed | 165 | int chanB = channelB_.read(); |
kikoaac | 2:6a38785d5f0c | 166 | //printf("QEI\n"); |
kikoaac | 1:1f4e8614d0ed | 167 | currState_ = (chanA << 1) | (chanB); |
kikoaac | 3:f285adb565b1 | 168 | |
kikoaac | 1:1f4e8614d0ed | 169 | if (encoding_ == X2_ENCODING) { |
kikoaac | 1:1f4e8614d0ed | 170 | |
kikoaac | 1:1f4e8614d0ed | 171 | if ((prevState_ == 0x3 && currState_ == 0x0) || |
kikoaac | 1:1f4e8614d0ed | 172 | (prevState_ == 0x0 && currState_ == 0x3)) { |
kikoaac | 1:1f4e8614d0ed | 173 | |
kikoaac | 1:1f4e8614d0ed | 174 | pulses_++; |
kikoaac | 1:1f4e8614d0ed | 175 | angle_pulses++; |
kikoaac | 1:1f4e8614d0ed | 176 | |
kikoaac | 3:f285adb565b1 | 177 | } else if ((prevState_ == 0x2 && currState_ == 0x1) || |
kikoaac | 3:f285adb565b1 | 178 | (prevState_ == 0x1 && currState_ == 0x2)) { |
kikoaac | 1:1f4e8614d0ed | 179 | |
kikoaac | 1:1f4e8614d0ed | 180 | pulses_--; |
kikoaac | 1:1f4e8614d0ed | 181 | angle_pulses--; |
kikoaac | 1:1f4e8614d0ed | 182 | |
kikoaac | 1:1f4e8614d0ed | 183 | } |
kikoaac | 1:1f4e8614d0ed | 184 | |
kikoaac | 1:1f4e8614d0ed | 185 | } else if (encoding_ == X4_ENCODING) { |
kikoaac | 1:1f4e8614d0ed | 186 | |
kikoaac | 1:1f4e8614d0ed | 187 | if (((currState_ ^ prevState_) != INVALID) && (currState_ != prevState_)) { |
kikoaac | 1:1f4e8614d0ed | 188 | change = (prevState_ & PREV_MASK) ^ ((currState_ & CURR_MASK) >> 1); |
kikoaac | 1:1f4e8614d0ed | 189 | |
kikoaac | 1:1f4e8614d0ed | 190 | if (change == 0) { |
kikoaac | 1:1f4e8614d0ed | 191 | change = -1; |
kikoaac | 1:1f4e8614d0ed | 192 | } |
kikoaac | 1:1f4e8614d0ed | 193 | |
kikoaac | 1:1f4e8614d0ed | 194 | pulses_ -= change; |
kikoaac | 1:1f4e8614d0ed | 195 | angle_pulses -= change; |
kikoaac | 1:1f4e8614d0ed | 196 | } |
kikoaac | 1:1f4e8614d0ed | 197 | |
kikoaac | 1:1f4e8614d0ed | 198 | } |
kikoaac | 1:1f4e8614d0ed | 199 | angle_ = angle_pulses*360/((double)pulsesPerRev_*4); |
kikoaac | 1:1f4e8614d0ed | 200 | sumangle = pulses_*360/((double)pulsesPerRev_*4); |
kikoaac | 3:f285adb565b1 | 201 | if(angle_>=360) { |
kikoaac | 1:1f4e8614d0ed | 202 | angle_pulses = angle_ = 0; |
kikoaac | 1:1f4e8614d0ed | 203 | round_rev++; |
kikoaac | 3:f285adb565b1 | 204 | } else if(angle_<=-360) { |
kikoaac | 1:1f4e8614d0ed | 205 | angle_pulses = angle_ = 0; |
kikoaac | 1:1f4e8614d0ed | 206 | round_rev--; |
kikoaac | 1:1f4e8614d0ed | 207 | } |
kikoaac | 1:1f4e8614d0ed | 208 | prevState_ = currState_; |
kikoaac | 1:1f4e8614d0ed | 209 | } |
kikoaac | 1:1f4e8614d0ed | 210 | |
kikoaac | 3:f285adb565b1 | 211 | void QEI::index(void) |
kikoaac | 3:f285adb565b1 | 212 | { |
kikoaac | 1:1f4e8614d0ed | 213 | |
kikoaac | 1:1f4e8614d0ed | 214 | revolutions_++; |
kikoaac | 1:1f4e8614d0ed | 215 | |
kikoaac | 1:1f4e8614d0ed | 216 | } |
kikoaac | 1:1f4e8614d0ed | 217 |