Рет қаралды 222
How to get accurate rotational speed estimation from a quadrature encoder without counting pulses in a given time. It's rather long starting with theory so you can skip to the demo at the end if you like. Uses an ESP32 micro and a quadrature encoder. Real time demo is shown. Code is here:
//Deconvolution filter to get rotational speed from rotational digital position
volatile int counter = 0;
// Quadrature encoder: change these numbers as required
#define outputA 35
#define outputB 32
// for sampling freq
int sample_pin = 21;
float d1, b0, b1, z0, z1, u0, u1, yout; // d is spectral factor coefficient from polynomial
void IRAM_ATTR isr_quadencoder() {
// quadrature encoder
if (digitalRead(outputA) == digitalRead(outputB)) {
//Clockwise
counter++;
} else {
//Counter Clockwise
counter--;
}
}
boolean running = false;
void setup() {
d1 = -0.0839;
b0 = 0.8392;
b1 = -b0;
z0 = 0.0;
z1 = 0.0;
u0 = 0.0;
u1 = 0.0;
// Serial port for debugging purposes
Serial.begin(57600);
pinMode(outputA, INPUT); //Encoder input A
pinMode(outputB, INPUT); //Encoder input B
pinMode(sample_pin, OUTPUT); //Sampling freq measure with scope
attachInterrupt(outputA, isr_quadencoder, FALLING);
// end setup procedure
}
//while loop
void loop() {
// Deconvolution filter
z1 = z0;
u1 = u0;
u0 = counter;
//z0=0.0839*z1+0.8392*u0-0.8392*u1;
z0 = -d1 * z1 + b0 * u0 + b1 * u1;
yout = 100 * z0;
Serial.print(6000);
Serial.print(", ");
Serial.print(-6000);
Serial.print(", ");
Serial.println(yout);
delay(4);
digitalWrite(sample_pin, running);
running = !running;
// end of while loop
}