[SOLVED] Arduino PID DC Motor Position Control Problem

Projects Programming
1

I'm doing a control engineering project, implementing a PID motor position control for automatic antenna tracking system. The system contain a dc motor, absolute encoder, and a motor driver.

Everything work as expected, but one thing. The motor cannot stop at set point value near 0 degree (350 - 359, 0 - 10 degree). The used code:

#include <PID_v1.h>
int RPWM = 5;
int LPWM = 6;
int L_EN = 7;
int R_EN = 8;
boolean pin_state[10];
byte input_pin[] = {1, 2, 3, 4, 9, 10, 11, 12, 13};
int dec_position = 0;
int dc = 0;
double kp = 50, ki = 45, kd = 2;
double input = 0, output = 0, setpoint = 0;
volatile long encoderPos = 0;
PID myPID(&input, &output, &setpoint, kp, ki, kd, DIRECT);

void setup() {
  pinMode(L_EN, OUTPUT);
  pinMode(R_EN, OUTPUT);
  pinMode(RPWM, OUTPUT);
  pinMode(LPWM, OUTPUT);
  for (byte i = 0; i < 9; i++) {
    pinMode(input_pin[i], INPUT);
  }
  TCCR1B = TCCR1B & 0b11111000 | 1; 
  myPID.SetMode(AUTOMATIC);
  myPID.SetSampleTime(1);
  myPID.SetOutputLimits(-255, 255);          
  digitalWrite(L_EN, HIGH);
  digitalWrite(R_EN, HIGH);
}

void loop() {
  if (Serial.available() > 0) {
    String baca = Serial.readString();
    setpoint = baca.toInt();
  }
  ReadEncoder();
  input = dc;
  myPID.Compute();
  pwmOut(output);
}

void pwmOut(int out) {
  if (out > 0) {
    analogWrite(RPWM, out);//Sets speed variable via PWM
  }
  else {
    analogWrite(LPWM, abs(out));//Sets speed variable via PWM
  }
}

void ReadEncoder() {
// FOR READING ENCODER POSITION, GIVING 0-359 OUTPUT CORRESPOND TO THE ENCODER POSITION
  for (byte i = 0; i < 9; i++) {
    pin_state[i] = !(digitalRead(input_pin[i]));
  }
  dec_position = (pin_state[8] * 256) + (pin_state[7] * 128) + (pin_state[6] * 64) + (pin_state[5] * 32) + (pin_state[4] * 16) + (pin_state[3] * 8) + (pin_state[2] * 4) + (pin_state[1] * 2) + pin_state[0];
  dc = map(dec_position, 0, 500, 0, 360);
}

When the set point is a value between 10 - 350 the sytem worked well. But when it is not, the motor never stop rotating.

I know the problem is due to a little position overshoot cause the encoder to read a very large error.

For instance, if the setpoint is 0 degree, the motor rotate to reach it. Motor rotation is slowing down as its "now" position approaching 0 degree, but the system is not overshoot free. Therefore, even 1 degree overshoot cause the error value is -359 (set point - now position) and the motor rotate again to reach the desired position.

Need help how to overcome this problem. Sorry for bad english.

2

Why doesn't your code have debug prints?

3

AWOL:
Why doesn't your code have debug prints?

because i've removed that in my code on this forum. The one i used, still have the debug print.

4

Didn't the prints tell you anything about how the code was behaving?

5

Does the antenna need to track through the whole 360 degrees ?

6

AWOL:
Didn't the prints tell you anything about how the code was behaving?

Yes it does.
It tell me about the set point, error, and process variable (now position). The system behave correctly.

7

rafisidqi@gmail.com:
The system behave correctly.

Can you please edit the original post and change the title to "[solved]"?

8

AWOL:
Can you please edit the original post and change the title to "[solved]"?

Oops sorry. I mean there is nothing wrong about the output of the pid calculation. The system indeed still have a main problem, as stated before.

Do you understand what the problem is? I'm not sure if i already explained it clear enough.

9

The stock PID code assumes position is a monotonic function, and it does NOT understand the wrap-around that occurs in rotary encoders. It calculates error as simply setpoint minus current position, with NO understanding that the encoder value wraps around at the zero-crossing. To do what you want, you'll have to modify the PID code so the error calculations handle the encoder wrap-around (by using unsigned or modulo arithmetic), and calculates the shortest path from the current position to the setpoint. This means that if the setpoint is zero, and it over-shoots to 359, it will actually reverse the motor to move directly from 359 BACK to 0, rather than continuing in the same direction, which means going all the way around again.

Regards,
Ray L.

10

UKHeliBob:
Does the antenna need to track through the whole 360 degrees ?

Yes, it have to.

