Ryoga Sato / QEI

QEI

Dependents:   robocon19_ros

QEI.cpp@1:4b2681ff734d, 2019-06-20 (annotated)

Committer:
surpace0924
Date:
Thu Jun 20 23:50:19 2019 +0000
Revision:
1:4b2681ff734d
Parent:
0:a24686ca50ab 
fix;

Who changed what in which revision?

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