encoder.cpp

00001 /* Karbot encoder class
00002  *
00003  * This class is based upon the QEI class by Aaron Berk and the encoder class
00004  * I wrote during ESP in 2nd year
00005  *
00006  * Written by Simon Krogedal
00007  * 27/05/21
00008  * Team 9 4th Year project
00009  * 
00010  * for NUCLEO-F401RE
00011  * 
00012  */
00013  
00014 #include "encoder.h"
00015 
00016 #define PI 3.1515
00017  
00018  
00019 encoder::encoder(
00020     PinName chanA,
00021     PinName chanB,
00022     int CPR,
00023     Side side,
00024     double Period,
00025     double diameter
00026     ) : channelA_(chanA, PullUp),
00027         channelB_(chanB, PullUp),
00028         pulsesPerRev_(CPR),
00029         side_(side),
00030         period(Period) {
00031                         
00032     enc_const = ((diameter * PI) / ((double)(4 * CPR)));
00033     
00034     pulses_       = 0;
00035     tot_clicks    = 0;
00036     click_rate    = 0;
00037     temp_tot      = 0;
00038     
00039     //Workout what the current state is.
00040     int channA = channelA_.read();
00041     int channB = channelB_.read();
00042     
00043     //2-bit state.
00044     currState_ = (channA << 1) | (channB);
00045     prevState_ = currState_;
00046     
00047     //X4 encoding uses interrupts on channel A,
00048     //and on channel B.
00049     channelA_.rise(callback(this, &encoder::encode));
00050     channelA_.fall(callback(this, &encoder::encode));
00051     channelB_.rise(callback(this, &encoder::encode));
00052     channelB_.fall(callback(this, &encoder::encode));
00053 }
00054 
00055 void encoder::reset(void) {
00056     pulses_      = 0;
00057 }
00058 
00059 int encoder::getCurrentState(void) {
00060 
00061     return currState_;
00062 
00063 }
00064 
00065 int encoder::getPulses(void) {
00066 
00067     return pulses_;
00068 
00069 }
00070 
00071 // +-------------+
00072 // | X2 Encoding |
00073 // +-------------+
00074 //
00075 // When observing states two patterns will appear:
00076 //
00077 // Counter clockwise rotation:
00078 //
00079 // 10 -> 01 -> 10 -> 01 -> ...
00080 //
00081 // Clockwise rotation:
00082 //
00083 // 11 -> 00 -> 11 -> 00 -> ...
00084 //
00085 // We consider counter clockwise rotation to be "forward" and
00086 // counter clockwise to be "backward". Therefore pulse count will increase
00087 // during counter clockwise rotation and decrease during clockwise rotation.
00088 //
00089 // +-------------+
00090 // | X4 Encoding |
00091 // +-------------+
00092 //
00093 // There are four possible states for a quadrature encoder which correspond to
00094 // 2-bit gray code.
00095 //
00096 // A state change is only valid if of only one bit has changed.
00097 // A state change is invalid if both bits have changed.
00098 //
00099 // Clockwise Rotation ->
00100 //
00101 //    00 01 11 10 00
00102 //
00103 // <- Counter Clockwise Rotation
00104 //
00105 // If we observe any valid state changes going from left to right, we have
00106 // moved one pulse clockwise [we will consider this "backward" or "negative"].
00107 //
00108 // If we observe any valid state changes going from right to left we have
00109 // moved one pulse counter clockwise [we will consider this "forward" or
00110 // "positive"].
00111 //
00112 // We might enter an invalid state for a number of reasons which are hard to
00113 // predict - if this is the case, it is generally safe to ignore it, update
00114 // the state and carry on, with the error correcting itself shortly after.
00115 void encoder::encode(void) {
00116 
00117     int change = 0;
00118     int chanA  = channelA_.read();
00119     int chanB  = channelB_.read();
00120 
00121     //2-bit state.
00122     currState_ = (chanA << 1) | (chanB);
00123 
00124         //Entered a new valid state.
00125         if (((currState_ ^ prevState_) != INVALID) && (currState_ != prevState_)) {
00126             //2 bit state. Right hand bit of prev XOR left hand bit of current
00127             //gives 0 if clockwise rotation and 1 if counter clockwise rotation.
00128             change = (prevState_ & PREV_MASK) ^ ((currState_ & CURR_MASK) >> 1);
00129 
00130             if (change == 0) {
00131                 change = -1;
00132             }
00133 
00134             pulses_ -= change;
00135         }
00136 
00137     prevState_ = currState_;
00138 
00139 }
00140 
00141 void encoder::sample_func(void) {
00142     int clicks = getPulses();
00143     click_rate = ((double)clicks / period);
00144     reset();                                                    //reset clicks
00145     tot_clicks += clicks;
00146     temp_tot += clicks;
00147 }
00148 
00149 double encoder::getClicks(void) {
00150 //            test_sample();
00151     if(side_ == Left)
00152         return click_rate;
00153     else
00154         return -click_rate;
00155 }
00156 
00157 double encoder::getSpeed(void) {
00158     double s = click_rate * enc_const;                          //angular frequency = 2pi*CPS/CPR and v = omega*r
00159     if(side_ == Left)
00160         return s;
00161     else
00162         return -s;
00163 }
00164 
00165 double encoder::getDistance(void) {                                      //calculates total distance and returns it
00166 //            test_sample();
00167     double d = ((double)click_store * enc_const);
00168     if(side_ == Left)
00169         return d;
00170     else
00171         return -d;
00172 }
00173 
00174 double encoder::tempDist(void) {                                     //calculates total distance and returns it
00175 //            test_sample();
00176     double d = ((double)temp_tot * enc_const);
00177     if(c_d)
00178         return d;
00179     else
00180         return -d;
00181 }
00182 
00183 void encoder::distRst(void) {
00184     int temp;
00185     if(side_ == Left)
00186         temp = tot_clicks;
00187     else
00188         temp = -tot_clicks;
00189         
00190     if(temp > click_store)
00191         click_store = temp;
00192     tot_clicks = 0;
00193 }
00194 
00195 void encoder::tempRst(void) {temp_tot = 0;}
00196 
00197 void encoder::start(void) {sampler.attach(callback(this, &encoder::sample_func), period);}