Hello!
I am trying to control the position of a dc motor with an encoder.
Until now works fine, the motor can reach any desired position with help of a pid loop.
The problem starts when i try to apply some force to move it:
If i move the motor in one way it works just fine.
But when in force the motor the other way it goes crazy (I mean the reading on the encoder in that way is fine but only if there is no power on the motor, whenever i power the motor the encoder works fine only in one direction):
#include <util/atomic.h>
#define ENCA 2
#define ENCB 3
#define PWM 5
#define IN2 6
#define IN1 7
volatile int posi = 0;
long prevT = 0;
float eprev = 0;
float eintegral = 0;
void setup() {
Serial.begin(9600);
pinMode(ENCA,INPUT);
pinMode(ENCB,INPUT);
attachInterrupt(digitalPinToInterrupt(ENCA),readEncoder,RISING);
pinMode(PWM,OUTPUT);
pinMode(IN1,OUTPUT);
pinMode(IN2,OUTPUT);
Serial.println("target pos");
}
void loop() {
int target = 1200;
// PID constants
float kp = 1;
float kd = 0.025;
float ki = 0.0;
long currT = micros();
float deltaT = ((float) (currT - prevT))/( 1.0e6 );
prevT = currT;
int pos = 0;
ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
pos = posi;
}
// error
int 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.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){
posi++;
}
else{
posi--;
}
}
Is there a way to fix this?
I was also thinking bout making some filters (like a low pass filters or so).
Any advice or help would be great.
Thank you!
Does this mean that the encoder is losing position or not?
How fast are things running? It may not be your problem, but the time interval of the control loop should be on the order of 1/10 of the time-constant of the system under control. Do you know how fast loop() is running and how fast the system responds? I'm always suspicious of PID when it doesn't control or track the delta-t. With the 9600 baud and the serial printing, your delta-t might be changing with time, which affects the derivative and integral calculations, and then you might get some odd interactions. I'd increase the baud rate and wrap the PID control in a BWOD if(micros() - last >= interval){last+=interval;...} construct.
Here i forcefully tried to keep the motor in the same spot and the graph looks like an ekg.
When i move it the otherway it just stays nicely in place and when i let go it goes back to the target position.
From what i understand you mean Shannon Therorem. I increased the sample time and the result isnt getting better.
The same thing happens when i use a simple proportional controller, without derivative or intergrative, so i dont think the sample time is the problem here.
I do have a quadrature encoder, i have two chanels, A and B, and i can tell the direction the motor is spinning by seeing wich one of those channels is HIGH first.
In other words if i see wich channel leads the other i can determine the direction, since the posi++ if chanel A is already HIGH, and posi-- if chanel A is LOW.
My concern is not Shannon information, but that if the process moves much slower/faster than the control system, like a free-running Arduino 10ms loop() controlling a house thermostat, the integral term can blow up before the furnace turns on, or that the derivative of house temperature over 10ms is only sensor noise; or conversely a slow human trying to control a short balancing robot, when it could fall all the way over by the time the human sees it.
Stick-built PIDs can run at very different speeds than the system they are controlling, and that often causes weird errors.
This could be a speed/timing issue as well -- that encoder library is fast. Your method should work at some speeds, since the readEncoder is called on the rising edge of ENCA:
If you can act on ENCB before another state change, it should work fine:
The encoder library has faster reading, and the logic and capability to detect, for example, if ENCA fell again by the time you get around to reading ENCB.
The producer specifies that there are 64 counts per revolution (rising and faling combined, A and B together).
Thats right, i think the encoder library takes all the posibilities in count.
Im gonna go and take a look in the library code and see.
The application for this motor is a robot arm, so i think that the speed of this process is much more slower than the speed of the control system. The point for me now is to have a base code, wich will recieve some data (angles and speed) from ROS and rotate the motor accordingly.
If you are running on Uno or a similar 16MHz controller, you might consider moving one of the encoder channels to a non-interrupt pin to save half the processing time.
Relative to your other code, 1200 rising ENCA edges should be the same shaft angle as 4800 quadrature edges as measured by the Encoder library.
