User's Manual

Turbo PMAC User Manual
Setting Up a Coordinate System 261
Reserved Variables
If kinematic calculations are used in a system, the global variables P1 – P32 and the coordinate-system
variables Q1 – Q10 should not be used for any other purposes, because Turbo PMAC will write to these
variables automatically in executing the kinematic routines. (Q10 is used to distinguish between inverse-
kinematic calculations that involve velocity calculations and those that do not, as explained below.) If
inverse-kinematic calculations involving PVT-mode moves are used, additionally the global variables
P101-P132 and the coordinate-system variables Q11 – Q19 should not be used for any other purposes,
because Turbo PMAC will write to these variables automatically in executing the velocity portions of the
inverse-kinematic routines.
Example
Take the example of a 2-axis shoulder-elbow robot, with an upper-arm length (L
1
) of 400mm, and a lower-
arm length (L
2
) of 300mm. Both the shoulder joint (A) and the elbow joint (B) have resolutions of 1000
counts per degree. When both joints are at their zero-degree positions, the two links are both extended
along the X-axis. The forward-kinematic equations are:
)BAcos(
2
L)Acos(
1
LX
+
+
=
)BAsin(
2
L)Asin(
1
LY
+
+
=
To implement these equations in a Turbo PMAC forward-kinematic program for Coordinate System 1 that
converts the shoulder angle in Motor 1 and the elbow angle in Motor 2 to the X and Y tip coordinates in
millimeters, the following setup and program could be used:
; Setup for program
I15=0 ; Trig calculations in degrees
&1 ; Address CS 1
M145->Y:$0000C0,10,1 ; Motor 1 home complete bit
M245->Y:$000140,10,1 ; Motor 2 home complete bit
M5182->Y:$00203F,22,1 ; CS 1 run-time error bit
Q91=400 ; L1
Q92=300 ; L2
Q93=1000 ; Counts per degree for A and B
; Forward-kinematic program buffer for repeated execution
&1 OPEN FORWARD ; Forward kinematics for CS 1
CLEAR ; Erase existing contents
IF (M145=1 AND M245=1) ; Properly position referenced?
Q7=Q91*COS(P1/Q93)+Q92*COS((P1+P2)/Q93) ; X position
Q8=Q91*SIN(P1/Q93)+Q92*SIN((P1+P2)/Q93) ; Y position
ELSE ; Not valid; halt operation
M5182=1 ; Set run-time error bit
ENDIF
CLOSE