RayLivingston:
The stock PID code assumes position is a monotonic function, and it does NOT understand the wrap-around that occurs in rotary encoders. It calculates error as simply setpoint minus current position, with NO understanding that the encoder value wraps around at the zero-crossing. To do what you want, you'll have to modify the PID code so the error calculations handle the encoder wrap-around (by using unsigned or modulo arithmetic), and calculates the shortest path from the current position to the setpoint. This means that if the setpoint is zero, and it over-shoots to 359, it will actually reverse the motor to move directly from 359 BACK to 0, rather than continuing in the same direction, which means going all the way around again.

Regards,
Ray L.

Thank you for your helpful reply. I'll update the progress as soon as possible.

11

Another way to do is is to have a translation layer between the encoder output and the PID input. Set your PID to track to the 180 position, then "rotate" the encoder's output in software before you input it into the PID algorithm so that your desired position outputs 180 from the translation layer. Changing the setpoint is then a matter of changing the offset in the translation layer, rather than changing the setpoint in the PID library. This moves the wrap-around discontinuity so that it is always on the opposite side of the setpoint, where it is least likely to cause trouble.

Sample "rotation" code (untested):

int rotate_sensor_reading( int position, int offset )
{
  position += offset;
  if( position >= 360 ) position -= 360;
  if( position < 0 ) position += 360;

  return position.
}
12

Jiggy-Ninja:
Another way to do is is to have a translation layer between the encoder output and the PID input. Set your PID to track to the 180 position, then "rotate" the encoder's output in software before you input it into the PID algorithm so that your desired position outputs 180 from the translation layer. Changing the setpoint is then a matter of changing the offset in the translation layer, rather than changing the setpoint in the PID library. This moves the wrap-around discontinuity so that it is always on the opposite side of the setpoint, where it is least likely to cause trouble.

Sample "rotation" code (untested):

int rotate_sensor_reading( int position, int offset )

{
  position += offset;
  if( position >= 360 ) position -= 360;
  if( position < 0 ) position += 360;

return position.
}

Try testing that, to see what a sudden huge step change in position feedback does to the PID calculations, then report back here....

Regards,
Ray L.

13

RayLivingston:
Try testing that, to see what a sudden huge step change in position feedback does to the PID calculations, then report back here....

Regards,
Ray L.

Mostly the same as a step change in setpoint, I would think. At worst, the D term will provide an extra, very brief pulse of output with my solution, unless there's something extra in the standard PID library that handles setpoint changes that I'm not aware of.

14

Jiggy-Ninja:
Mostly the same as a step change in setpoint, I would think. At worst, the D term will provide an extra, very brief pulse of output with my solution, unless there's something extra in the standard PID library that handles setpoint changes that I'm not aware of.

And what happens when you give a PID a large step change in setpoint? It reacts STRONGLY, commanding a maximum-effort move. If you really WANT it to move quickly to a new position far away, that is the correct thing to do. If you are already close to the setpoint, you want it to move smoothly to the setpoint, and not suddenly go foot-to-the-floor, which will cause it to over-shoot, and most likely oscillate wildly, back and forth over the point where you inject that step change. It is perhaps the worst possible solution. Fixing the PID Compute function to correctly handle the encoder wrap-around is not terribly difficult, and is the only correct solution, and it will also allow the PID to be smart enough to take the shortest path, knowing that sometimes going CCW will be shorter than CW, and vice-versa.

Regards,
Ray L.

15

And what happens when you give a PID a large step change in setpoint? It reacts STRONGLY, commanding a maximum-effort move. If you really WANT it to move quickly to a new position far away, that is the correct thing to do. If you are already close to the setpoint, you want it to move smoothly to the setpoint, and not suddenly go foot-to-the-floor, which will cause it to over-shoot, and most likely oscillate wildly, back and forth over the point where you inject that step change. It is perhaps the worst possible solution.

I agree with all of this.

Fixing the PID Compute function to correctly handle the encoder wrap-around is not terribly difficult, and is the only correct solution,

Non sequitur. Nothing you wrote in the previous section supports this assertion, since the problem of step change response is completely different than the problem of wrap-around discontinuity, and will have a completely different solution.

I'm not saying your solution is wrong, because it's not. It is a correct solution. It's just not the correct solution.

and it will also allow the PID to be smart enough to take the shortest path, knowing that sometimes going CCW will be shorter than CW, and vice-versa.

So will my solution, and it won't require modifying the PID library. Making the PID algorithm track a constant 180 and virtually rotating the encoder "underneath" it won't remove the discontinuity, but it will keep it always at the farthest position away from the setpoint, where the motor should never cross.

16

I KNOW my solution works, because that's how I've done it in all the many PIDs I've personally written, and it's also how it's done in every other PID I've ever seen that uses rotary encoders. In the meantime, I'll wait for you to actually implement your solution, and prove it works as you claim....

Regards,
Ray L.

17

RayLivingston:
I KNOW my solution works,

