imET
Joined: 05 Jul 2005
Posts: 4
Location: Canada
|
| Servo PID |
 Posted: Wed Jul 20, 2005 4:16 pm |
 |
|
I deleted the duplicate
post now everything is gone
so here it is again.
...found this tru google
| Code: |
/*
* * * * * * * * * * * * * * * * * * * * * * *
* * CSERVO.C *
* * Originally by L.I.Williams *
* * Cybernetic Software *
* * [email protected] *
* * Issue 1.0 Date: 28Sep98 *
* * * * * * * * * * * * * * * * * * * * * * *
*
* CSERVO.C
* 28Sep98 Iss 1.0 Created by LIW
* A freeware PID servo algorithm written in C with extensive comments
*/
/************************************************************************
* HISTORY:
* Date Iss Comment
* 28Sep98 1.0 Created
*************************************************************************/
/* This is C source code. It's named CSERVO.TXT so that it can be viewed
easily with browsers and Notepad etc. */
/* Features:
* 2's complement fixed point arithmetic, no use of floating point
* Uses binary shifts for gains, no use of multiply or divide
* No calls to library routines needed (except during testing)
* Clamped calculations to deal with arithmetic overflow
* Test harness to check routine
* Includes tuning instructions, loop diagram and extensive comments
* Consultancy, customisation and assembler language versions are available
for your application, e-mail [email protected] or call
+44 1202 623363 (see end)
* Suitable for torque amplifier, velocity amplifier, hydraulics
and phase locked loop applications using simple processors
Disclaimer etc
You're free to use this code provided you acknowledge the original
author in the source and quote my e-mail address. Note that this
particular code has not been used in the form you see here. It does
compile and the output looks right using the test harness. Let me know
if you find what you think are mistakes.
Description
This routine is designed to be called at fixed (or near fixed) time
intervals either under a timer interrupt or from a foreground polling
loop. It can also be called in response to a feedback event (with some
modifications.). 16 bit arithmetic has been matched to a 10 bit D/A or
PWM driven in offset binary. The PWM output should control the
*acceleration* of the position variable (a torque amplifier motor drive
for example). It will also work where the output controls velocity.
Servo Loop Block Diagram
------------------------
VelErr Vel VelAct
Velocity error -------------- Gain -------------|
|
|
PosErr Pos PosAct + PWMout
Position error -------------- Gain ----------- + -------- + --> PWM
| + +
| | |
| Integral IntegAct | |
|--- Sum --- Gain -------------| 0x8000
Offset
Binary
All the input signals are clamped into a useful range to prevent
arithmetic overflow, converted to PWM units using gains and then summed
together. All the gains are binary shifts (multiples of 2). This
allows settling time/stiffness/damping to be resolved within a factor of
2, which is sufficient. Position error is summed at each call to make
an integral action term added in as well. Velocity action is 'damping'.
Position action is 'stiffness'. Integral action is 'dynamic offset
compensation'. Use integral action for systems that must track changing
position with zero following error.
See end for further servo tuning and design rules and consultancy and
customisation information.
*/
/* Include files: */
#include <stdio.h> // Only used for test harness
/************************************************************************
Variables
***********************************************************************/
// These should be 16 bit quantities to match a 10 bit PWM
static int VelErr; // Velocity error
static int PrevPosErr; // Previous position error
static int PosErr; // Position error
static unsigned int IntegErr; // Integral position error
static int IntegOvf; // Most significant 16 bits, only a few used
static int VelAct; // Velocity error x gain, velocity action
static int PosAct; // Position error x gain, Position action
static int IntegAct; // Position integral x gain, integral action
static int PWMout; // Offset binary output to PWM or D/A
// static means these variables retain their values in global memory
// from one call to the next.
/************************************************************************
Start of code
***********************************************************************/
void CServo(void)
{
int temp; // Temporary working register
unsigned int uitemp; // Temporary working register
unsigned int uitemp1; // Temporary working register
/************************* Position Action ****************************/
/* Calculate x8 to give Position error action term, PosAct.
x8 is just a typical gain value */
// read_PosErr(); // get the new position error from hardware
/* *** SAFETY CRITICAL *** Now clamp so that shift does not overflow.
The clamps in this code can be CRITICAL FOR SAFETY because arithmetic
overflow reverses the sign of a variable and causes positive feedback.
Because this only occurs under extreme conditions corresponding to large
unusual signals, the loop can work perfectly well without the clamps
under normal operating conditions, and still be unsafe */
PosAct = PosErr;
/* PosErr is used again for the integral action in a moment so we clamp
it's value in PosAct */
if (PosAct >= 0xfff) // 4095, Clamps to 7ff8 after multiply
PosAct = 0xfff;
else if (PosAct < (signed)0xf001) // -4095
PosAct = 0xf001;
/* Change the clamp above if you change this shift. I've used explicit
hex constants here so you can see how it works. Obviously if you wish
to make the gain variable you will need to calculate the constants as
7FFF >> gain and -(7FFF >> gain) */
PosAct = PosAct << 3;
/********************** End of Position Action *************************/
/************************* Velocity Action *****************************
This routine assumes a velocity error derived from a separate
measurement system. If the velocity is calculated by differencing
position feedback it's important to realise that the position resolution
must be very high to resolve velocity to a high precision despite this
routine being called at a high frequency. The velocity action has the
highest gain and that amplifies any quantisation of the velocity error.
Various sorts of smoothing, averaging or a running average may be needed.
See end for consultancy information.
*/
/* read_velocity(); // Read velocity transducer */
VelErr = PosErr - PrevPosErr; // or calculate velocity
PrevPosErr = PosErr; // Previous position error saved
/* This calculation is unlikely to overflow but we should clamp it just
in case. If PosErr rotates continuously through +32767 to -32768
normally, then we should not clamp, but rely on wrap round to produce a
correct velocity value, provided we do not get position changing by more
than +/-32767 per call to CServo() */
/* Now clamp to positive maximum if
VelErr < 0 PosErr >= 0 PrevPosErr < 0, indicating positive overflow
In assembler we would test the 2's complement overflow flag. Instead
these tests have been arranged to use only the sign bit for economy */
if (VelErr < 0)
{
if (PosErr >= 0)
if (PrevPosErr < 0)
VelErr = 0x7fff; // 32767 Positive Overflow
}
else
/* Negative overflow is:
VelErr >= 0 PosErr < 0 PrevPosErr >= 0 Negative Overflow */
if (PosErr < 0)
if (PrevPosErr >= 0)
VelErr = 0x8001;
/* It's a good idea to exclude 0x8000 (-32768) because -0x8000 is 0x8000
and still negative, which can be troublesome. */
/* We further clamp VelErr so that the gain of 128 that follows to make
VelAct, cannot cause overflow. This clamp is only invoked for the
largest velocity errors, which corresponds to extreme conditions such as
an instant large velocity demand starting from rest. You could combine
this clamp with the one above, but it makes the code less modular and
gives me a headache. */
if (VelErr >= 0xff)
VelErr = 0xff;
else
if (VelErr < (signed)0xff01)
VelErr = 0xff01;
/************************* Velocity Gain *****************************/
VelAct = VelErr << 7;
/* The maximum of this is 7f00, and not 7ffff as we might prefer, but
the difference is not important */
/********************* End of Velocity Action ********************/
/***************** Integral Action **********************************
This is integral of Position error. Integral action is slow and needs
only a limited output range, but must be extended precision 32 bit so
that a position error of 1 bit will *eventually* accumulate and change
the PWM output. */
uitemp = IntegErr; // Save for carry testing
/* We are adding a signed 16 bit error to an unsigned 16 bit least
value so we have to take care, but in 2's complement the compiler
plants the same code for a simple addition either way */
/* Extend to 32 bits using ucInterOvf and clamp to 0x00050000. 24 bits
would do using a char 8 bit variable. I've stuck to 16 bit integers for
consistency. The clamp prevents a build up of excessive integral action
(only a little is needed) which is called 'integral windup'. In
assembler we would use the carry from the above but in C ... */
if (PosErr >= 0) // IntegErr should be increased
{
IntegErr = IntegErr + PosErr; // Sum (integrate) error
if (IntegErr < uitemp) // It's gone down, there's a carry
{
if (IntegOvf != 0x5)
IntegOvf++; // so increment the MS byte
else
IntegErr = 0; // Clamp to 00050000 on limit
}
}
else
{
// PositionError -ve. IntegErr should decrease
uitemp1 = (-PosErr);
// uitemp1 is an unsigned integer like IntegErr and we now subtract that.
IntegErr = IntegErr - uitemp1;// Sum (integrate) error
if (IntegErr > uitemp) // It's gone up
{
if (IntegOvf != 0xFFFB)
IntegOvf--;
else
IntegErr = 0; // Clamp to FFFB0000
}
}
/* Now divide by 256 by picking the bytes. Note that IntegErr must
be unsigned or else the shift right will set the MS byte when
IntegErr is negative */
IntegAct = (IntegOvf << 8) | (IntegErr >> 8) ;
/* Now multiply by 2 to give an overall gain of /128. Change the
clamp above if you change this shift. */
IntegAct = IntegAct << 1;
/************* End of Integral Action ***************************************/
/***************** Output Summation ***************************************/
/* Offset binary + velocity term + Position term + Integral term
PWMout = 0x8000 + VelAct + PosAct + IntegAct. We have to clamp
the individual addition operations */
PWMout = VelAct + PosAct;
/* Positive overflow is PWMout < 0 VelAct >= 0 PosAct >= 0 */
if (PWMout < 0)
{
if (VelAct >= 0) // >= gives simple sign bit test
if (PosAct >= 0)
PWMout = 0x7fff; // 32767 Positive Overflow
}
else
/* Negative overflow is PWMout >= 0 VelAct < 0 PosAct < 0 */
if (VelAct < 0)
if (PosAct < 0)
PWMout = 0x8001;
/* Now add in the integral action and clamp again */
temp = PWMout;
PWMout = temp + IntegAct;
/* Positive overflow is PWMout < 0 temp >= 0 IntegAct >= 0 */
if (PWMout < 0)
{
if (temp >= 0) // >= gives simple sign bit test
if (IntegAct >= 0)
PWMout = 0x7fff; // 32767 Positive Overflow
}
else
/* Negative overflow is PWMout >= 0 temp < 0 IntegAct < 0 */
if (temp < 0)
if (IntegAct < 0)
PWMout = 0x8001;
/* Now convert to offset binary. We don't need to check for overflow */
PWMout = PWMout + 0x8000;
/* write_PWM(); // Now output the value to the PWM */
/* The signs in this code assume that a PWM value above 0x8000
causes a positive torque demand and increasing velocity and
position, 0x8000 is zero acceleration, and below 0x8000 is
negative acceleration leading to decreasing velocity and
decreasing position.
We`ve implemented VelErr * 128 + PosErr * 8 + IntegAct * 2 + 0x8000
We could organise it a little more efficiently as say:
((VelErr * 32 + PosErr) * 4 + IntegAct) * 2 + 0x8000
but that would make tuning changes difficult, and it would be almost
impossible to make the gains variables rather than constants. Depending
on your application you could remove the integral term, and even
conceivably the position term. You could also modify the 0x8000 term to
include any known fixed offset effects (although the integral action
will deal with them)
*/
/*********************** Signal Monitor *****************************/
/* As with electrical circuits you will need to look at the signals
dynamically. For example: */
printf(
"%04X %04X %04X %04X %04X %04X %04X %04X\n",
PosErr,VelErr,IntegOvf,IntegErr,VelAct,PosAct,IntegAct,PWMout);
/* In real time you'll probably have to avoid library routines
and produce your own routine to convert half a dozen values
to ASCII hex bytes and write them one by one to a UART */
}
/*********************** Test Harness ******************************/
void main()
{
/* This is just various values to check out the arithmetic. Note that
the integral action has memory and will need resetting if you wish to
stop one call interacting with the next */
printf("\nPosErr VelErr IntegOvf IntegErr VelAct PosAct IntegAct PWMout\n\n");
PosErr=0; PrevPosErr=0; CServo();
PosErr=0x7fff; PrevPosErr=0; CServo();
PosErr=0x8000; PrevPosErr=0; IntegErr=IntegOvf=0; CServo();
PosErr=0x0020; PrevPosErr=0; IntegErr=IntegOvf=0; CServo();
PosErr=0x0040; CServo();
PosErr=0x0060; CServo();
PosErr=0x0080; CServo();
PosErr=0xFF00; PrevPosErr=0; IntegErr=IntegOvf=0; CServo();
PosErr=0x0100; PrevPosErr=0; IntegErr=IntegOvf=0; CServo();
}
/***********************************************************************
* End of code
***********************************************************************/
/*********************** Tuning Rules *********************************
Increase the velocity gain to increase damping and suppress
oscillations - it adds 'treacle' to the motion!
Increase position gain to make the loop stiff and hold a position.
Increase the integral gain, if the loop seems to settle to the wrong
position and then very slowly homes in to the right position.
If the system misbehaves close only the velocity loop, then try adding
position action and finally integral action.
To tune up from one good setup towards the optimum:
*Simultaneously* - double the velocity gain, quadruple the position gain
and multiply the integral gain by 8. This gives the same dynamic
response occurring in half the time. However it increases the actuator
power requirement by 8 times and may cause amplifier saturation.
Shorter settling times will increase jitter holding position.
Most jitter comes from the velocity action, which will have the highest
gain.
Some jitter is needed - the loops are open if nothing is happening!
********************* System Design Rules **************************
Since some jitter must occur, allow for it by making your position, and
especially velocity resolution, better than your accuracy requirement.
The PWM or D/A resolution is well matched when one bit changes of
velocity/position/ position integral pass through unity arithmetic gains
to produce 1 bit changes at the output. It's difficult to design for
this in advance, but....
Estimate a loop circulation delay by adding up the delays for filters,
amplifiers, sample and hold, calculation times etc. Use propagation
delay = 1/(radian bandwidth) where necessary. Loop circulation delay is
a figure of merit and should be kept as low as possible. It's
reciprocal is the open loop bandwidth. The closed loop settling time
will not be shorter than 10 times this value. With enough information
(and a lot of luck) you can use this to calculate gains in advance of
building a machine. Of course a simulation package could be used as
well.
***************** Consultancy and Customisation ************************
The author, Lewis Williams, is a freelance software engineer trading as
'Cybernetic Software'. With 20 years experience in motion control I'm
pleased to meet your special requirements for modest remuneration working
on or off your site. For example:
* Assembler language versions for a particular DSP or microcontroller
* Versions including digital filters to cope with noise problems
* Calculation of optimal gains for particular load and actuator
combinations
* Consultancy on sampling rates, quantisation effects and feedback
and actuator technologies at the design stage
* Help with load problems such as available power or flexible couplings
such as belts or shafts
* Customisation for unusual technologies such as pneumatics
* Self tuning adaptive or self optimising versions for use with
variable loads
* Indexing algorithms for generating motion demand profiles
* Motion specifying languages
and so on. Please contact [email protected] tel:+44 1202 623363
and ask for CV, business terms and a quotation
*/
/***********************************************************************
* End of CSERVO.C
***********************************************************************/
|
|
|
FAQ 
Search
Register
Profile 
Log in to check your private messages 
Log in
