Simon Krogedal / Lift_arm_control

Control project for the Lift-arm. Works with ROS Melodic

Dependencies:   mbed Servo ros_lib_melodic ULN2003_StepperDriver Async_4pin_Stepper

src/encoder.cpp@4:9edb248c6431, 2021-05-31 (annotated)

Committer:
krogedal
Date:
Mon May 31 16:47:02 2021 +0000
Revision:
4:9edb248c6431
Parent:
3:4b6080e86761 
Updated to work with ROS

Who changed what in which revision?

UserRevisionLine numberNew contents of line
krogedal 0:441289ea4e29 1 #ifndef ENCODER_H
krogedal 0:441289ea4e29 2 #define ENCODER_H
krogedal 0:441289ea4e29 3
krogedal 0:441289ea4e29 4 /* Karbot encoder class
krogedal 3:4b6080e86761 5 *
krogedal 3:4b6080e86761 6 * This class is based upon the QEI class by Aaron Berk and the encoder class
krogedal 3:4b6080e86761 7 * I wrote during ESP in 2nd year
krogedal 3:4b6080e86761 8 *
krogedal 0:441289ea4e29 9 * Written by Simon Krogedal
krogedal 0:441289ea4e29 10 * 27/05/21
krogedal 0:441289ea4e29 11 * Team 9 4th Year project
krogedal 0:441289ea4e29 12 *
krogedal 0:441289ea4e29 13 * for NUCLEO-F401RE
krogedal 0:441289ea4e29 14 *
krogedal 0:441289ea4e29 15 */
krogedal 0:441289ea4e29 16
krogedal 0:441289ea4e29 17 #include "encoder.h"
krogedal 0:441289ea4e29 18
krogedal 0:441289ea4e29 19
krogedal 0:441289ea4e29 20 encoder::encoder(
krogedal 0:441289ea4e29 21 PinName chanA,
krogedal 0:441289ea4e29 22 PinName chanB,
krogedal 0:441289ea4e29 23 int CPR,
krogedal 0:441289ea4e29 24 bool c,
krogedal 0:441289ea4e29 25 double p,
krogedal 0:441289ea4e29 26 double ec
krogedal 1:7c355adbc977 27 ) : channelA_(chanA, PullUp),
krogedal 1:7c355adbc977 28 channelB_(chanB, PullUp),
krogedal 0:441289ea4e29 29 c_d(c),
krogedal 0:441289ea4e29 30 period(p),
krogedal 0:441289ea4e29 31 enc_const(ec) {
krogedal 0:441289ea4e29 32
krogedal 0:441289ea4e29 33 pulses_ = 0;
krogedal 0:441289ea4e29 34 pulsesPerRev_ = CPR;
krogedal 0:441289ea4e29 35 tot_clicks = 0;
krogedal 0:441289ea4e29 36 click_rate = 0;
krogedal 0:441289ea4e29 37 temp_tot = 0;
krogedal 0:441289ea4e29 38
krogedal 0:441289ea4e29 39 //Workout what the current state is.
krogedal 1:7c355adbc977 40 int channA = channelA_.read();
krogedal 1:7c355adbc977 41 int channB = channelB_.read();
krogedal 0:441289ea4e29 42
krogedal 0:441289ea4e29 43 //2-bit state.
krogedal 1:7c355adbc977 44 currState_ = (channA << 1) | (channB);
krogedal 0:441289ea4e29 45 prevState_ = currState_;
krogedal 0:441289ea4e29 46
krogedal 0:441289ea4e29 47 //X4 encoding uses interrupts on channel A,
krogedal 0:441289ea4e29 48 //and on channel B.
krogedal 1:7c355adbc977 49 channelA_.rise(callback(this, &encoder::encode));
krogedal 1:7c355adbc977 50 channelA_.fall(callback(this, &encoder::encode));
krogedal 1:7c355adbc977 51 channelB_.rise(callback(this, &encoder::encode));
krogedal 1:7c355adbc977 52 channelB_.fall(callback(this, &encoder::encode));
krogedal 0:441289ea4e29 53 }
krogedal 0:441289ea4e29 54
krogedal 1:7c355adbc977 55 void encoder::reset(void) {
krogedal 0:441289ea4e29 56 pulses_ = 0;
krogedal 0:441289ea4e29 57 }
krogedal 0:441289ea4e29 58
krogedal 1:7c355adbc977 59 int encoder::getCurrentState(void) {
krogedal 0:441289ea4e29 60
krogedal 0:441289ea4e29 61 return currState_;
krogedal 0:441289ea4e29 62
krogedal 0:441289ea4e29 63 }
krogedal 0:441289ea4e29 64
krogedal 1:7c355adbc977 65 int encoder::getPulses(void) {
krogedal 0:441289ea4e29 66
krogedal 0:441289ea4e29 67 return pulses_;
krogedal 0:441289ea4e29 68
krogedal 0:441289ea4e29 69 }
krogedal 0:441289ea4e29 70
krogedal 0:441289ea4e29 71 // +-------------+
krogedal 0:441289ea4e29 72 // | X2 Encoding |
krogedal 0:441289ea4e29 73 // +-------------+
krogedal 0:441289ea4e29 74 //
krogedal 0:441289ea4e29 75 // When observing states two patterns will appear:
krogedal 0:441289ea4e29 76 //
krogedal 0:441289ea4e29 77 // Counter clockwise rotation:
krogedal 0:441289ea4e29 78 //
krogedal 0:441289ea4e29 79 // 10 -> 01 -> 10 -> 01 -> ...
krogedal 0:441289ea4e29 80 //
krogedal 0:441289ea4e29 81 // Clockwise rotation:
krogedal 0:441289ea4e29 82 //
krogedal 0:441289ea4e29 83 // 11 -> 00 -> 11 -> 00 -> ...
krogedal 0:441289ea4e29 84 //
krogedal 0:441289ea4e29 85 // We consider counter clockwise rotation to be "forward" and
krogedal 0:441289ea4e29 86 // counter clockwise to be "backward". Therefore pulse count will increase
krogedal 0:441289ea4e29 87 // during counter clockwise rotation and decrease during clockwise rotation.
krogedal 0:441289ea4e29 88 //
krogedal 0:441289ea4e29 89 // +-------------+
krogedal 0:441289ea4e29 90 // | X4 Encoding |
krogedal 0:441289ea4e29 91 // +-------------+
krogedal 0:441289ea4e29 92 //
krogedal 0:441289ea4e29 93 // There are four possible states for a quadrature encoder which correspond to
krogedal 0:441289ea4e29 94 // 2-bit gray code.
krogedal 0:441289ea4e29 95 //
krogedal 0:441289ea4e29 96 // A state change is only valid if of only one bit has changed.
krogedal 0:441289ea4e29 97 // A state change is invalid if both bits have changed.
krogedal 0:441289ea4e29 98 //
krogedal 0:441289ea4e29 99 // Clockwise Rotation ->
krogedal 0:441289ea4e29 100 //
krogedal 0:441289ea4e29 101 // 00 01 11 10 00
krogedal 0:441289ea4e29 102 //
krogedal 0:441289ea4e29 103 // <- Counter Clockwise Rotation
krogedal 0:441289ea4e29 104 //
krogedal 0:441289ea4e29 105 // If we observe any valid state changes going from left to right, we have
krogedal 0:441289ea4e29 106 // moved one pulse clockwise [we will consider this "backward" or "negative"].
krogedal 0:441289ea4e29 107 //
krogedal 0:441289ea4e29 108 // If we observe any valid state changes going from right to left we have
krogedal 0:441289ea4e29 109 // moved one pulse counter clockwise [we will consider this "forward" or
krogedal 0:441289ea4e29 110 // "positive"].
krogedal 0:441289ea4e29 111 //
krogedal 0:441289ea4e29 112 // We might enter an invalid state for a number of reasons which are hard to
krogedal 0:441289ea4e29 113 // predict - if this is the case, it is generally safe to ignore it, update
krogedal 0:441289ea4e29 114 // the state and carry on, with the error correcting itself shortly after.
krogedal 0:441289ea4e29 115 void encoder::encode(void) {
krogedal 0:441289ea4e29 116
krogedal 0:441289ea4e29 117 int change = 0;
krogedal 0:441289ea4e29 118 int chanA = channelA_.read();
krogedal 0:441289ea4e29 119 int chanB = channelB_.read();
krogedal 0:441289ea4e29 120
krogedal 0:441289ea4e29 121 //2-bit state.
krogedal 0:441289ea4e29 122 currState_ = (chanA << 1) | (chanB);
krogedal 0:441289ea4e29 123
krogedal 0:441289ea4e29 124 //Entered a new valid state.
krogedal 0:441289ea4e29 125 if (((currState_ ^ prevState_) != INVALID) && (currState_ != prevState_)) {
krogedal 0:441289ea4e29 126 //2 bit state. Right hand bit of prev XOR left hand bit of current
krogedal 0:441289ea4e29 127 //gives 0 if clockwise rotation and 1 if counter clockwise rotation.
krogedal 0:441289ea4e29 128 change = (prevState_ & PREV_MASK) ^ ((currState_ & CURR_MASK) >> 1);
krogedal 0:441289ea4e29 129
krogedal 0:441289ea4e29 130 if (change == 0) {
krogedal 0:441289ea4e29 131 change = -1;
krogedal 0:441289ea4e29 132 }
krogedal 0:441289ea4e29 133
krogedal 0:441289ea4e29 134 pulses_ -= change;
krogedal 0:441289ea4e29 135 }
krogedal 0:441289ea4e29 136
krogedal 0:441289ea4e29 137 prevState_ = currState_;
krogedal 0:441289ea4e29 138
krogedal 0:441289ea4e29 139 }
krogedal 0:441289ea4e29 140
krogedal 0:441289ea4e29 141 void encoder::sample_func(void) {
krogedal 0:441289ea4e29 142 int clicks = getPulses();
krogedal 0:441289ea4e29 143 click_rate = ((double)clicks / period);
krogedal 0:441289ea4e29 144 reset(); //reset clicks
krogedal 0:441289ea4e29 145 tot_clicks += clicks;
krogedal 0:441289ea4e29 146 temp_tot += clicks;
krogedal 0:441289ea4e29 147 }
krogedal 0:441289ea4e29 148
krogedal 0:441289ea4e29 149 double encoder::getClicks(void) {
krogedal 0:441289ea4e29 150 // test_sample();
krogedal 0:441289ea4e29 151 if(c_d)
krogedal 0:441289ea4e29 152 return click_rate;
krogedal 0:441289ea4e29 153 else
krogedal 0:441289ea4e29 154 return -click_rate;
krogedal 0:441289ea4e29 155 }
krogedal 0:441289ea4e29 156
krogedal 0:441289ea4e29 157 double encoder::getSpeed(void) {
krogedal 0:441289ea4e29 158 double s = click_rate * enc_const; //angular frequency = 2pi*CPS/CPR and v = omega*r
krogedal 0:441289ea4e29 159 if(c_d)
krogedal 0:441289ea4e29 160 return s;
krogedal 0:441289ea4e29 161 else
krogedal 0:441289ea4e29 162 return -s;
krogedal 0:441289ea4e29 163 }
krogedal 0:441289ea4e29 164
krogedal 0:441289ea4e29 165 double encoder::getDistance(void) { //calculates total distance and returns it
krogedal 0:441289ea4e29 166 // test_sample();
krogedal 0:441289ea4e29 167 double d = ((double)click_store * enc_const);
krogedal 0:441289ea4e29 168 if(c_d)
krogedal 0:441289ea4e29 169 return d;
krogedal 0:441289ea4e29 170 else
krogedal 0:441289ea4e29 171 return -d;
krogedal 0:441289ea4e29 172 }
krogedal 0:441289ea4e29 173
krogedal 0:441289ea4e29 174 double encoder::tempDist(void) { //calculates total distance and returns it
krogedal 0:441289ea4e29 175 // test_sample();
krogedal 0:441289ea4e29 176 double d = ((double)temp_tot * enc_const);
krogedal 0:441289ea4e29 177 if(c_d)
krogedal 0:441289ea4e29 178 return d;
krogedal 0:441289ea4e29 179 else
krogedal 0:441289ea4e29 180 return -d;
krogedal 0:441289ea4e29 181 }
krogedal 0:441289ea4e29 182
krogedal 0:441289ea4e29 183 void encoder::distRst(void) {
krogedal 0:441289ea4e29 184 int temp;
krogedal 0:441289ea4e29 185 if(c_d)
krogedal 0:441289ea4e29 186 temp = tot_clicks;
krogedal 0:441289ea4e29 187 else
krogedal 0:441289ea4e29 188 temp = -tot_clicks;
krogedal 0:441289ea4e29 189 if(temp > click_store)
krogedal 0:441289ea4e29 190 click_store = temp;
krogedal 0:441289ea4e29 191 tot_clicks = 0;
krogedal 0:441289ea4e29 192 }
krogedal 0:441289ea4e29 193
krogedal 0:441289ea4e29 194 void encoder::tempRst(void) {temp_tot = 0;}
krogedal 0:441289ea4e29 195
krogedal 0:441289ea4e29 196 void encoder::start(void) {sampler.attach(callback(this, &encoder::sample_func), period);}
krogedal 0:441289ea4e29 197
krogedal 0:441289ea4e29 198 #endif