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@0:441289ea4e29, 2021-05-27 (annotated)

Committer:
krogedal
Date:
Thu May 27 18:36:23 2021 +0000
Revision:
0:441289ea4e29
Child:
1:7c355adbc977
Set up basic motor control

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