32 lines
1.1 KiB
Plutus Core
32 lines
1.1 KiB
Plutus Core
/* Forward kinematics to calculate virtual axes from real axes
|
|
| CEN | = FWD * | S1_LO |
|
|
| GAP | | S2_HI |
|
|
|
|
Equations:
|
|
ax{AX_CEN}.enc.actpos:=(ax{AX_LO}.enc.actpos+ax{AX_HI}.enc.actpos)/2;
|
|
ax{AX_GAP}.enc.actpos:=ax{AX_HI}.enc.actpos-ax{AX_LO}.enc.actpos;
|
|
*/
|
|
|
|
var FWD1[2] := {0.5, 0.5};
|
|
var FWD2[2] := { -1, 1 };
|
|
|
|
/* Inverse kinematics to calculate real axes from virtal axes
|
|
| S1_LO | = INV * | CEN |
|
|
| S2_HI | | GAP |
|
|
|
|
Equations:
|
|
ax{AX_LO}.traj.extsetpos:=ax{AX_CEN}.traj.setpos-ax{AX_GAP}.traj.setpos/2;
|
|
ax{AX_HI}.traj.extsetpos:=ax{AX_CEN}.traj.setpos+ax{AX_GAP}.traj.setpos/2;
|
|
*/
|
|
|
|
var INV1[2] := { 1, -0.5};
|
|
var INV2[2] := { 1, 0.5};
|
|
|
|
####### Kinematics for slit system.
|
|
# Macros cannot be used in an include. In order to find files, the loadPLCFile.cmd parameter "INC" defines the dirs that MSI will look for the file)
|
|
include "axis_kin_2DoF.plc_inc"
|
|
|
|
####### State machine
|
|
# Macros cannot be used in an include. In order to find files, the loadPLCFile.cmd parameter "INC" defines the dirs that MSI will look for the file)
|
|
include "axis_sm.plc_inc"
|