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@2:6a38785d5f0c, 2015-07-21 (annotated)

Committer:
kikoaac
Date:
Tue Jul 21 08:12:12 2015 +0000
Revision:
2:6a38785d5f0c
Parent:
1:1f4e8614d0ed
Child:
3:f285adb565b1
QEI

;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kikoaac 1:1f4e8614d0ed 1 #include "QEI.h"
kikoaac 1:1f4e8614d0ed 2
kikoaac 1:1f4e8614d0ed 3 QEI::QEI(PinName channelA,
kikoaac 1:1f4e8614d0ed 4 PinName channelB,
kikoaac 1:1f4e8614d0ed 5 PinName index,
kikoaac 1:1f4e8614d0ed 6 int pulsesPerRev,
kikoaac 1:1f4e8614d0ed 7 Encoding encoding
kikoaac 1:1f4e8614d0ed 8 ) : channelA_(channelA), channelB_(channelB),
kikoaac 1:1f4e8614d0ed 9 index_(index) {
kikoaac 1:1f4e8614d0ed 10
kikoaac 1:1f4e8614d0ed 11 pulses_ = 0;
kikoaac 1:1f4e8614d0ed 12 revolutions_ = 0;
kikoaac 1:1f4e8614d0ed 13 pulsesPerRev_ = pulsesPerRev;
kikoaac 1:1f4e8614d0ed 14 encoding_ = encoding;
kikoaac 1:1f4e8614d0ed 15
kikoaac 1:1f4e8614d0ed 16 //Workout what the current state is.
kikoaac 1:1f4e8614d0ed 17 int chanA = channelA_.read();
kikoaac 1:1f4e8614d0ed 18 int chanB = channelB_.read();
kikoaac 1:1f4e8614d0ed 19
kikoaac 1:1f4e8614d0ed 20 //2-bit state.
kikoaac 1:1f4e8614d0ed 21 currState_ = (chanA << 1) | (chanB);
kikoaac 1:1f4e8614d0ed 22 prevState_ = currState_;
kikoaac 1:1f4e8614d0ed 23
kikoaac 1:1f4e8614d0ed 24 channelA_.rise(this, &QEI::encode);
kikoaac 1:1f4e8614d0ed 25 channelA_.fall(this, &QEI::encode);
kikoaac 1:1f4e8614d0ed 26
kikoaac 1:1f4e8614d0ed 27
kikoaac 1:1f4e8614d0ed 28 if (encoding == X4_ENCODING) {
kikoaac 1:1f4e8614d0ed 29 channelB_.rise(this, &QEI::encode);
kikoaac 1:1f4e8614d0ed 30 channelB_.fall(this, &QEI::encode);
kikoaac 1:1f4e8614d0ed 31 }
kikoaac 1:1f4e8614d0ed 32 if (index != NC) {
kikoaac 1:1f4e8614d0ed 33 index_.rise(this, &QEI::index);
kikoaac 1:1f4e8614d0ed 34 }
kikoaac 1:1f4e8614d0ed 35
kikoaac 1:1f4e8614d0ed 36 }
kikoaac 1:1f4e8614d0ed 37 void QEI::state(int i)
kikoaac 1:1f4e8614d0ed 38 {
kikoaac 1:1f4e8614d0ed 39 if(i==1)
kikoaac 1:1f4e8614d0ed 40 {
kikoaac 1:1f4e8614d0ed 41 channelA_.disable_irq();
kikoaac 1:1f4e8614d0ed 42 channelB_.disable_irq();
kikoaac 1:1f4e8614d0ed 43 }
kikoaac 1:1f4e8614d0ed 44 else if(i==0)
kikoaac 1:1f4e8614d0ed 45 {
kikoaac 1:1f4e8614d0ed 46 channelA_.enable_irq();
kikoaac 1:1f4e8614d0ed 47 channelB_.enable_irq();
kikoaac 1:1f4e8614d0ed 48 }
kikoaac 1:1f4e8614d0ed 49 }
kikoaac 1:1f4e8614d0ed 50 void QEI::reset(void) {
kikoaac 1:1f4e8614d0ed 51
kikoaac 1:1f4e8614d0ed 52 pulses_ = 0;
kikoaac 1:1f4e8614d0ed 53 revolutions_ = 0;
kikoaac 1:1f4e8614d0ed 54 round_rev = 0;
kikoaac 1:1f4e8614d0ed 55 sumangle = angle_ =0;
kikoaac 1:1f4e8614d0ed 56 }
kikoaac 1:1f4e8614d0ed 57 void QEI::set(int pul , int rev) {
kikoaac 1:1f4e8614d0ed 58
kikoaac 1:1f4e8614d0ed 59 pulses_ = pul;
kikoaac 1:1f4e8614d0ed 60 revolutions_ = rev;
kikoaac 1:1f4e8614d0ed 61
kikoaac 1:1f4e8614d0ed 62 }
kikoaac 1:1f4e8614d0ed 63 int QEI::getCurrentState(void) {
kikoaac 1:1f4e8614d0ed 64
kikoaac 1:1f4e8614d0ed 65 return currState_;
kikoaac 1:1f4e8614d0ed 66
kikoaac 1:1f4e8614d0ed 67 }
kikoaac 1:1f4e8614d0ed 68
kikoaac 1:1f4e8614d0ed 69 int QEI::getPulses(void) {
kikoaac 1:1f4e8614d0ed 70
kikoaac 1:1f4e8614d0ed 71 return pulses_;
kikoaac 1:1f4e8614d0ed 72
kikoaac 1:1f4e8614d0ed 73 }
kikoaac 1:1f4e8614d0ed 74
kikoaac 1:1f4e8614d0ed 75 int QEI::getRevolutions(void) {
kikoaac 1:1f4e8614d0ed 76
kikoaac 1:1f4e8614d0ed 77 return revolutions_;
kikoaac 1:1f4e8614d0ed 78
kikoaac 1:1f4e8614d0ed 79 }
kikoaac 1:1f4e8614d0ed 80 double QEI::getAngle()
kikoaac 1:1f4e8614d0ed 81 {
kikoaac 1:1f4e8614d0ed 82 return angle_;
kikoaac 1:1f4e8614d0ed 83 }
kikoaac 1:1f4e8614d0ed 84 int QEI::getAng_rev()
kikoaac 1:1f4e8614d0ed 85 {
kikoaac 1:1f4e8614d0ed 86 return round_rev;
kikoaac 1:1f4e8614d0ed 87 }
kikoaac 1:1f4e8614d0ed 88 double QEI::getSumangle()
kikoaac 1:1f4e8614d0ed 89 {
kikoaac 1:1f4e8614d0ed 90 return sumangle;
kikoaac 1:1f4e8614d0ed 91 }
kikoaac 0:a24686ca50ab 92
kikoaac 1:1f4e8614d0ed 93 double QEI::getRPM()
kikoaac 1:1f4e8614d0ed 94 {
kikoaac 1:1f4e8614d0ed 95 static double prev_angle;
kikoaac 1:1f4e8614d0ed 96 Mper.stop();
kikoaac 1:1f4e8614d0ed 97
kikoaac 1:1f4e8614d0ed 98 RPM = (sumangle - prev_angle) / Mper.read() * 60.0 / 360;
kikoaac 1:1f4e8614d0ed 99 Mper.reset();
kikoaac 1:1f4e8614d0ed 100 Mper.start();
kikoaac 1:1f4e8614d0ed 101 prev_angle = sumangle;
kikoaac 1:1f4e8614d0ed 102 return RPM;
kikoaac 1:1f4e8614d0ed 103 }
kikoaac 1:1f4e8614d0ed 104 double QEI::getRPS()
kikoaac 1:1f4e8614d0ed 105 {
kikoaac 1:1f4e8614d0ed 106 static double prev_angle;
kikoaac 1:1f4e8614d0ed 107 Rper.stop();
kikoaac 1:1f4e8614d0ed 108
kikoaac 1:1f4e8614d0ed 109 RPS = (sumangle - prev_angle) / Rper.read() / 360;
kikoaac 1:1f4e8614d0ed 110 Rper.reset();
kikoaac 1:1f4e8614d0ed 111 Rper.start();
kikoaac 1:1f4e8614d0ed 112 prev_angle = sumangle;
kikoaac 1:1f4e8614d0ed 113 return RPS;
kikoaac 1:1f4e8614d0ed 114 }
kikoaac 1:1f4e8614d0ed 115 double QEI::getRPMS()
kikoaac 1:1f4e8614d0ed 116 { static double prev_angle;
kikoaac 1:1f4e8614d0ed 117 MSper.stop();
kikoaac 1:1f4e8614d0ed 118
kikoaac 1:1f4e8614d0ed 119 RPMS = (sumangle - prev_angle) / (double)MSper.read_ms() / 360;
kikoaac 1:1f4e8614d0ed 120 MSper.reset();
kikoaac 1:1f4e8614d0ed 121 MSper.start();
kikoaac 1:1f4e8614d0ed 122 prev_angle = sumangle;
kikoaac 1:1f4e8614d0ed 123 return RPMS;
kikoaac 1:1f4e8614d0ed 124 }
kikoaac 1:1f4e8614d0ed 125 double QEI::getRPUS()
kikoaac 1:1f4e8614d0ed 126 { static double prev_angle;
kikoaac 1:1f4e8614d0ed 127 USper.stop();
kikoaac 1:1f4e8614d0ed 128
kikoaac 1:1f4e8614d0ed 129 RPUS = (sumangle - prev_angle) / (double)USper.read_us() / 360;
kikoaac 1:1f4e8614d0ed 130 USper.reset();
kikoaac 1:1f4e8614d0ed 131 USper.start();
kikoaac 1:1f4e8614d0ed 132 prev_angle = sumangle;
kikoaac 1:1f4e8614d0ed 133 return RPUS;
kikoaac 1:1f4e8614d0ed 134 }
kikoaac 1:1f4e8614d0ed 135 void QEI::encode(void) {
kikoaac 2:6a38785d5f0c 136 NVIC_DisableIRQ( I2C_IRQn ) ;
kikoaac 1:1f4e8614d0ed 137 int change = 0;
kikoaac 1:1f4e8614d0ed 138 int chanA = channelA_.read();
kikoaac 1:1f4e8614d0ed 139 int chanB = channelB_.read();
kikoaac 2:6a38785d5f0c 140 //printf("QEI\n");
kikoaac 1:1f4e8614d0ed 141 currState_ = (chanA << 1) | (chanB);
kikoaac 1:1f4e8614d0ed 142
kikoaac 1:1f4e8614d0ed 143 if (encoding_ == X2_ENCODING) {
kikoaac 1:1f4e8614d0ed 144
kikoaac 1:1f4e8614d0ed 145 if ((prevState_ == 0x3 && currState_ == 0x0) ||
kikoaac 1:1f4e8614d0ed 146 (prevState_ == 0x0 && currState_ == 0x3)) {
kikoaac 1:1f4e8614d0ed 147
kikoaac 1:1f4e8614d0ed 148 pulses_++;
kikoaac 1:1f4e8614d0ed 149 angle_pulses++;
kikoaac 1:1f4e8614d0ed 150
kikoaac 1:1f4e8614d0ed 151 }
kikoaac 1:1f4e8614d0ed 152 else if ((prevState_ == 0x2 && currState_ == 0x1) ||
kikoaac 1:1f4e8614d0ed 153 (prevState_ == 0x1 && currState_ == 0x2)) {
kikoaac 1:1f4e8614d0ed 154
kikoaac 1:1f4e8614d0ed 155 pulses_--;
kikoaac 1:1f4e8614d0ed 156 angle_pulses--;
kikoaac 1:1f4e8614d0ed 157
kikoaac 1:1f4e8614d0ed 158 }
kikoaac 1:1f4e8614d0ed 159
kikoaac 1:1f4e8614d0ed 160 } else if (encoding_ == X4_ENCODING) {
kikoaac 1:1f4e8614d0ed 161
kikoaac 1:1f4e8614d0ed 162 if (((currState_ ^ prevState_) != INVALID) && (currState_ != prevState_)) {
kikoaac 1:1f4e8614d0ed 163 change = (prevState_ & PREV_MASK) ^ ((currState_ & CURR_MASK) >> 1);
kikoaac 1:1f4e8614d0ed 164
kikoaac 1:1f4e8614d0ed 165 if (change == 0) {
kikoaac 1:1f4e8614d0ed 166 change = -1;
kikoaac 1:1f4e8614d0ed 167 }
kikoaac 1:1f4e8614d0ed 168
kikoaac 1:1f4e8614d0ed 169 pulses_ -= change;
kikoaac 1:1f4e8614d0ed 170 angle_pulses -= change;
kikoaac 1:1f4e8614d0ed 171 }
kikoaac 1:1f4e8614d0ed 172
kikoaac 1:1f4e8614d0ed 173 }
kikoaac 1:1f4e8614d0ed 174 angle_ = angle_pulses*360/((double)pulsesPerRev_*4);
kikoaac 1:1f4e8614d0ed 175 sumangle = pulses_*360/((double)pulsesPerRev_*4);
kikoaac 1:1f4e8614d0ed 176 if(angle_>=360)
kikoaac 1:1f4e8614d0ed 177 {
kikoaac 1:1f4e8614d0ed 178 angle_pulses = angle_ = 0;
kikoaac 1:1f4e8614d0ed 179 round_rev++;
kikoaac 1:1f4e8614d0ed 180 }
kikoaac 1:1f4e8614d0ed 181 else if(angle_<=-360)
kikoaac 1:1f4e8614d0ed 182 {
kikoaac 1:1f4e8614d0ed 183 angle_pulses = angle_ = 0;
kikoaac 1:1f4e8614d0ed 184 round_rev--;
kikoaac 1:1f4e8614d0ed 185 }
kikoaac 1:1f4e8614d0ed 186 prevState_ = currState_;
kikoaac 2:6a38785d5f0c 187 NVIC_EnableIRQ( I2C_IRQn ) ;
kikoaac 1:1f4e8614d0ed 188 }
kikoaac 1:1f4e8614d0ed 189
kikoaac 1:1f4e8614d0ed 190 void QEI::index(void) {
kikoaac 1:1f4e8614d0ed 191
kikoaac 1:1f4e8614d0ed 192 revolutions_++;
kikoaac 1:1f4e8614d0ed 193
kikoaac 1:1f4e8614d0ed 194 }
kikoaac 1:1f4e8614d0ed 195