Kiko Ishimoto / QEI2

ＱＥＩその２

Dependents:   Nucleo_Motor Nucleo_Motor mbed_test_enc mbed_touteki_MR1 ... more

Fork of QEI2 by Kiko Ishimoto

QEIver2です。 前回のQEIよりハードウェアの節約を実現しました。 基本的にメソッドの追加などはないですが、コンストラクタに引数を追加しました。 第五引数にTimerクラスのアドレスを突っ込むことでTimerクラスの共有ができます。 また従来のQEIクラスの非効率的なTimerクラスの使い方をなくすことで、複数のQEIを使うことができました

QEI.cpp@5:49fa8827718d, 2015-09-23 (annotated)

Committer:
kikoaac
Date:
Wed Sep 23 06:01:56 2015 +0000
Revision:
5:49fa8827718d
Parent:
3:f285adb565b1
Child:
4:dd8e607964d2
?????????

;

Who changed what in which revision?

UserRevisionLine numberNew 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 3:f285adb565b1 39 Pin[0] = channelA;
kikoaac 3:f285adb565b1 40 Pin[1] = channelB;
kikoaac 3:f285adb565b1 41 Pin[2] = index;
kikoaac 1:1f4e8614d0ed 42 pulses_ = 0;
kikoaac 1:1f4e8614d0ed 43 revolutions_ = 0;
kikoaac 1:1f4e8614d0ed 44 pulsesPerRev_ = pulsesPerRev;
kikoaac 1:1f4e8614d0ed 45 encoding_ = encoding;
kikoaac 3:f285adb565b1 46
kikoaac 1:1f4e8614d0ed 47 //Workout what the current state is.
kikoaac 1:1f4e8614d0ed 48 int chanA = channelA_.read();
kikoaac 1:1f4e8614d0ed 49 int chanB = channelB_.read();
kikoaac 1:1f4e8614d0ed 50
kikoaac 1:1f4e8614d0ed 51 //2-bit state.
kikoaac 1:1f4e8614d0ed 52 currState_ = (chanA << 1) | (chanB);
kikoaac 1:1f4e8614d0ed 53 prevState_ = currState_;
kikoaac 1:1f4e8614d0ed 54
kikoaac 1:1f4e8614d0ed 55 channelA_.rise(this, &QEI::encode);
kikoaac 1:1f4e8614d0ed 56 channelA_.fall(this, &QEI::encode);
kikoaac 1:1f4e8614d0ed 57
kikoaac 1:1f4e8614d0ed 58
kikoaac 1:1f4e8614d0ed 59 if (encoding == X4_ENCODING) {
kikoaac 1:1f4e8614d0ed 60 channelB_.rise(this, &QEI::encode);
kikoaac 1:1f4e8614d0ed 61 channelB_.fall(this, &QEI::encode);
kikoaac 1:1f4e8614d0ed 62 }
kikoaac 1:1f4e8614d0ed 63 if (index != NC) {
kikoaac 1:1f4e8614d0ed 64 index_.rise(this, &QEI::index);
kikoaac 1:1f4e8614d0ed 65 }
kikoaac 1:1f4e8614d0ed 66
kikoaac 1:1f4e8614d0ed 67 }
kikoaac 1:1f4e8614d0ed 68 void QEI::state(int i)
kikoaac 1:1f4e8614d0ed 69 {
kikoaac 3:f285adb565b1 70 if(i==1) {
kikoaac 3:f285adb565b1 71 channelA_.disable_irq();
kikoaac 3:f285adb565b1 72 channelB_.disable_irq();
kikoaac 3:f285adb565b1 73 } else if(i==0) {
kikoaac 3:f285adb565b1 74 channelA_.enable_irq();
kikoaac 3:f285adb565b1 75 channelB_.enable_irq();
kikoaac 1:1f4e8614d0ed 76 }
kikoaac 1:1f4e8614d0ed 77 }
kikoaac 3:f285adb565b1 78 void QEI::qei_reset(void)
kikoaac 3:f285adb565b1 79 {
kikoaac 1:1f4e8614d0ed 80
kikoaac 1:1f4e8614d0ed 81 pulses_ = 0;
kikoaac 1:1f4e8614d0ed 82 revolutions_ = 0;
kikoaac 1:1f4e8614d0ed 83 round_rev = 0;
kikoaac 1:1f4e8614d0ed 84 sumangle = angle_ =0;
kikoaac 1:1f4e8614d0ed 85 }
kikoaac 3:f285adb565b1 86 void QEI::set(int pul , int rev)
kikoaac 3:f285adb565b1 87 {
kikoaac 1:1f4e8614d0ed 88
kikoaac 1:1f4e8614d0ed 89 pulses_ = pul;
kikoaac 1:1f4e8614d0ed 90 revolutions_ = rev;
kikoaac 1:1f4e8614d0ed 91
kikoaac 1:1f4e8614d0ed 92 }
kikoaac 3:f285adb565b1 93 int QEI::getCurrentState(void)
kikoaac 3:f285adb565b1 94 {
kikoaac 1:1f4e8614d0ed 95
kikoaac 1:1f4e8614d0ed 96 return currState_;
kikoaac 1:1f4e8614d0ed 97
kikoaac 1:1f4e8614d0ed 98 }
kikoaac 1:1f4e8614d0ed 99
kikoaac 3:f285adb565b1 100 int QEI::getPulses(void)
kikoaac 3:f285adb565b1 101 {
kikoaac 1:1f4e8614d0ed 102
kikoaac 1:1f4e8614d0ed 103 return pulses_;
kikoaac 1:1f4e8614d0ed 104
kikoaac 1:1f4e8614d0ed 105 }
kikoaac 1:1f4e8614d0ed 106
kikoaac 3:f285adb565b1 107 int QEI::getRevolutions(void)
kikoaac 3:f285adb565b1 108 {
kikoaac 1:1f4e8614d0ed 109
kikoaac 1:1f4e8614d0ed 110 return revolutions_;
kikoaac 1:1f4e8614d0ed 111
kikoaac 1:1f4e8614d0ed 112 }
kikoaac 1:1f4e8614d0ed 113 double QEI::getAngle()
kikoaac 1:1f4e8614d0ed 114 {
kikoaac 1:1f4e8614d0ed 115 return angle_;
kikoaac 1:1f4e8614d0ed 116 }
kikoaac 1:1f4e8614d0ed 117 int QEI::getAng_rev()
kikoaac 1:1f4e8614d0ed 118 {
kikoaac 1:1f4e8614d0ed 119 return round_rev;
kikoaac 1:1f4e8614d0ed 120 }
kikoaac 1:1f4e8614d0ed 121 double QEI::getSumangle()
kikoaac 1:1f4e8614d0ed 122 {
kikoaac 1:1f4e8614d0ed 123 return sumangle;
kikoaac 1:1f4e8614d0ed 124 }
kikoaac 0:a24686ca50ab 125
kikoaac 1:1f4e8614d0ed 126 double QEI::getRPM()
kikoaac 1:1f4e8614d0ed 127 {
kikoaac 1:1f4e8614d0ed 128 static double prev_angle;
kikoaac 3:f285adb565b1 129
kikoaac 3:f285adb565b1 130 RPM = (sumangle - prev_angle) / gettime() * 60.0 / 360;
kikoaac 3:f285adb565b1 131
kikoaac 3:f285adb565b1 132 prev_angle = sumangle;
kikoaac 1:1f4e8614d0ed 133 return RPM;
kikoaac 1:1f4e8614d0ed 134 }
kikoaac 1:1f4e8614d0ed 135 double QEI::getRPS()
kikoaac 1:1f4e8614d0ed 136 {
kikoaac 1:1f4e8614d0ed 137 static double prev_angle;
kikoaac 3:f285adb565b1 138
kikoaac 3:f285adb565b1 139 RPS = (sumangle - prev_angle) / gettime() / 360;
kikoaac 3:f285adb565b1 140 prev_angle = sumangle;
kikoaac 1:1f4e8614d0ed 141 return RPS;
kikoaac 1:1f4e8614d0ed 142 }
kikoaac 1:1f4e8614d0ed 143 double QEI::getRPMS()
kikoaac 3:f285adb565b1 144 {
kikoaac 3:f285adb565b1 145 static double prev_angle;
kikoaac 3:f285adb565b1 146
kikoaac 3:f285adb565b1 147 RPMS = (sumangle - prev_angle) / gettime()*1000/ 360;
kikoaac 3:f285adb565b1 148 prev_angle = sumangle;
kikoaac 1:1f4e8614d0ed 149 return RPMS;
kikoaac 1:1f4e8614d0ed 150 }
kikoaac 1:1f4e8614d0ed 151 double QEI::getRPUS()
kikoaac 3:f285adb565b1 152 {
kikoaac 3:f285adb565b1 153 static double prev_angle;
kikoaac 3:f285adb565b1 154 RPUS =(sumangle - prev_angle) / gettime()*1000*1000/ 360;
kikoaac 3:f285adb565b1 155 prev_angle = sumangle;
kikoaac 3:f285adb565b1 156 prev_angle = sumangle;
kikoaac 1:1f4e8614d0ed 157 return RPUS;
kikoaac 1:1f4e8614d0ed 158 }
kikoaac 3:f285adb565b1 159 void QEI::encode(void)
kikoaac 3:f285adb565b1 160 {
kikoaac 1:1f4e8614d0ed 161 int change = 0;
kikoaac 1:1f4e8614d0ed 162 int chanA = channelA_.read();
kikoaac 1:1f4e8614d0ed 163 int chanB = channelB_.read();
kikoaac 2:6a38785d5f0c 164 //printf("QEI\n");
kikoaac 1:1f4e8614d0ed 165 currState_ = (chanA << 1) | (chanB);
kikoaac 3:f285adb565b1 166
kikoaac 1:1f4e8614d0ed 167 if (encoding_ == X2_ENCODING) {
kikoaac 1:1f4e8614d0ed 168
kikoaac 1:1f4e8614d0ed 169 if ((prevState_ == 0x3 && currState_ == 0x0) ||
kikoaac 1:1f4e8614d0ed 170 (prevState_ == 0x0 && currState_ == 0x3)) {
kikoaac 1:1f4e8614d0ed 171
kikoaac 1:1f4e8614d0ed 172 pulses_++;
kikoaac 1:1f4e8614d0ed 173 angle_pulses++;
kikoaac 1:1f4e8614d0ed 174
kikoaac 3:f285adb565b1 175 } else if ((prevState_ == 0x2 && currState_ == 0x1) ||
kikoaac 3:f285adb565b1 176 (prevState_ == 0x1 && currState_ == 0x2)) {
kikoaac 1:1f4e8614d0ed 177
kikoaac 1:1f4e8614d0ed 178 pulses_--;
kikoaac 1:1f4e8614d0ed 179 angle_pulses--;
kikoaac 1:1f4e8614d0ed 180
kikoaac 1:1f4e8614d0ed 181 }
kikoaac 1:1f4e8614d0ed 182
kikoaac 1:1f4e8614d0ed 183 } else if (encoding_ == X4_ENCODING) {
kikoaac 1:1f4e8614d0ed 184
kikoaac 1:1f4e8614d0ed 185 if (((currState_ ^ prevState_) != INVALID) && (currState_ != prevState_)) {
kikoaac 1:1f4e8614d0ed 186 change = (prevState_ & PREV_MASK) ^ ((currState_ & CURR_MASK) >> 1);
kikoaac 1:1f4e8614d0ed 187
kikoaac 1:1f4e8614d0ed 188 if (change == 0) {
kikoaac 1:1f4e8614d0ed 189 change = -1;
kikoaac 1:1f4e8614d0ed 190 }
kikoaac 1:1f4e8614d0ed 191
kikoaac 1:1f4e8614d0ed 192 pulses_ -= change;
kikoaac 1:1f4e8614d0ed 193 angle_pulses -= change;
kikoaac 1:1f4e8614d0ed 194 }
kikoaac 1:1f4e8614d0ed 195
kikoaac 1:1f4e8614d0ed 196 }
kikoaac 1:1f4e8614d0ed 197 angle_ = angle_pulses*360/((double)pulsesPerRev_*4);
kikoaac 1:1f4e8614d0ed 198 sumangle = pulses_*360/((double)pulsesPerRev_*4);
kikoaac 3:f285adb565b1 199 if(angle_>=360) {
kikoaac 1:1f4e8614d0ed 200 angle_pulses = angle_ = 0;
kikoaac 1:1f4e8614d0ed 201 round_rev++;
kikoaac 3:f285adb565b1 202 } else if(angle_<=-360) {
kikoaac 1:1f4e8614d0ed 203 angle_pulses = angle_ = 0;
kikoaac 1:1f4e8614d0ed 204 round_rev--;
kikoaac 1:1f4e8614d0ed 205 }
kikoaac 1:1f4e8614d0ed 206 prevState_ = currState_;
kikoaac 1:1f4e8614d0ed 207 }
kikoaac 1:1f4e8614d0ed 208
kikoaac 3:f285adb565b1 209 void QEI::index(void)
kikoaac 3:f285adb565b1 210 {
kikoaac 1:1f4e8614d0ed 211
kikoaac 1:1f4e8614d0ed 212 revolutions_++;
kikoaac 1:1f4e8614d0ed 213
kikoaac 1:1f4e8614d0ed 214 }
kikoaac 1:1f4e8614d0ed 215