kashiko aoyama / QEI

r_test1_2017_10_11_Wed_A

Fork of QEI by Aaron Berk

QEI.cpp@1:8658787f9ec5, 2017-10-11 (annotated)

Committer:
shobonwarrior
Date:
Wed Oct 11 08:14:24 2017 +0000
Revision:
1:8658787f9ec5
Parent:
0:5c2ad81551aa 
r_test1_2017/10/11/Wed_A

Who changed what in which revision?

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