hidaka sato / QEI

This is made by Ihimoto Kiko ,BEBE.

Dependents:   2016_slave_rorikon_2 2016_slave_MD_rorikon 2016_slave_MD_rorikon

Fork of QEI by Kiko Ishimoto

QEI.cpp@1:b9a5e2d6fbb9, 2016-08-16 (annotated)

Committer:
sgrsn
Date:
Tue Aug 16 04:46:40 2016 +0000
Revision:
1:b9a5e2d6fbb9
Parent:
0:a24686ca50ab 
a

Who changed what in which revision?

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