Hey all,
So I'm trying to read an rotary encoder by creating a loop of possible states (finite state machine). Basically, there are two values in an array that track the A and B phases of the encoder. When either phase is high, the value is 1 and when the phase is low the value is 0. By this, the state loop is as follows:
[0 0] - Both Phases Low
[0 1] - B phase High, A phase Low
[1 1] - Both Phases High
[1 0] - A phase high, B phase Low
[0 0] - Both phases low, back to the original state
I implement this by having pin change interrupts on the A and B phases of the encoder, and the interrupt service routine then checks the state of the pin to determine whether it has risen high or dropped low, and modifies the state and encoder count accordingly. Each state loop contributes two counts, so going forward adds two counts and going backward subtracts two counts. I don't know why, but when I monitor the state I get weird values like -26214 and 9612 written to the state variables rather than 0 and 1. This causes me to miss counts of the encoder. This should all be handled by the interrupts at the bottom of my code. I had a more simple method to track the encoder previously and it worked decently so I think the encoder hardware is fine. If anyone could check this out and let me know if you spot an issue, that would be very much appreciated. Thank you all for your time and help!
volatile int totalRotations = 0, target = 0, targetAdd, counter = 0, targetInProg, eCounter;
volatile float dgrees = 0, kp, ki, kd, dt, targetAddD, u, dedt, eint, e, previousE;
volatile long previousT, currentT;
const int ppr = 600;
#define aphase 3 //white wire encoder phase a
#define bphase 2 //green wire encoder phase b
#define pwm 6 //pwm control pin 1
#define joystick 0 //joystick in analog pin 0
#define enableh 8
#define redout 13
#define blackout 12
#define reset 11
int joypos;
int pwmVal;
int state[1];
void setup() {
digitalWrite(reset, HIGH);
Serial.begin(115200);
pinMode(aphase, INPUT_PULLUP);
pinMode(bphase, INPUT_PULLUP);
pinMode(enableh, OUTPUT);
pinMode(redout, OUTPUT);
pinMode(blackout, OUTPUT);
pinMode(pwm, OUTPUT);
pinMode(reset, OUTPUT);
digitalWrite(enableh, HIGH);
attachInterrupt(digitalPinToInterrupt(bphase), ai0, CHANGE);
attachInterrupt(digitalPinToInterrupt(aphase), ai1, CHANGE);
previousT = 0;
previousE = 0.0;
eint = 0;
targetInProg = 0;
//Assign kp,ki, and kd values
Serial.println("Kp:");
while (Serial.available() == 0) {}
kp = Serial.parseFloat();
Serial.print("kp=");
Serial.println(kp);
Serial.println("Ki:");
while (Serial.available() == 0) {}
ki = Serial.parseFloat();
Serial.print("ki=");
Serial.println(ki);
Serial.println("Kd:");
while (Serial.available() == 0) {}
kd = Serial.parseFloat();
Serial.print("kd=");
Serial.println(kd);
delay(500);
}
void loop() {
//If Serial Monitor has input, read that input for a degree value, convert to counter value, and create target
if ((Serial.available() > 0) && (targetInProg == 0)) {
targetAddD = Serial.parseFloat();
//Reset when input is 0, otherwise set target
if (targetAddD == 0) {
digitalWrite(reset, LOW);
}
else {
targetAdd = targetAddD * 1200 / 360;
target = target + targetAdd;
targetInProg = 1;
}
}
//Timing
currentT = micros();
dt = ((float)currentT - (float)previousT) / (1.0e6);
previousT = currentT;
//Calculate Error and parameters for PID using Finite Difference
e = target - counter;
dedt = ((float)e - (float)previousE) / dt;
eint = eint + (float)e * dt;
//Compute Control Signal and send to motor
float u = (float)kp * (float)e + ki * eint + kd * dedt;
if (u > 0) {
if (u > 255) {
u = 255;
}
analogWrite(redout, fabs(u));
digitalWrite(blackout, LOW);
}
if (u < 0) {
if (u < -255) {
u = -255;
}
analogWrite(blackout, fabs(u));
digitalWrite(redout, LOW);
}
//read Counter and Degrees to the Serial Monitor
//Serial.print("Counter: ");
//Serial.println(counter);
dgrees = counter * 360 / 1200.0 - 360.0 * totalRotations;
if ( dgrees >= 360.0) {
dgrees = dgrees - 360;
totalRotations++;
}
if (dgrees <= -360.0) {
dgrees - dgrees + 360.0;
totalRotations--;
}
//Serial.print("Target: ");
//Serial.println(target);
//Serial.print("Error: ");
Serial.println(e);
Serial.print(state[0]);
Serial.print(" ");
Serial.println(state[1]);
//Serial.print("DT: ");
//Serial.println(dt,6);
//Serial.print("targetInProg: ");
//Serial.println(targetInProg);
//When error is equal for 15 loops in a row during a target operation, kill the target operation and read "Target Reached"
if ((targetInProg == 1) && (previousE == e)) {
eCounter++;
}
if ((eCounter == 15) && (targetInProg == 1)) {
targetInProg = 0;
Serial.println("Target Reached");
eCounter = 0;
}
//Record error for next loop
previousE = e;
}
void ai0() {
//B Change
if (digitalRead(bphase) == HIGH) {
//B Rising
if (state[0] == 0 && state[1] == 0) {
counter--;
state[1] = 1;
}
else {
state[1] = 1;
}
}
else {
//B Falling
if (state[0] == 0 && state[1] == 1)
{
state[1] = 0;
counter++;
}
else {
state[1] = 0;
}
}
}
void ai1() {
//A Change
if (digitalRead(aphase) == HIGH) {
//A Rising
if (state[0] == 0 && state[1] == 0) {
counter++;
state[0] = 1;
}
else {
state[0] = 1;
}
}
else {
//A Falling
if (state[0] == 1 && state[1] == 0) {
state[0] = 0;
counter--;
}
else {
state[0] = 0;
}
}
}
This is the serial monitor output of the state and the error. Error (target position - current position) is marked in red and the state is marked in blue, see how the state has numbers that are not 1 and 0
