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@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?

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 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