Delta Tau GEO BRICK LV ユーザーズマニュアル
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
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:
along the X-axis. The forward-kinematic equations are:
)
B
A
cos(
2
L
)
A
cos(
1
L
X
+
+
=
)
B
A
sin(
2
L
)
A
sin(
1
L
Y
+
+
=
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
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