Hello,
I am designing a low pass filter for my Arduino PID controller. Is it possible to determine what the cutoff frequency should be from the image of the plot I attached? I tried a range of values from the like 5 to 10,000 Hz. None worked. For the PID control I used this code:
#include <util/atomic.h>
// Pins
#define ENCA 2
#define ENCB 3
#define PWM 5
#define IN1 6
#define IN2 7
// globals
long prevT = 0;
int posPrev = 0;
// Use the "volatile" directive for variables
// used in an interrupt
volatile int pos_i = 0;
volatile float velocity_i = 0;
volatile long prevT_i = 0;
float v1Filt = 0;
float v1Prev = 0;
float v2Filt = 0;
float v2Prev = 0;
float eintegral = 0;
void setup() {
Serial.begin(115200);
pinMode(ENCA,INPUT);
pinMode(ENCB,INPUT);
pinMode(PWM,OUTPUT);
pinMode(IN1,OUTPUT);
pinMode(IN2,OUTPUT);
attachInterrupt(digitalPinToInterrupt(ENCA),
readEncoder,RISING);
}
void loop() {
// read the position in an atomic block
// to avoid potential misreads
int pos = 0;
float velocity2 = 0;
ATOMIC_BLOCK(ATOMIC_RESTORESTATE){
pos = pos_i;
velocity2 = velocity_i;
}
// Compute velocity with method 1
long currT = micros();
float deltaT = ((float) (currT-prevT))/1.0e6;
float velocity1 = (pos - posPrev)/deltaT;
posPrev = pos;
prevT = currT;
// Convert count/s to RPM
float v1 = velocity1/70.0*60.0;
float v2 = velocity2/70.0*60.0;
// Low-pass filter (25 Hz cutoff)
v1Filt = 0.12019831*v1Filt + 0.43990085*v1 + 0.43990085*v1Prev;
v1Prev = v1;
v2Filt = 0.12019831*v2Filt + 0.43990085*v2 +0.43990085*v2Prev;
v2Prev = v2;
// // Low-pass filter (25 Hz cutoff)
// v1Filt = 0.6*v1Filt + 0.07*v1 + 0.07*v1Prev;
// v1Prev = v1;
// v2Filt = 0.6*v2Filt + 0.07*v2 + 0.07*v2Prev;
// v2Prev = v2;
// Set a target
float vt = 200;
// float vt = 100*(sin(currT/1e6)>0);
// Compute the control signal u
float kp = 5;
float ki = 10;
float e = vt-v1Filt;
eintegral = eintegral + e*deltaT;
float u = kp*e + ki*eintegral;
// Set the motor speed and direction
int dir = 1;
if (u<0){
dir = -1;
}
int pwr = (int) fabs(u);
if(pwr > 255){
pwr = 255;
}
setMotor(dir,pwr,PWM,IN1,IN2);
Serial.print(vt);
Serial.print(" ");
Serial.print(v1Filt);
Serial.println();
delay(1);
}
void setMotor(int dir, int pwmVal, int pwm, int in1, int in2){
analogWrite(pwm,pwmVal); // Motor speed
if(dir == 1){
// Turn one way
digitalWrite(in1,HIGH);
digitalWrite(in2,LOW);
}
else if(dir == -1){
// Turn the other way
digitalWrite(in1,LOW);
digitalWrite(in2,HIGH);
}
else{
// Or dont turn
digitalWrite(in1,LOW);
digitalWrite(in2,LOW);
}
}
void readEncoder(){
// Read encoder B when ENCA rises
int b = digitalRead(ENCB);
int increment = 0;
if(b>0){
// If B is high, increment forward
increment = 1;
}
else{
// Otherwise, increment backward
increment = -1;
}
pos_i = pos_i + increment;
// Compute velocity with method 2
long currT = micros();
float deltaT = ((float) (currT - prevT_i))/1.0e6;
velocity_i = increment/deltaT;
prevT_i = currT;
}
To get the coefficients for the PID control, I used this code: ArduinoTutorials/BasicFilters/Design/LowPass/LowPassFilter.ipynb at main · curiores/ArduinoTutorials · GitHub
Essentially you just take the latter code and input a cutoff frequency which will give you the coefficients you put in the former code. My problem is I don't know how to tell what the cutoff frequency should be from this plot:
