19 lines
834 B
Plaintext
19 lines
834 B
Plaintext
|
|
#- Slave axes (normally physical/real axes): AX_S1, AX_S2, AX_S3, AX_S4
|
|
var SLAVE_AXES[4] := {ax${AX_S1}.enc.actpos, ax${AX_S2}.enc.actpos, ax${AX_S3}.enc.actpos, ax${AX_S4}.enc.actpos};
|
|
|
|
#- Master axes (normally virtual axes): AX_M1, AX_M2, AX_M3, AX_M4
|
|
var MASTER_AXES[4] := {ax${AX_M1}.traj.setpos, ax${AX_M2}.traj.setpos, ax${AX_M3}.traj.setpos, ax${AX_M4}.traj.setpos};
|
|
|
|
#- forward kinematics
|
|
ax${AX_M1}.enc.actpos := dot(FWD1, SLAVE_AXES);
|
|
ax${AX_M2}.enc.actpos := dot(FWD2, SLAVE_AXES);
|
|
ax${AX_M3}.enc.actpos := dot(FWD3, SLAVE_AXES);
|
|
ax${AX_M4}.enc.actpos := dot(FWD4, SLAVE_AXES);
|
|
|
|
#- inverse kinematics
|
|
ax${AX_S1}.traj.extsetpos := dot(INV1, MASTER_AXES);
|
|
ax${AX_S2}.traj.extsetpos := dot(INV2, MASTER_AXES);
|
|
ax${AX_S3}.traj.extsetpos := dot(INV3, MASTER_AXES);
|
|
ax${AX_S4}.traj.extsetpos := dot(INV4, MASTER_AXES);
|