DELTA TAU TURBO PMAC USER’S MANUAL
MAR-2003
Open Servo User’s Manual
Introduction
Turbo PMAC’s “Open Servo” feature permits users to write their own custom algorithm in a
high-level language that will execute on Turbo PMAC’s high-priority servo interrupt. This
algorithm can be used either for actual servo control functions, or for other tasks that must
execute at a very high priority, such as very high-frequency I/O, special pre-processing of
feedback data, or special post-processing of servo commands.
“Open Servo” algorithms are compiled into DSP machine code in the host computer before being
downloaded into Turbo PMAC’s active memory. They may be retained in Turbo PMAC’s nonvolatile flash memory using the SAVE command. When executed, they replace only the standard
servo-loop algorithm for the motor. All other tasks, including trajectory generation, motion and
PLC program execution, and safety checking, are still executed by the Turbo PMAC’s built-in
firmware.
The Open Servo feature is a second method for creating “user-written” servo algorithms in Turbo
PMAC. Previously, this could only be done by writing the algorithm in assembly language for
the DSP56300 family using Motorola’s cross-assembler, and downloading the assembled code to
the Turbo PMAC. The Open Servo feature permits these algorithms to be written without the
need to understand and use assembly language.
Turbo PMAC can only hold and execute a single Open Servo algorithm. This algorithm can be
run by multiple motors. The sections below explain tools for this single algorithm to access
motor-specific registers; if different procedures are desired for different motors, this must be
handled by explicit logic in the algorithm.
The compiled Open Servo program is similar to the compiled PLC programs, but there are two
key differences:


Open Servo algorithms run on the servo interrupt, with guaranteed execution every cycle
(or the Turbo PMAC will watchdog); compiled PLC programs either run on the real-time
interrupt (PLCC 0) with possible pre-emption by motion program calculations, or in
background (PLCC 1 – 31) with no deterministic execution rate.
Open Servo algorithms have specific access mechanisms to special registers used for
servo functions.
Requirements
The Open Servo requires a Turbo PMAC controller (Turbo PMAC(1), Turbo PMAC2, UMAC,
or QMAC) with version 1.938 or newer firmware to execute the algorithm. It requires a PC
running PEWIN32PRO version 3.2 or newer PMAC Executive program.
Open Servo Computational Features
The Open Servo provides powerful computational features to permit easy writing of sophisticated
and flexible algorithms.
OPEN SERVO ALGORITHMS
1
DELTA TAU TURBO PMAC USER’S MANUAL
MAR-2003
Access to Turbo PMAC Variables
Open Servo algorithms can utilize all of Turbo PMAC’s I, P, Q, and M-variables, reading and
writing to them as appropriate. As in other user programs, it uses floating-point arithmetic to
process these variable values, even those that are stored as fixed-point values (see Floating-Point
vs. Fixed-Point Mathematics, below). Q-variables are always accessed from Open Servo
algorithms according to the Coordinate System 1 addressing scheme, no matter which coordinate
system the motor executing the Open Servo algorithm is assigned to.
Compiler-Assigned Pointer Variables
For direct and efficient access to Turbo PMAC registers, Open Servo algorithms support two
types of pointer variables for which the register assignment is made at compilation time, not at
program execution time.
L-variables are pointers to short (24-bit) registers, treated as integer (fixed-point) values. These
work in the same way as L-variables do in compiled PLC programs. They can access either X or
Y short registers, either as entire 24-bit registers (treated as signed integers only), or as portions
of the registers 1, 4, 8, 12, 16, or 20 bits wide (treated as signed or unsigned integers, except for
1-bit variables, which are unsigned only).
F-variables are pointers to long (48-bit) registers. If the F-variable definition is an “L” format
(e.g. F1->L:$10F0), the register is accessed as a 48-bit floating-point register. If the F-variable
definition is a “D” format variable (e.g. F2->D:$88), the register is accessed as a 48-bit signed
integer, but conversion to or from Turbo PMAC’s 48-bit floating-point format is automatically
performed, so it can be used in floating-point mathematics.
(Do not confuse L-variables, which are short-word compiler pointers, with “L-format” Fvariables and M-variables, which are long-word variables.)
Note that Turbo PMAC itself cannot recognize L-variables or F-variables; these variables have
meaning only to the compiler on the host computer.
By contrast, when using Turbo PMAC’s M-variable pointers, the register assignment is made
when the line is executed, each time it is executed. This assignment requires about 600
nanoseconds additional computation time (on a 100 MHz CPU) each time the variable is
accessed. However, this does permit the M-variable definition to be changed during execution,
enabling techniques such as indirect addressing.
It is possible to use L-variables for fast integer arithmetic while retaining the run-time flexibility
of M-variable definitions. (This does add the run-time definition-access computational penalty
described above.) Instead of directly defining L-variables to registers for the compiler, you can
reference a range of L-variables to Turbo PMAC M-variable definitions with the LMOVERLAY
{start},{end} compiler directive. This directive must precede the actual Open Servo
program. For example, LMOVERLAY 10,20 instructs the compiler that the definitions of L10
through L20 are to be assigned at run time using the definitions of M10 through M20 respectively
at the time each statement is executed, not at compilation time.
Using the M-variable definition and accessing this definition at run time permits indirect
addressing techniques through real-time modification of this M-variable definition using another
pointer variable.
OPEN SERVO ALGORITHMS
2
DELTA TAU TURBO PMAC USER’S MANUAL
MAR-2003
Floating-Point vs. Fixed-Point Mathematics
Each statement in the Open Servo can be executed either floating-point or integer (fixed-point)
mathematics. In a floating-point statement, all variables used are processed through an
intermediate working format that is 48-bit floating-point, regardless of the storage format of the
variable. Floating-point statements can utilize any of Turbo PMAC’s I, P, Q, or M-variables, and
the compiler’s long F-variable pointers. They cannot use the compiler’s short fixed-point Lvariable pointers. All constants used in these statements are stored as 48-bit floating-point values.
In an integer statement, all variables used are processed through an intermediate working format
that is 24-bit signed integer, regardless of the storage format of the variable. Integer statements
can only utilize the compiler’s short fixed-point L-variable pointers. They cannot use the
compiler’s long F-variable pointers, or Turbo PMAC’s I, P, Q, or M-variables. All constants
used in these statements are stored as 24-bit signed integers. All constants used and intermediate
values computed must fit in the range of these integers: -8,388,608 to +8,388,607. No
mathematical functions (e.g. SIN, COS) may be used in an integer statement.
Note that if a constant appears on a line before any variable (e.g. IF (0=P1) …), the line is
assumed to be floating-point. This means that the statement IF (0=L1) is illegal because it
mixes floating-point and fixed-point data types.
Short-word integer operations will execute more than 10 times faster than the same operations
done as long floating-point operations, but usually will not have the dynamic range to handle the
actual control calculations. They can be useful for efficient calculation of associated logic,
however.
(Note that PMAC’s built-in servo algorithms – PID and Extended Servo Algorithm – use long
[48-bit/56-bit] fixed-point mathematics. This format is not supported in the Open Servo.)
If conversion between integer and floating-point data types is required, Open Servo provides the
ITOF (integer-to-float) and FTOI (float-to-integer) functions. The ITOF function (with an
integer expression as its argument) can be used in a floating-point statement, such as:
P20=ITOF(L10+L11)*3.14159/P628
The FTOI function (with a floating-point expression as its argument) can be used in a fixed-point
statement, such as:
L10=L9+FTOI(P5*100)-5
The FTOI function will round the value of the floating-point expression to the nearest integer.
It is not permissible to nest FTOI and ITOF functions within an expression.
Operators
As with any Turbo PMAC user program, Open Servo can utilize the following mathematical and
logical operators:


+
-
(addition)
(subtraction)
OPEN SERVO ALGORITHMS
3
DELTA TAU TURBO PMAC USER’S MANUAL






*
/
%
&
|
^
MAR-2003
(multiplication)
(division)
(modulo, remainder)
(bit-by-bit AND)
(bit-by-bit OR)
(bit-by-bit XOR)
All of these operators can be used in either floating-point or integer statements. Integer division
rounds the result to the nearest integer; in the case where the fraction is exactly 0.5, it will round
to the next more positive integer (e.g. -7.5 to -7, and 7.5 to 8).
Comparators
As with any Turbo PMAC user program, Open Servo can utilize the following comparators in
conditional statements:








=
>
<
~
!=
!>
!<
!~
(equal to)
(greater than)
(less than)
(approximately equal to [within 0.5])
(not equal to)
(not greater than, less than or equal to)
(not less than, greater than or equal to)
(not approximately equal to [not within 0.5])
The ~ and !~ comparators can only be used in floating-point statements. Note that the <>, >=,
and <= comparators, which can be used in some programming languages, cannot be used in the
Open Servo or other Turbo PMAC programs.
Functions
As with any Turbo PMAC user program, Open Servo can utilize the following mathematical
functions. Note that these functions can only be used in floating-point statements within the
Open Servo:












