pid.f

Embedded magazine source code in the year 1990

( PID.F ... Proportional, Integral, Derivative Control Loop Code)
( Steven E. Sarns and Jack Woehr)

( Variables and Initializations)

variable KP                      100   KP                !
( Kp is the proportional term coefficient)
variable KI                      200   KI                !
( Ki is the integral term coefficient)
variable KD                       20   KD                !
( Kd is the deriviative term coefficient)
variable COMMAND                  50   COMMAND           !
( COMMAND is the desired output result)
variable ACTUAL                    0   ACTUAL            !
( ACTUAL is the output as actually measured)
variable ERROR                     0   ERROR             !
( ERROR is [COMMAND - ACTUAL])
variable INTEGRAL                  0   INTEGRAL          !
( INTEGRAL is the summation of all error values [+ and -])
variable INTEGRAL_LIMIT          200   INTEGRAL_LIMIT    !
( INTEGRAL_LIMIT is the max/min value that INTEGRAL can sum to)
variable DELTA_ERROR               0   DELTA_ERROR       !
( DELTA_ERROR is change in error since last reading)
variable PROPORTIONAL_TERM         0   PROPORTIONAL_TERM !
( PROPORTIONAL_TERM is Kp * error)
variable INTEGRAL_TERM             0   INTEGRAL_TERM     !
( INTEGRAL_TERM is Ki * sum[error])
variable DERIVATIVE_TERM           0   DERIVATIVE_TERM   !
( DERIVATIVE_TERM is Kd* d[error]/d[time])
variable LAST_ERROR                0   LAST_ERROR        !
( LAST_ERROR is the error during the previous loop)
variable OUTPUT                    0   OUTPUT            !
( OUTPUT contains the value sent to the DAC)
variable LOOP_TIME                 4   LOOP_TIME         !
( LOOP_TIME is measured in 1/18.2 sec ticks)

( Rounded division, akin to /MOD */MOD, etc.)
: /ROUND ( n1 n2 --- n3) tuck /mod rot 2/ rot <= abs + ;
: */ROUND ( n1 n2 n3 --- n4) dup >r */mod r> 2/ rot <= abs + ;

( I/O, Hardware specific )

hex

( Port addresses)
E200 constant a2d-port
E210 constant d2a-port

( Analog-to-digital channel we will use)
7 constant a2d-channel

( Number of samples for averaging)
5 constant #samples

( Waste 100 uSecs waiting for analog-to-digital conversion to complete)
: A2D-WAIT ( ---) 8 0 do loop ;

( Normalize a2d reading by subtracting midrange value)
: A2D-NORM ( ---) 80 - ;

 ( Get a reading from analog-to-digital hardware and normalize)
: (ATOD) ( --- n1)
    a2d-channel a2d-port pc! ( start conversion on our chosen channel)
    a2d-wait   		( delay for conversion to complete)
    a2d-port pc@	( get result)
    a2d-norm ;		( calculate offset from nominal zero)

( Take five a2d readings, average and store to ACTUAL)
: ATOD ( --- )
   0
   #samples 0 do (atod) + loop
   #samples /round ACTUAL ! ;

( Send a value out the digital-to-analog converter)
: DTOA ( n --- ) d2a-port pc! ;

( CLOCK is where the program spends all of its time, waiting for)
( the clock tick over, looping for LOOP_TIME until timeout.)
( ATOD is executed for lack of anything better to do during)
( this time, pausing to allow multitasking.)
: CLOCK  ( --- )
   LOOP_TIME @ 0
    do
     ticks @
     begin
      ATOD
      pause
      dup ticks @ <>
     until drop
    loop ;

( Stores the desired value)
: CMD ( n --- )      COMMAND ! ;

( See how far off we are and store)
: CALC_ERROR ( --- )
    COMMAND @ ACTUAL @ - ERROR ! ;

( Proportional Term)

( Caculate error significance as proportional term and store)
: CALC_PROPORTIONAL_TERM ( --- )
     ERROR @ KP @ 100 */round PROPORTIONAL_TERM ! ;
( Proportional term is Kp * ERROR)
( Note that Kp is divided by 100 to allow "fractional" values)

( Integral Term)