I have never once said it wouldn't. While I haven't done a rigorous proof, I'm quite certain that our two solutions are mathematically identical, just rephrased in different terms.

In the meantime, I'll wait for you to actually implement your solution, and prove it works as you claim....

Math proves it. The PID algorithm is calculated on the error of the process variable. Since the error is a simple subtraction of the process variable and setpoint, a constant additive offset applied to both will change literally nothing in any of the downstream calculations.

Error == Setpoint - Process == (Setpoint+Offset) - (Process+Offset)

At least in pure theory. I know of two caveats to this approach that I have already mentioned.

Step change in setpoint causes large spike in derivative term: Many implementations deal with this by taking the derivative of the process variable since it is continuous. My translation layer would break that, since the setpoint of the PID algorithm is never changed. A change in setpoint is seen by the PID algorithm as a change in position, so it will return to the pure theoretical behavior.

This can otherwise be dealt with by either ramping the setpoint change or limiting how fast the process variable can change.

The discontinuity still exists 180 degrees from the setpoint: Since this solution just remaps the values of the points on the circle, the 0-360 discontinuity still exists. However, I think it is highly unlikely that the motor would ever cross over that position during normal operation, since it would require it to take the "long way around" when trying to reach the setpoint.

This scenario would also be a problem for your "alternate error calculation" method, since if by some fluke the motor crossed over the position opposite of the setpoint the error in your calculation would abruptly change sign in the same way as it would in mine. Unless you make it really smart to detect that sort of thing.

That's all I can think of. Is there something I missed?

18

Yes. Rotary encoders are very often used in systems where the motor turns continuously for many turns. One example is CNC machines, with the encoder mounted on the motor, which drives a ballscrew, which may turn 100 turns to drive an axis from one end stop to the other. You are clearly NOT understanding my point at all. Simply blindly subtracting the current position from the setpoint (or vice-versa) DOES NOT WORK if the encoder will ever turn 360 degrees or more. As stated in my original post, you MUST do, in effect, modulo arithmetic to calculate the error, AND take into account that there are ALWAYS two possible paths from the current position to the setpoint, one of which will be shorter than the other (except for the one case where the two positions are separated by precisely PPR/2). When you do this, the PID will correctly calculate for ANY current position and setpoint, even when the encoder crosses from a count of PPR-1 to 0 )or vice-versa), it will correctly calculate an error of 1 count, without having to do any fudging of the encoder count. The modulo math alone deals with that automatically. Just look at how everyone here uses millis() to calculate delays, without getting tied in knots when the millis() counter wraps around to zero every 40-some days. It is precisely the same logic being applied there - do the subtraction, and get the right result, always.

Regards,
Ray L.

19

Problem solved. Just like what RayLivingston said i change error definition in arduino PID_v1.h library. Here's the code, hope this will explain what i mean,

//Input is the process variable     
double error; 
if (*mySetpoint>input) {
     if (abs(*mySetpoint-input) < abs(-360 + *mySetpoint - input)) error = *mySetpoint - input;
     else  error = -360 + *mySetpoint - input;
}
else{
     if(abs(*mySetpoint-input)< abs(360 - input + *mySetpoint)) error = *mySetpoint - input;
     else  error = 360 - *mySetpoint + input;
}
20

dear rafisidqi@gmail.com

i didn't get how you solved the problem. did you change the library and save it like this?

bool PID::Compute()
{
   if(!inAuto) return false;
   unsigned long now = millis();
   unsigned long timeChange = (now - lastTime);
   if(timeChange>=SampleTime)
   {
      /*Compute all the working error variables*/
      double input = *myInput;
	  double error; 
	  if (*mySetpoint>input) {
      if (abs(*mySetpoint-input) < abs(-360 + *mySetpoint - input)) error = *mySetpoint - input;
      else  error = -360 + *mySetpoint - input;
	  }
	  else{
      if(abs(*mySetpoint-input)< abs(360 - input + *mySetpoint)) error = *mySetpoint - input;
      else  error = 360 - *mySetpoint + input;
	  }
            
      double dInput = (input - lastInput);
      outputSum+= (ki * error);

      /*Add Proportional on Measurement, if P_ON_M is specified*/
      if(!pOnE) outputSum-= kp * dInput;

      if(outputSum > outMax) outputSum= outMax;
      else if(outputSum < outMin) outputSum= outMin;

      /*Add Proportional on Error, if P_ON_E is specified*/
	   double output;
      if(pOnE) output = kp * error;
      else output = 0;

      /*Compute Rest of PID Output*/
      output += outputSum - kd * dInput;

	    if(output > outMax) output = outMax;
      else if(output < outMin) output = outMin;
	    *myOutput = output;

      /*Remember some variables for next time*/
      lastInput = input;
      lastTime = now;
	    return true;
   }
   else return false;
}