[ODE] Servo to reach a target position

[Royce Mitchell III]

Hello David,

Thursday, April 10, 2003, 4:47:59 PM, you wrote:

> Noticed a bug.... see below (the fix:'s).  Unless I'm really confused, you
> need to divide the Error by the timestep instead of a constant 10 in the
> CalcI function.

This was merely a cheap way to choose a cap for max integration.
There's nothing fundamentally wrong with doing it that way.

> Conceptually, it seems to me that the I calculation should only take into
> account the integral of the error over the past (N) seconds.  As opposed
> to the total error since the dawn of time...  But I could be wrong.

The problem with integrating only the past N seconds is that as you
start approach your target, your I would start to work against you.
Look up I's purpose. The last thing you want is when you're finally
homing in to your target, and all of a sudden your I term starts
dropping because the accumulated I over the past few seconds is no
longer maxed. It will cause you to probably shoot past your target.

Theoretically, we let I accumulate from the dawn of time. In practice,
we set a max value that the accumulation can reach. My method for
achieving this was to divide error by 10 for my accumulation, and to
set the max sum to the max E seen. It would have been more "correct"
to increment the sum by E and calculate max sum as E*10.

Perhaps some example uses of I would help clarify...

1) Let's say you have a machine with a limited range of movement, and
in order to get Kp high enough for it to be able to respond, it always
gets unstable, rocking back and forth. This means that, assuming
you're not using Ki or Kd, yet, your Kp is too high. However, as
already stated, it doesn't respond with a lower Kp. This is a case
where you need to use Ki. If the machine doesn't respond, then as
error accumulates, I gets bigger and eventually gives a signal needed
to break the machine away from where it's stuck.

2) Let's say you have a machine where you have very indirect control
over the process. For example, a situation I was in where my control
over the process was to apply more or less "brake". ( This was a roll
stand in a printing press ). The problem I was running into was that
because of external factors, no matter what target "brake" I chose,
there were situations where the swing-arm wouldn't center. This was a
bad thing, because the paper would brake on line-speed changes if the
arm didn't stay centered, especially when performing a splice. The
solution to the problem was again, Ki. By applying a small Ki, the
system would slowly adjust my output to get the arm centered. It had
to be very low to not affect reacting to more dynamic situations where
Kp and Kd were the highlight of the show. In this case, very little Ki
would cause the arm to center when it otherwise might sit 25% to the
right, or whatever.

Anyways, my inclusion of dT was a mistake, as it is nowhere used, and
should nowhere be used. This is not a physics calculation, it is a
control algorithm.

A position control example: If I am 10 feet away, I always want to be
producing a 10 Volt signal to be at the proper "approach speed". At 9
feet away, I want to have a 9 volt signal. 8 feet = 8 volts, etc. It
doesn't matter how often I update my control loop. It doesn't matter
how long it's been since I last updated. I still want 8 volts if I'm 8
feet away. So, dT doesn't matter. Obviously, the more often I update
my control loop, the smoother my approach is going to be, but it
doesn't change the calculation in the least bit.

Hope this all helps. I have pasted the corrected code below, and
reverted the changes you made.

// pid.h

#ifndef PID_H
#define PID_H

#include <math.h>

class PID
{
      // Kp = Proportional gain
      // Ki = Integral gain
      // Kd = Derivative gain
      // Isum = accumulated error for I-term
      // E = saved Error from previous calculation
      // MaxE = maximum error signal seen
      // OV = Output Variable
      float Kp, Ki, Kd, Isum, E, MaxE, OV;
public:
      PID ( float Kp_=0, float Ki_=0, float Kd_=0 )
      {
              Reset();
              SetGain ( Kp_, Ki_, Kd_ );
      }
      void Reset()
      {
              Isum = 0;
              E = 0;
              MaxE = 0;
              OV = 0;
      }

      void SetGain ( float Kp_, float Ki_=0, float Kd_=0 )
      {
              Kp = Kp_;
              Ki = Ki_;
              Kd = Kd_;
      }

      // SP = destination/target
      // PV = feedback
      // return value = command output
      float StepPosition ( float SP, float PV )
      {
              float E1 = E; // previous error
              E = SP - PV; // new error

              float Op=0, Oi=0, Od=0; // P, I, and D terms of calculation

              // P-Loop
              Op = CalcP ( E );

              // I-Loop
              Oi = CalcI ( E );

              // D-Loop
              Od = CalcD ( E, E1 );

              // Final Summation
              OV = Op + Oi + Od;

              return OV;
      }

      // SP = destination/target
      // PV = feedback
      // return value = command output
      float StepSpeed ( float SP, float PV )
      {
              float E1 = E; // previous error
              E = SP - PV; // new error

              float Op=0, Oi=0, Od=0; // P, I, and D terms of calculation

              // P-Loop
              Op = CalcP ( E );

              // I-Loop
              Oi = CalcI ( E );

              // D-Loop
              Od = CalcD ( E, E1 );

              // Final Summation
              OV += Op + Oi + Od;

              return OV;
       }

private:
      float CalcP ( float E )
      {
              return E * Kp;
      }

      float CalcI ( float E )
      {
              if ( Ki > 0 )
              {
                      Isum += E / 10;
                      if ( fabs(E) > MaxE )
                              MaxE = (float)fabs(E);
                      if ( Isum > MaxE )
                              Isum = MaxE;
                      else if ( Isum < -MaxE )
                              Isum = -MaxE;
                      return Ki * Isum;
              }
              else
                      return 0;
      }

      float CalcD ( float E, float E1 )
      {
              if ( Kd > 0 )
              {
                      float dE = E - E1; // change in error
                      return Kd * E;
              }
              else
                      return 0;
      }
};

#endif//PID_H

// pidtest.cpp

#include <stdio.h>
#include "../pid.h"

void main()
{
      PID pid ( 0.1f );
      float position = 0;
      for ( int i = 0; i < 50; i++ )
      {
              position += pid.StepPosition ( 10, position, 0.1f );
              printf ( "%.02f ", position );
      }
      printf ( "\n\nfinal position: %.02f\n", position );
}

// end of pidtest.cpp