( LIMITER prevents integrator "windup" during open loop periods)
: LIMITER ( --- )
    INTEGRAL_LIMIT @ 100 KI @ */round
( Multiply by KI, normalize)
    INTEGRAL @ min  INTEGRAL !
( Integrate error during this loop, check for out-of-bounds)
    INTEGRAL_LIMIT @ 100 KI @ */round  negate INTEGRAL @  max
( Compare the result of KI*INTEGRAL to the max that can be)
    INTEGRAL ! ;
( Store back to INTEGRAL the value within bounds.)

( INTEGRATE sums the errors)
: INTEGRATE ( --- )
    INTEGRAL @    ERROR @   +   INTEGRAL !  ;
( Get old sum, add error, store result back in INTEGRAL)

: CALC_INTEGRAL_TERM ( --- )
    INTEGRATE     LIMITER ( Check the case if INTEGRAL is negative)
    INTEGRAL @    KI @      100  */round   INTEGRAL_TERM ! ;
( INTEGRAL_TERM is Ki * sum(error) since time began)

( Derivative Term)

( DELTA_ERROR is the change in the error since the last reading)
: CALC_DELTA_ERROR ( --- )
    ERROR @ LAST_ERROR @ - DELTA_ERROR !
( DELTA_ERROR is this error - last error)
    ERROR @ LAST_ERROR ! ;
( Move current error to old error for next loop)

( DERIVATIVE_TERM is Kd * d[error]/d[time])
: CALC_DERIVATIVE_TERM ( --- )
    CALC_DELTA_ERROR DELTA_ERROR @ LOOP_TIME @ 18  */round
( Get d[error], get loop time, divide)
    KD @ 100 */round DERIVATIVE_TERM ! ;
( Multiply by Kd, normalize)
( Note that loop time is in 1/18th second ticks)

( TERMS prints out the 3 term coefficients - handy when)
( experimenting with loop stability)
: TERMS ( ---  )  cr  KP @ .  KI @ . KD @ . ;

( TERMS! stores all 3 terms from the stack)
: TERMS! ( n1 n2 n3 --- ) KD ! KI ! KP ! ;

( Summation and Report)

( SUMMATION calculates all of the terms, sends the result to the)
( DAC and leaves a copy in the variable OUTPUT)
: SUMMATION ( --- )
    CALC_ERROR           CALC_PROPORTIONAL_TERM
    CALC_INTEGRAL_TERM   CALC_DERIVATIVE_TERM
    PROPORTIONAL_TERM @ INTEGRAL_TERM @ DERIVATIVE_TERM @
    + +  128 +    255 min 0 max dup
    DTOA       OUTPUT !  ;
( 128 is the mid-range value of the output range of the DAC,)
( ie., the nominal zero value of the loop)


( REPORT prints values to screen)
: REPORT   ( --- ) cr
    COMMAND           @  6 .r
    ACTUAL            @  6 .r
    PROPORTIONAL_TERM @  6 .r
    INTEGRAL_TERM     @  6 .r
    DERIVATIVE_TERM   @  6 .r
    OUTPUT            @  128 -     6 .r ;
( The output printed is referenced to zero by subtracting 128,)
( that mid-range value of the DAC and A/D which denotes zero as)
( mentioned above)

( Now we wrap all up as a TASK so we can run it in the background)
( while experimentally stuffing its terms with new values from)
( the foreground task)
BACKGROUND: PID  ( --- )
    BEGIN
      CLOCK
( Wait for interval timer to release control to next step)
      SUMMATION
( Sum all of the terms of the PID equation)
      REPORT
( Print values to CRT)
    AGAIN ;

( Here is some illustrative diddling of the terms)
( Try these and watch result)
: UNDER_DAMP ( --- ) 20 80  0 TERMS! ;
: OVER_DAMP  ( --- ) 40  5  0 TERMS! ;
: ACCURATE   ( --- )  0 20  0 TERMS! ;
: CRIT_DAMP  ( --- ) 40 30 50 TERMS! ;

( Start up program with a command to go to zero)
( The desired value can be changed at any time)
( with the word CMD above)
: GO ( --- ) 0 CMD PID WAKE MULTI ;