PID Tuning problem or wrong PID Implementation?

[Benn]

Hi all. I'm facing another problem. I made a robot that travel from one point to another, avoiding obstacles, using Wavefront Algorithm. The algorithm is working very good, but the robot didn't use to go straight, so i tried to implement a PID controller. Now the robot is going more good than without PID, but still bad:( Sometimes he oscillates, overshoot the setpoint,, or moving brutally...
The gears  have encoders with 57pulses/rotation, and the PID rate is 10 times/sec.
I tried different values for Kp, Kd, Ki, but still bad.
Does anyone have an idea of the problem?
Here is the PID function:

void PID(void)
{
   int errorl = 0;       //error from left encoder
   int errorr = 0;       //error from right encoder
   int derrorl = 0;       //derivative error left
   int derrorr = 0;       //derivative error right
   
   int KP = 2;       //PID proportional gain constant
   float KD = 1;       //PID derivative gain constant
   float KI = 0.5;    //PID intergral gain constant

   

   errorl = setpoint - a; //calculate error values
   errorr = setpoint - b; // a is the encoder counter for left motor and b for right motor

   a = 0; //reset encoder counts ready for next sample
   b = 0;

   derrorl = (errorl - preverrorl);
   derrorr = (errorr - preverrorr);

   cl = ((KP*errorl) + (KD*derrorl) + (KI*Ierrorl)); //PID equations
   cr = ((KP*errorr) + (KD*derrorr) + (KI*Ierrorr));


   if (cl > cr)
   {
      OC2R = dtcMtrMedium + cl; //use output from PID equations to alter motor speeds
      OC2RS = dtcMtrMedium + cl;

      OC3R = dtcMtrMedium - cr;
      OC3RS = dtcMtrMedium - cr;
   }

   else
   if (cl < cr)
   {   
      OC2R = dtcMtrMedium - cl;  
      OC2RS = dtcMtrMedium - cl;
   
      OC3R = dtcMtrMedium + cr;
      OC3RS = dtcMtrMedium + cr;
   }   
   else
   {
      OC2R = dtcMtrMedium + cl;  
      OC2RS = dtcMtrMedium + cl;
   
      OC3R = dtcMtrMedium + cr;
      OC3RS = dtcMtrMedium + cr;   
   }

   preverrorl = errorl; //set previous error to current error
   preverrorr = errorr;

   Ierrorl = Ierrorl + errorl; //add current error to integral error
   Ierrorr = Ierrorr + errorr;
}

[madsci1016]

You have no code to limit I wind-up, that could be your problem.

How are you tuning the system? First set I and D to 0.

Find a good P term that gives a quick response, but will have oscillations. Then find a D to reduce oscillations.

I would stay away from the I term for your system. Keep I = 0, for now, and tell me how your systems looks.

[Benn]

I did as you say...left I = 0, and tuned the others...and it looks good for a short movement, but in time, it deviates from the path very much:((
I also increased the PID rate at 100 times/second, and the robot reaches it's setpoint very quick and very good, but, as i said, in time, it goes very bad:(
WHYY??

[madsci1016]

When you say it deviates after a while, do you mean that if you are telling it to run a straight line, after a while it starts to curve one way or the other? Or that it starts oscillating?

If you are just saying it curves one way or the other, the answer is, ... tough. Adding a small amount of an I term (starting with 0.1 and up) may help some, but you also need to limit how large Ierror can get, or the system will oscillate from I term wind-up. 

But really, you have encoders with low resolution per turn, 57 pulses per rotation. To compare, on my SAGAR robot the wheels have 3000 pulses per rotation. The lower resolution means less ability to precisely keep the wheel spinning a certain speed. A faster control rate will just make it worse, as it will be even less precise. i wouldn't have a rate faster then 5Hz.

Also, you could be suffering from slip. wheels with encoders are never perfect, they always have some give that throw off encoder calculations, that can't be fixed.

Your best bet for you project, since you are using the encoder to localize your position, would be to track the number of pulses, and compute your location, rather then trying to force your robot to go straight, as it will never go perfectly straight.

[waltr]

How are you using the wheel encoders?
Counting how many encode ticks happen in a fixed period of time? Or how many hardware timer counts happen for one encoder tick?

I have used the second method on a couple of bots with 8 and 12 encoder ticks per wheel revolution and the bot would track straight for a long distance. This only uses Proportional feedback, there is some over-shoot and oscillation in the speed but barely noticeable by watching the bot move.
The PWM duty is up-dated at every encoder tick, new count available, and not on a fixed time period.

I also have a diagnostic output to the serial port (cabled with MAX232 to comm port or XBees to com port). When working on the wheel speed feed-back code I output the encoder count, PWM duty count, encoder target count (this sets the desired wheel speed). I capture the data to a file using either Hyperterm or Teraterm then import the data file into Excel for plotting and analysis. It really helps to get real data to evaluate the feed-back control loop.

[Benn]

So, waltr, i count up to 5 encoder pulses per one period of PID.
Do you think i have a chance to make it going straight with only 57 pulses/revolution?
Could you explain your metod please much more?
The Saturday, 15th of May, is the deadline for the project. Everything is perfect, but i'm so disapointed with those error, because the bot it's deviating from his path, and not going straight:(
I tried hundreds of times to get the error as small as posible, but in vain...

[waltr]

Ahh...you count the number of encoder pulse in a fixed period and 5 pulses is at the maximum speed. So at half that speed there is 2 encoder pulses. This is not much resolution of the bots real speed.

My method is to run a hardware timer/counter in the processor. On each encoder pulse I save the counter value and reset the counter to zero. I get about 50 counts at nominal speed. So if the bots wheels at 10% slow I get a count of 55. If your bot's wheels are 10% low you get a count of 5 or maybe 4 depending on where the encoder position is when the PID cycle starts.

So basically you can not resolve a small error and thus can not make a correction until the error is large enough. Another example: if the error is about 8% then no correctable error is detected and the bot merrily turns. Once the error is over ~15-20% then it is detectable and can be corrected to less than about 20%.

Since my encoders have a low number of counts per wheel revolution I went the other way and measured the time from one encoder pulse to the next with a fairly fast hardware counter. That allowed me to get finer resolution of the wheel speed. I also correct the PWM duty on every encoder pulse so that any little speed error actually averages out.

Does that help?

[Benn]

Man, that's genial...it did not cross my mind such an idea...hmmm...that's very good for a bot with so few pulses/revolution.
Thank you very much. I'll try to implement this and see the result. It sounds like the solution for my problem...i'll tell you if i'll get it work:)
Thanks again;)
One more question: did you use PID or not? from your explanation i understand that you didn't...?

[waltr]

Correct, I only used 'P'. I have the code in for 'D' but found that I didn't need it.

Good luck and keep us updated.

[waltr]

Benn,

 I'll post the answer to your PM here so that anyone interested can follow.


I would never expect the encoder pulses from each wheel to happen at the same time. Each wheel is handled separately. I don't have the code here (I'm at work and the code is at home) and I am using a PIC so the code isn't 100% comparable.  However, since the latest code is written in C (the original code was in assembler) I'm sure you could benefit from looking it over. All the wheel speed measurements, corrections and PWM duty cycle setting in done in an ISR (Interrupt Service Routine). Here is a short overview:

ISR:
if timer interrupt // about every 200us
  increment left_wheel_encoder_count
  increment right_wheel_encoder_count
  clear timer interrupt flag

// wheel encoder pulse input is on a pin that causes an interrupt
if right wheel encode pulse is a rising edge
  new_right_wheel_speed = right_wheel_encoder_count
  clear right_wheel_encoder_count   // start a new count
  set new_right_wheel_speed_flag

if left wheel encode pulse is a rising edge
  new_left_wheel_encoder_count = left_wheel_encoder_count
  clear left_wheel_encoder_count
  set new_left_wheel_speed_flag

// PWM interrupt every fourth PWM period, about 1.2msec
if new_right_wheel_speed_flag is set
  right_wheel_error = new_right_wheel_encoder_count - right_wheel_target_speed // be sure to obtain the correct sign
  right_PWM_count = (Pconstant * right_wheel_error) + PWM_count // here the correction is applied
  clear new_right_wheel_speed_flag
  PWM_DC_REG = right_PWM_count  // will be loaded on the next PWM cycle

if new_left_wheel_speed_flag is set
  left_wheel_error = new_left_wheel_encoder_count - left_wheel_target_speed
  left_PWM_count = (Pconstant * left_wheel_error) + PWM_count
  PWM_DC_REG = right_PWM_count
  clear new_left_wheel_speed_flag
exit from ISR

So there are three interrupt routines (6 if you count each wheel separately).
The Timer ISR only counts the encode pulse time.
The wheel encoder pulse (rising only) saves the count from the Timer and sets a flag (bit) to let the PWM ISR know there is a new speed measurement.
The PWM ISR calculates to wheel speed error and applies the correction to the PWM duty cycle register (hardware module). It then clears the new speed count flag since it is finished with this new value.

You will need to use different values for the Timer interrupts period and the PWM update period.

When the MAIN code wishes to change the wheel speed it changes the value of right_wheel_target_speed & left_wheel_target_speed. This allows the feedback control loop to do the actual speed change.

Does this help? If you want to see may PIC C code let me know and I'll post it tonight.

[Benn]

Thank you very much. It's very helpfull. Also, if you have time, please post the C code, because i work also with PIC32MX, and MPLab.
Thanks again. I'll let you know the result.

[waltr]

Ok Benn, here is my code as it currently stands.

[Benn]

ok...i promised to come back with some informations...
The Contest is over, i won a prize for the best presentation.
Regarding PID, it worked ok...also not perfect. Special thanks to waltr for his help, and also others!

[waltr]

Congrats on the prize.

[eschulma]

Just wanted to thank waltr for his suggestion about using timing between pulses for low value encoders (mine have 128 per revolution). In my case I simply wanted to drive in a straight line. So I just looked to see if pulse timing for the left encoder matched the right encoder, to the extent it didn't, that error was fed into my PID controller. (I found I did have to add an integral term to get it running in a stable way.)

You can do this without putting the code directly in the interrupt routine, if you are already running a very fast loop (as I was) at the expense of a small amount of accuracy.