#include "Encoder.h" /* Encoder(int encA, int encB , int cpr, int index) - encA, encB - encoder A and B pins - cpr - counts per rotation number (cpm=ppm*4) - index pin - (optional input) */ Encoder::Encoder(int _encA, int _encB , float _ppr, int _index){ // Encoder measurement structure init // hardware pins pinA = _encA; pinB = _encB; // counter setup pulse_counter = 0; pulse_timestamp = 0; cpr = _ppr; A_active = 0; B_active = 0; I_active = 0; // index pin index_pin = _index; // its 0 if not used // velocity calculation variables prev_Th = 0; pulse_per_second = 0; prev_pulse_counter = 0; prev_timestamp_us = _micros(); // extern pullup as default pullup = Pullup::USE_EXTERN; // enable quadrature encoder by default quadrature = Quadrature::ON; } // Encoder interrupt callback functions // A channel void Encoder::handleA() { bool A = digitalRead(pinA); switch (quadrature){ case Quadrature::ON: // CPR = 4xPPR if ( A != A_active ) { pulse_counter += (A_active == B_active) ? 1 : -1; pulse_timestamp = _micros(); A_active = A; } break; case Quadrature::OFF: // CPR = PPR if(A && !digitalRead(pinB)){ pulse_counter++; pulse_timestamp = _micros(); } break; } } // B channel void Encoder::handleB() { bool B = digitalRead(pinB); switch (quadrature){ case Quadrature::ON: // // CPR = 4xPPR if ( B != B_active ) { pulse_counter += (A_active != B_active) ? 1 : -1; pulse_timestamp = _micros(); B_active = B; } break; case Quadrature::OFF: // CPR = PPR if(B && !digitalRead(pinA)){ pulse_counter--; pulse_timestamp = _micros(); } break; } } // Index channel void Encoder::handleIndex() { if(hasIndex()){ bool I = digitalRead(index_pin); if(I && !I_active){ index_found = true; // align encoder on each index long tmp = pulse_counter; // corrent the counter value pulse_counter = round((double)pulse_counter/(double)cpr)*cpr; // preserve relative speed prev_pulse_counter += pulse_counter - tmp; } I_active = I; } } // Sensor update function. Safely copy volatile interrupt variables into Sensor base class state variables. void Encoder::update() { // Copy volatile variables in minimal-duration blocking section to ensure no interrupts are missed noInterrupts(); angle_prev_ts = pulse_timestamp; long copy_pulse_counter = pulse_counter; interrupts(); // TODO: numerical precision issue here if the pulse_counter overflows the angle will be lost full_rotations = copy_pulse_counter / (int)cpr; angle_prev = _2PI * ((copy_pulse_counter) % ((int)cpr)) / ((float)cpr); } /* Shaft angle calculation */ float Encoder::getSensorAngle(){ return _2PI * (pulse_counter) / ((float)cpr); } /* Shaft velocity calculation function using mixed time and frequency measurement technique */ float Encoder::getVelocity(){ // Copy volatile variables in minimal-duration blocking section to ensure no interrupts are missed noInterrupts(); long copy_pulse_counter = pulse_counter; long copy_pulse_timestamp = pulse_timestamp; interrupts(); // timestamp long timestamp_us = _micros(); // sampling time calculation float Ts = (timestamp_us - prev_timestamp_us) * 1e-6f; // quick fix for strange cases (micros overflow) if(Ts <= 0 || Ts > 0.5f) Ts = 1e-3f; // time from last impulse float Th = (timestamp_us - copy_pulse_timestamp) * 1e-6f; long dN = copy_pulse_counter - prev_pulse_counter; // Pulse per second calculation (Eq.3.) // dN - impulses received // Ts - sampling time - time in between function calls // Th - time from last impulse // Th_1 - time form last impulse of the previous call // only increment if some impulses received float dt = Ts + prev_Th - Th; pulse_per_second = (dN != 0 && dt > Ts/2) ? dN / dt : pulse_per_second; // if more than 0.05f passed in between impulses if ( Th > 0.1f) pulse_per_second = 0; // velocity calculation float velocity = pulse_per_second / ((float)cpr) * (_2PI); // save variables for next pass prev_timestamp_us = timestamp_us; // save velocity calculation variables prev_Th = Th; prev_pulse_counter = copy_pulse_counter; return velocity; } // getter for index pin // return -1 if no index int Encoder::needsSearch(){ return hasIndex() && !index_found; } // private function used to determine if encoder has index int Encoder::hasIndex(){ return index_pin != 0; } // encoder initialisation of the hardware pins // and calculation variables void Encoder::init(){ // Encoder - check if pullup needed for your encoder if(pullup == Pullup::USE_INTERN){ pinMode(pinA, INPUT_PULLUP); pinMode(pinB, INPUT_PULLUP); if(hasIndex()) pinMode(index_pin,INPUT_PULLUP); }else{ pinMode(pinA, INPUT); pinMode(pinB, INPUT); if(hasIndex()) pinMode(index_pin,INPUT); } // counter setup pulse_counter = 0; pulse_timestamp = _micros(); // velocity calculation variables prev_Th = 0; pulse_per_second = 0; prev_pulse_counter = 0; prev_timestamp_us = _micros(); // initial cpr = PPR // change it if the mode is quadrature if(quadrature == Quadrature::ON) cpr = 4*cpr; // we don't call Sensor::init() here because init is handled in Encoder class. } // function enabling hardware interrupts of the for the callback provided // if callback is not provided then the interrupt is not enabled void Encoder::enableInterrupts(void (*doA)(), void(*doB)(), void(*doIndex)()){ // attach interrupt if functions provided switch(quadrature){ case Quadrature::ON: // A callback and B callback if(doA != nullptr) attachInterrupt(digitalPinToInterrupt(pinA), doA, CHANGE); if(doB != nullptr) attachInterrupt(digitalPinToInterrupt(pinB), doB, CHANGE); break; case Quadrature::OFF: // A callback and B callback if(doA != nullptr) attachInterrupt(digitalPinToInterrupt(pinA), doA, RISING); if(doB != nullptr) attachInterrupt(digitalPinToInterrupt(pinB), doB, RISING); break; } // if index used initialize the index interrupt if(hasIndex() && doIndex != nullptr) attachInterrupt(digitalPinToInterrupt(index_pin), doIndex, CHANGE); }