Hello,
I'm currently facing an issue with trying to perform PID position control of a DC motor. The PID loop can in fact be succesfully tuned, however the setpoint of 90° that the code sees is not 90° in reality. Using debugging, the program does oscillate the motor around the setpoint, however if the shaft is manually forced in another position, it reads this new location as 90°. The encoder has been tested without PID control in a separate program, and is confirmed to be working without issues. Is there some interaction with encoders and PID control I am not aware of? I am using a DC motor with the Arduino Uno Board, code is attached below. Thank you all for any help.
#define ENCA 2 // YELLOW
#define ENCB 3 // WHITE
#define PWM 9
#define IN2 7
#define IN1 6
int pos = 0;
long prevT = 0;
float eprev = 0;
float eintegral = 0;
float degree1 = 0;
void setup() {
Serial.begin(9600);
pinMode(ENCA,INPUT);
pinMode(ENCB,INPUT);
attachInterrupt(digitalPinToInterrupt(ENCA),readEncoder,RISING);
Serial.println("target pos e");
}
void loop() {
// set target position
int target = 90.0;
//target = 250*sin(prevT/1e6);
// PID constants
float kp = 100.0;
float kd = 1.0;
float ki = 0.0;
// time difference
long currT = micros();
float deltaT = ((float) (currT - prevT))/( 1.0e6 );
prevT = currT;
degree1 = (pos/1000.00)*360.0;
// error
float e = pos-target;
// derivative
float dedt = (e-eprev)/(deltaT);
// integral
eintegral = eintegral + e*deltaT;
// control signal
float u = kp*e + kd*dedt + ki*eintegral;
// motor power
float pwr = fabs(u);
if( pwr > 255 ){
pwr = 255;
}
// motor direction
int dir = 1;
if(u<0){
dir = -1;
}
// signal the motor
setMotor(dir,pwr,PWM,IN1,IN2);
// store previous error
eprev = e;
Serial.print(target);
Serial.print(" ");
Serial.print(pos);
Serial.print(" ");
Serial.print(e);
Serial.println();
}
void setMotor(int dir, int pwmVal, int pwm, int in1, int in2){
analogWrite(pwm,pwmVal);
if(dir == 1){
digitalWrite(in1,HIGH);
digitalWrite(in2,LOW);
}
else if(dir == -1){
digitalWrite(in1,LOW);
digitalWrite(in2,HIGH);
}
else{
digitalWrite(in1,LOW);
digitalWrite(in2,LOW);
}
}
void readEncoder(){
int b = digitalRead(ENCB);
if(b > 0){
pos++;
}
else{
pos--;
}
}