SIN
COS
TAN
ASIN
ACOS
ATAN
ATAN2
ABS
INT
EXP
LN
SQRT
OPEN SERVO ALGORITHMS
(trigonometric sine)
(trigonometric cosine)
(trigonometric tangent)
(trigonometric arc sine
(trigonometric arc cosine)
(trigonometric arc tangent)
(special 2-argument, 4-quadrant arc tangent)*
(absolute value)
(greatest integer within)
(exponentiation)
(natural logarithm)
(square root)
4
DELTA TAU TURBO PMAC USER’S MANUAL
MAR-2003
The trigonometric functions use degrees if Turbo PMAC variable I15 is set to the default value of
0; they use radians if I15 is set to 1. The present value of I15 is evaluated each time a
trigonometric function is executed.
*Note that the ATAN2 function uses Q0 as its second argument – the “cosine” argument. The
first argument – the “sine” argument – is inside the parentheses immediately following ATAN2.
The Q0 used is always Coordinate System 1’s Q0, no matter which coordinate system the
executing motor has been assigned to.
Special Saturation-Check Function
The Open Servo has a special function to check a signed quantity against an unsigned limit
magnitude and saturate the quantity at the magnitude of the limit. The function
FLIMIT({value},{limit}) compares the signed quantity {value} against the positive
value {limit} and the negative value –{limit}. If {value} is greater than {limit}, the
function returns the value of {limit}; if {value} is less than –{limit}, the function
returns the value of -{limit}. Otherwise, the function simply returns the value of {value}.
Both quantities – {value} and {limit} – must be floating-point expressions (the expressions
can be simply variables or constants).
For example, the statement:
P2=FLIMIT(P1,20000)
is equivalent to:
IF (P1>20000)
P2=20000
ELSE
IF (P1<-20000)
P2=-20000
ELSE
P2=P1
ENDIF
ENDIF
The FLIMIT function reduces the size of both the source code and the resulting compiled code.
Arrays
Open Servo algorithms support two types of arrays: variable arrays and register arrays. Both
provide useful capabilities.
Variable Arrays
Variable arrays work with the Turbo PMAC’s standard PMAC I, P, Q, and M-variables. The
number of the array index is placed inside parentheses, and specifies the variable number for the
specified type of variable. The expression that determines this number is a floating-point
expression, so it can use Turbo PMAC I, P, Q, or M-variables, constants (which will be treated as
floating-point values) and the compiler’s F-variables, but it cannot use the compiler’s L-variables
(unless the value has been converted to floating-point with the ITOF function). The resulting
OPEN SERVO ALGORITHMS
5
DELTA TAU TURBO PMAC USER’S MANUAL
MAR-2003
value of this floating-point expression is automatically rounded to the nearest integer to select the
variable number to be used. Some examples of statements using these variable arrays are:
P(P1)=P10*32
P30=I(ITOF(L10)*100+30)*P29
Register Arrays
Register arrays work with the compiler’s short L-variables and long F-variables. These arrays
must be declared to the compiler before the start of the actual Open Servo algorithm. In use, the
number of the array index is placed inside square brackets, and specifies the address offset from
the declared beginning of the array. The expression that determines this number is a fixed-point
expression, so it can only use the compiler’s L-variables and constants that fit within the range of
a 24-bit signed integer.
L and F-variable register arrays must be declared to the compiler before the start of the actual
Open Servo algorithm. Examples of these definitions are:
L100->X:$010000[32]
F200->D:$030040[64]
F300->L:$030080[128]
The declared array size must be a power of 2 in the range 2 to 8192. L-variable register arrays
always use full 24-bit X or Y registers, treating the values as signed integers.
Special Access to Motor Values and Registers
The Open Servo algorithms have several special statements to support useful features for servo
loop execution.
MTRNUM Function
The MTRNUM function returns the number of the motor (1 – 32) for which the algorithm is
presently executing. The number is returned as a fixed-point (integer) value. For example, the
statement L10=MTRNUM*100 would set fixed-point variable L10 to 200 when the Open Servo
algorithm is executing for Motor 2, or to 1800 when executing for Motor 18. Similarly, the
statement P10=ITOF(MTRNUM*100+30) would set floating-point variable P10 to 230 when
the Open Servo algorithm is executing for Motor 2, or to 1830 when executing for Motor 18.
COPYREG Command
The COPYREG command copies five key registers for the executing motor into five consecutive
P-variables, where they can easily be used for calculations. The user does not have to know the
addresses of these registers. In doing this copying, Turbo PMAC automatically converts the data
to 48-bit floating-point format.
The syntax of this command is COPYREG {P-variable name}, where {P-variable
name} specifies the number of the first variable into which data will be copied. The five
registers to be copied by this command are:




Actual Velocity (1/[Ixx09*32] counts / [Ixx60+1] servo cycles)
Desired Velocity (1/[Ixx08*32] counts / [Ixx60+1] servo cycles)
Following Error (1/[Ixx08*32] counts)
Actual Position (1/[Ixx08*32] counts)
OPEN SERVO ALGORITHMS
6
DELTA TAU TURBO PMAC USER’S MANUAL

MAR-2003
Desired Position (1/[Ixx08*32] counts)
The actual position value is derived from the register selected by Ixx03 for the motor (“PositionLoop Feedback Address”), with the source value multiplied by the Ixx08 scale factor and
extended into a 48-bit long word. The actual velocity value is derived from the position value
selected by Ixx04 for the motor (“Velocity-Loop Feedback Address”), taking this cycle’s actual
velocity-loop position value minus the value at the previous loop closure and multiplying the
difference by the Ixx09 scale factor. Note that this scale factor is not necessarily the same as for
the desired velocity.
For the desired position value, Turbo PMAC adds the trajectory commanded position and the
master position (from the “position following”, or “electronic gearing” function), then subtracts
the compensation position (from the position, or “leadscrew” compensation tables), creating a net
desired position value. The desired velocity value is simply this cycle’s desired position value
minus the value at the previous loop closure.
The following error value is the desired position the actual position. The subtraction is done
using 48-bit fixed-point values; then the difference is converted to floating-point format. There
are several advantages to using the following error value directly. First, it saves some
computational time. Second, when the commanded and actual positions get very large, it
preserves fractional position data better.
If the command COPYREG P5 were used, the Actual-Velocity value would be copied into P5,
Desired Velocity into P6, Following Error into P7, Actual Position into P8, and Desired Position
into P9. Note the differing units between the actual and desired velocity registers. (The desired
velocity value is not typically used in actual servo loop closure. Turbo PMAC uses this register
in the numerical integration process to compute the desired position value each servo cycle.)
Offsets from Registers of Executing Motor
The compiler’s L-variables and F-variables can be declared by address offset to specific registers
of the executing motor. In this way, they automatically index properly from motor to motor,
permitting the same variables and code to be used for multiple motors. These variables can be
declared by offset to the motor’s “R0” register, which is the motor’s command output register
($BF for Motor 1), or by offset to the motor’s “R1” register, which is the motor’s status register
($B0 for Motor 1). L-variables can be declared to 24-bit X or Y registers this way; F-variables
can be declared to 48-bit fixed-point or floating-point registers this way. Some examples:
L220->X:(R1-$27)
L270->Y:(R1+0)
F392->D:(R1-$24)
F34->L:(R0+11)
;
;
;
;
Ixx08
Motor
Motor
Ixx16
scale factor register
status register
master position register
maximum commanded speed
The offset must be in the range –64 <= offset <= 63 (-$40 <= offset <= $3F).
Returned Value
The RETURN command takes the integer value inside the following parentheses and places it in a
24-bit signed integer register where Turbo PMAC’s standard firmware will take it and use it as
the servo command. Typically, the commanded value will be computed as a floating-point value,
so must be converted to an integer with the ITOF function. Typical uses of the RETURN
command could be:
OPEN SERVO ALGORITHMS
7
DELTA TAU TURBO PMAC USER’S MANUAL
MAR-2003
RETURN(FTOI(P345))
L10=FTOI(P92/65536)
RETURN(L10)
The RETURN command will typically be the last line of an Open Servo algorithm. Putting it
earlier in the algorithm will not cause the command data to be used any sooner by the Turbo
PMAC. If the Open Servo program is used for a task other than servo-loop closure, there is no
need to use the RETURN command. In this case, when the Open Servo algorithm reaches the
CLOSE statement that is required at the end of the program, it will automatically write a 0 to this
holding register.
If Turbo PMAC is not performing commutation for this motor (Ixx01 bit 0 = 0), it will take the
resulting value and copy it to the register specified by Ixx02. If you do not use the RETURN
command in this case, Turbo PMAC will still copy the zero value that it has placed in the holding
register that would have been used by the RETURN command into the register specified by Ixx02.
If Turbo PMAC is performing commutation for this motor (Ixx01 bit 0 = 1), it will use the value
from this holding register as the “quadrature current” (torque) command input to the commutation
algorithm. In this case, Ixx02 specifies the multiple output registers from the commutation
algorithm.
The returned value must be an integer value in the range –8,388,608 to +8,388,607. Most of the
command output ranges associated with Turbo PMAC’s automatic servo loops are expressed as
16-bit values, with a range of –32,768 to +32,767. The values associated with RETURN are
therefore 256 times larger. The actual command output device will not necessarily have this full
24-bit resolution (and probably will not). In general, however, an n-bit output device uses the
high “n” bits of the 24-bit returned value.
Variable Value Assignments
Mathematical operations in an Open Servo algorithm are performed with “variable value
assignment” statements, just as in other PMAC programs. The syntactical rules for these
statements are the same as in other PMAC interpreted and compiled programs. Any I, P, Q, M,
L, or F-variable can be assigned a value, whether referenced directly or as part of any array.
Logical Control
Logical branching and looping control in Open Servo algorithms is performed with IF / [ELSE] /
ENDIF branching constructs, and WHILE / ENDWHILE looping constructs, just as in other
PMAC programs. The syntactical rules for these statements are the same as in PMAC PLC
programs; they do not support a few features possible in motion programs (such as an action on
the same line as a condition), and they do support a few features not possible in motion programs
(such as multiple-line conditions). Refer to the Program Command section of the Software
Reference manual for details (see IF, ELSE, ENDIF, WHILE, ENDWHILE, AND, OR).
If WHILE / ENDWHILE loops are used in an Open Servo, it is the user’s responsibility to make
sure that the algorithm never gets “stuck” in a loop so long that other tasks are compromised.
Turbo PMAC will not automatically release from a loop in an Open Servo for any other task of
equal or lower priority. Failure to release from a loop in a timely fashion can result in “servo
OPEN SERVO ALGORITHMS
8
DELTA TAU TURBO PMAC USER’S MANUAL
MAR-2003
error” (failure to complete one cycle’s servo-interrupt tasks by the next servo interrupt), “run-time
error” (failure to compute commanded move equations in time for that move to start, causing the
motion program to abort), or “watchdog timer error” (failure to cycle through all required tasks in
a timely fashion, causing the Turbo PMAC to shut down completely).
Processor Utilization
Servo algorithms are one of the most important tasks executed by the Turbo PMAC’s processor,
but far from the only one. While Turbo PMAC’s DSP processor is very efficient, it is still
possible to overload the processor, particularly with floating-point algorithms executing in
compiled code from the Open Servo. These do not run nearly as efficiently as the standard servo
algorithms, which have been written in assembly language and use fixed-point mathematics.
It is generally recommended that the portion of processor time devoted to phase and servo tasks
not exceed 50%, in order to allow sufficient time for lower-priority tasks, such as motion program
and PLC program calculations. To find out how much processor time an Open Servo algorithm
occupies, refer to the section Evaluating Turbo PMAC’s Computational Load.
Memory Utilization
The DSP563xx CPU for the Turbo PMAC employs a “Harvard architecture”, with separate areas
of “program”, or instruction, memory, and “data” memory. The actual instructions of the Open
Servo are loaded into “P” program memory; all of the data registers it uses are in “X” and “Y”
data memory.
If an Option 5Cx 80 MHz CPU configuration is ordered, employing the DSP56303, only 3K
words of program memory are available for the Open Servo (P:$040000 through P:$040BFF).
This may not be enough for large algorithms, so it is recommended for the user to order a 100
MHz Option 5Dx or 160 MHz 5Ex CPU configuration, utilizing the DSP56309 and DSP56311
CPUs, respectively, for any complex algorithms. With these processors, 19K words of program
memory are available for Open Servo code (P:$040000 through P:$044BFF). For reference, the
Open Servo example below that mimics the basic PID algorithm occupies 330 words of program
memory.
The downloader will tell you how many words of program memory the program you have just
compiled occupies. You can also tell this by reading memory location P:$040014 (use an
RHP:$040014 on-line command), which contains the location of the end of the Open Servo
algorithm in the Turbo PMAC program memory; subtract the starting location $040000
(remember that these are hexadecimal values) to get the program length.
For general data memory, most users will utilize some of Turbo PMAC’s 8192 P-variables to
store values. It is the user’s responsibility to keep track of which P-variables are used for Open
Servo algorithms and which are employed for other user tasks. Q-variables may also be used for
Open Servo algorithms; they are always accessed according to Coordinate System 1’s numbering.
It is the user’s responsibility to make sure that these P or Q-variables are not used for other tasks
as well
Many users will find it advantageous to utilize motor-specific registers that are not otherwise
being used because the Open Servo is executing for that motor. Registers listed as being gains or
intermediate values for either the PID or the ESA servo algorithms may safely be used by the
Open Servo. The registers in the range $000092 – $0000AC (for Motor 1; equivalent registers
for other motors) with the exception of $0000A5 (previous net desired position) may be used for
OPEN SERVO ALGORITHMS
9
DELTA TAU TURBO PMAC USER’S MANUAL
MAR-2003
this. Note that the “integrated position error” register at $00009E, and the “previous net desired
velocity” register at $00009A are automatically zeroed when the loop is opened. It is not
necessary to use these registers in the same format as the automatic servo algorithms would.
Note that any of these registers representing an I-variable for either the PID or ESA algorithms
would be overwritten by any command writing to that I-variable; that the values in such a register
are copied into flash memory on a SAVE command; that the last saved value from such a register
is copied from flash memory on a board reset. Some users may want to utilize these I-variable
registers as gains for their own servo algorithms – it is not necessary to use them for the same
purpose, or with the same scaling, as the built-in algorithms would.
For large amounts of extra data memory, it is recommended to use the “User Buffer” set up with
the on-line DEFINE UBUFFER command. The User Buffer occupies a number of registers at
the high end of X/Y data memory. With an Option 5x0 standard memory configuration, the end
of data memory is at X/Y:$0107FF; if DEFINE UBUFFER 2048 is declared, all data memory
from $010000 through $0107FF is reserved for the user’s own purposes. With an Option 5x3
extended memory configuration, the end of data memory is at X/Y:$03FFFF; there is by default a
User Buffer of 65,536 words, reserving all memory registers from X/Y:$030000 to X/Y:$03FFFF
for user use. It is the user’s responsibility to make sure that registers in the UBUFFER utilized
for Open Servo data storage are not used for other purposes as well.
Writing Your Open Servo Program
Your Open Servo program should be written in a plain-text editor such as the editor in the new
“PEWIN32 Pro” PMAC Executive program. While the program can be written in any plain-text
editor, it must be compiled by the PEWIN32PRO editor’s download function. In this program,
released in October 2001, the download routine will automatically recognize Open Servo
routines, compile them, and download the resulting machine code. (Older versions of the PMAC
Executive program are not capable of doing this.)
In the file containing the Open Servo preceding the actual program must be all L-variable and Fvariable pointer definitions, and all “#define” macro substitutions, or “#include” references
to accessible files that contain these definitions and substitutions. Remember that the built-in
compiler does not download these definitions and substitutions to the Turbo PMAC; it uses them
to do the compilation properly.
The OPEN SERVO command is a signal to the compiler that the statements following up to the
(required) CLOSE command are to be compiled into DSP machine code before downloading.
The CLEAR command that is used following the OPEN command on interpreted buffers is not
required for Open Servo algorithms, because downloading the newly compiled code
automatically clears older code, but it may still be used here.
Example 1: Proportional Control
The following algorithm shows one of the simplest possible Open Servo algorithms,
implementing a simple proportional control law using the motor’s Ixx30 parameter as the
proportional gain.
OPEN SERVO
CLEAR
OPEN SERVO ALGORITHMS
; Following lines to be compiled
; Not necessary, but acceptable
10
DELTA TAU TURBO PMAC USER’S MANUAL
MAR-2003
COPYREG P30
; Copy following error into P32
P35=P32*I(ITOF(MTRNUM*100+30))/65536 ; Multiply by gain, scale
RETURN(FTOI(P35))
; Make an integer and output
CLOSE
Example 2: Bi-Quad Filter
The next example shows an implementation of a “bi-quad” filter capable of running on multiple
motors, storing values from cycle to cycle for each motor. It uses “#define” substitution
macros to keep the code readable, and the MTRNUM function for variable and register arrays to
separate stored values for each motor. Variable arrays (which are easier for the user to access)
are used for user-set “gains”, and register arrays (which are quicker for the algorithm to access)
are used for algorithm-calculated stored values.
This Open Servo program implements the following transfer function:
U ( z)  K p
( z  a)( z  c)
E( z)
( z  b)( z  d )
It implements this as the following difference equation:
u k  K p ek  a  c ek 1  acek  2   b  d u k 1  bdu k  2
In order to start the algorithm correctly, it reads the servo cycle counter and compares it to the
counter the last time the last time this algorithm was executed. If the algorithm was not executed
the previous cycle, it zeros out the “history” values for the algorithm. It also does a saturation
check on the commanded output. This algorithm assumes a standard memory option (5x0) whose
data memory ends at X/Y:$0107FF and a UBUFFER defined of at lease 2048 words.
; Definitions and substitutions
#define
Kp
P(ITOF(MTRNUM*100+30)) ; Gain term is Pxx30
#define
A
P(ITOF(MTRNUM*100+31)) ; A zero is Pxx31
#define
B
P(ITOF(MTRNUM*100+32)) ; B zero is Pxx32
#define
C
P(ITOF(MTRNUM*100+33)) ; C pole is Pxx33
#define
D
P(ITOF(MTRNUM*100+34)) ; D pole is Pxx34
#define
E
P42
; Error term e(k) (not saved)
#define
Temp1 P45
; Temporary value
#define
Temp2 P46
; Temporary value
#define
Temp3 P47
; Temporary value
#define
U
P48
; Output term u(k) (not saved)
#define
LastE F1[MTRNUM-1]
; e(k-1) is F1[#-1]
F1->L:$010000[32]
; Float reg array in UBUFFER
#define
PrevE F2[MTRNUM-1]
; e(k-2) is F2[#-1]
F2->L:$010020[32]
; Float reg array in UBUFFER
#define
LastU F3[MTRNUM-1]
; u(k-1) is F3[#-1]
F3->L:$010040[32]
; Float reg array in UBUFFER
#define
PrevU F4[MTRNUM-1]
; u(k-2) is F4[#-1]
F4->L:$010060[32]
; Float reg array in UBUFFER
#define
ServoCycle
L0
L0->X:$0,0,24,S
; Servo cycle counter
#define
LastServoCycle
L1[MTRNUM-1]
L1->X:$010080[32] ; Register array in UBUFFER
#define
ServoExtension
L2
OPEN SERVO ALGORITHMS
11
DELTA TAU TURBO PMAC USER’S MANUAL
MAR-2003
L2->Y:(R1-$21)
; Ixx60 register as integer
#define
OutputLimit 8388607
; Start of actual algorithm
OPEN SERVO CLEAR
COPYREG P40
; Following error into P42
; If loop was not closed last cycle, zero out stored values
IF (ServoCycle-LastServoCycle!=ServoExtension+1)
LastU=0
PrevU=0
LastE=0
PrevE=0
ENDIF
LastServoCycle=ServoCycle
; Store for next cycle
Temp1=Kp*(E+(A+C)*LastE+A*C*PrevE)
Temp2=(B+D)*LastU+B*D*PrevU
Temp3=Temp1-Temp2
U=FLIMIT(Temp3,OutputLimit)
PrevE=LastE
LastE=E
PrevU=LastU
LastU=U
RETURN(FTOI(U))
CLOSE
;
;
;
;
Compute TF numerator
Compute TF denominator
Combine for net command
Saturation check
; Store values for next cycle
; Return command value as integer
Example 3: PMAC’s PID Filter
The following example mimics the action of Turbo PMAC’s basic PID loop (without the notch,
deadband compensation, position-error limiting, or friction feedforward terms), but with floatingpoint calculations. It uses the PID’s own I-variables, accessed as L-variables for speed, then
converted to floating-point values.
;****************************************
;
This program is a user written servo
; that replicates the PMAC PID loop.
;
It uses the following equations defined
; in the PMAC Users Manual but modified for PMAC passed terms:
; K16 = 2**-16
; K128 = 1/128
; K23 = 2**-23
; FE = DPOS - APOS - From PMAC in Ix08 * 32 counts
; AVEL, DVEL - From PMAC in Ix09 * 32 counts/servo period
; IPOS = ITOF(Ix33) * FE * K23 + IPOS
; DACEL = DVEL - PDVEL
; PDVEL = DVEL
; DACOUT = ITOF(Ixx30)*K16 * ( FE + K128 *(ITOF(Ix32) * DVEL +
; ITOF(Ix35) * DACEL - ITOF(Ix31) *AVEL) + IPOS)
#define K16 0.000015259
#define K128 0.0078125
#define K23 0.000000119
OPEN SERVO ALGORITHMS
; Constant of 2^-16
; Constant of 1/128
; Constant of 2^-23
12
DELTA TAU TURBO PMAC USER’S MANUAL
MAR-2003
; Use L-variables for quick access to PID I-variable registers
#define Ix08 L1
L1->Y:(R1-$27)
#define Ix09 L2
L2->Y:(R1-$14)
#define Ix30 L3
L3->Y:(R1-$17)
#define Ix31 L4
L4->X:(R1-$1E)
#define Ix32 L5
L5->X:(R1-$21)
#define Ix33 L6
L6->X:(R1-$11)
#define Ix35 L7
L7->Y:(R1-$1D)
#define Ix63 L8
L8->Y:(R1-$11)
#define Ix69 L9
; Note that this is 24-bit value, not 16
L9->Y:(R1-2)
#define STATUS L10
L10->X:(R1+0)
#define IPOS F1
F1->L:(R1-$12)
#define PDVEL F2
F2->L:(R1-$29)
;
;
;
;
Integrated position error register
Automatically zeroed on motor open loop
Previous desired velocity register
Automatically zeroed on motor open loop
#define
#define
#define
#define
#define
#define
;
;
;
;
;
;
Floating-point
Floating-point
Floating-point
Floating-point
Floating-point
Floating-point
AVEL P0
DVEL P1
FE P2
APOS P3
DPOS P4
DACOUT P5
actual velocity
net desired velocity
following error
actual position
net desired position
commanded output
OPEN SERVO CLEAR
COPYREG P0
; Copy Motor AVEL,DVEL,FE,APOS,DPOS to float P0..4
; FE, APOS, & DPOS in 1 / [Ix08* 32] counts
; AVEL in 1 / [Ix09*32] counts/servo period
; DVEL in 1 / [Ix08*32] counts/servo period;
; if( Ix34 = 1 && Des_Vel0 = 1 OR Ix34 = 0)
; then integrate IPOS = IPOS + FE * Ix33 && Limit to Ix63
If (STATUS&$12000 = $12000 Or STATUS&$10000 = 0) ; Test Ix34 mode
IPOS = FLIMIT(ITOF(Ix33)*FE*K23+IPOS, ITOF(Ix63)*ITOF(Ix08)*2)
; Scale Ix63 to include Ix08
EndIf
DACOUT = FLIMIT(ITOF(Ix30)*K16 * ( FE + K128 *(ITOF(Ix32) * DVEL +
ITOF(Ix35) * (DVEL - PDVEL) - ITOF(Ix31) *AVEL) + IPOS), ITOF(Ix69))
PDVEL =DVEL
; Store for next cycle
RETURN(FTOI(DACOUT))
CLOSE
OPEN SERVO ALGORITHMS
13
DELTA TAU TURBO PMAC USER’S MANUAL
MAR-2003
Downloading Your Program
Each time the Executive program’s downloader sends the compiled code for an Open Servo
algorithm to the Turbo PMAC, it first completely erases the existing “user-written servo” code in
the Turbo PMAC. Therefore, you must always re-compile your entire Open Servo, even for the
slightest change. The CLEAR command used at the start of these examples is not therefore
needed, but it can be included for consistency of style with interpreted programs.
Remember that the download process only puts the Open Servo algorithm into the volatile active
RAM memory. If you wish to keep this algorithm loaded in the Turbo PMAC through a reset or
power-cycling, you must copy it to non-volatile flash memory with the SAVE command first.
Executing Your Open Servo Program
For the Open Servo algorithm to execute for a given motor, Ixx00 for the motor must be equal to
1 so that motor calculations are activated. Bit 0 of Ixx59 must be set to 1 (Ixx59 = 1 or 3) so it
will choose the “user-written servo” algorithm generated from the Open Servo instead of a builtin servo algorithm (PID or ESA). Finally, the servo loop must be closed for this motor. The
Open Servo algorithm will not executed if the motor is either in the “open-loop” enabled state, or
the “killed” (open-loop disabled) state.
The Open Servo algorithm obeys the Ixx60 servo-cycle extension parameter for each Motor xx.
As with the built-in PID and ESA servo algorithms, it executes every [Ixx60+1] servo interrupts.
With the default Ixx60 value of 0, it executes every servo interrupt.
Those users who are employing the Open Servo algorithm for tasks other than actually closing
servo loops must be careful not to disable the algorithm unintentionally. The algorithm will not
be running immediately on power-up/reset unless bit 0 of Ixx80 for the motor is set to 1. A
<CONTROL-K> (“kill all”) command disables the servo loops of all motors, including a pseudomotor running an Open Servo algorithm. With the default settings of bits 21 and 22 of Ixx24 an
amplifier fault or fatal following-error fault on any motor disables the servo loops of all motors,
including a pseudo-motor running an Open Servo algorithm.
The Open Servo replaces only the actual servo-loop closure algorithm for the selected motor(s).
Other tasks done as part of the servo interrupt, including encoder-conversion-table processing,
commanded trajectory generation, position following, and time-base control, are executed by the
built-in firmware, whether or not the Open Servo is selected for any motor. Also, related tasks
done outside of the servo interrupt, such as checking against a fatal following error limit, are
executed by the built-in firmware.
OPEN SERVO ALGORITHMS
14