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@1:7c355adbc977, 2021-05-27 (annotated)

Committer:
krogedal
Date:
Thu May 27 18:53:26 2021 +0000
Revision:
1:7c355adbc977
Parent:
0:441289ea4e29
Child:
3:4b6080e86761
fixed a bunch of typos

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