motion/ecmc_master_slave
0
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Update macros to sm

Browse Source
This commit is contained in:
sandst_a
2024-09-26 16:00:27 +02:00
parent 184f01302d
commit c485498cb7
5 changed files with 113 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files

89
README.md
View File

@@ -1,5 +1,88 @@
# ecmc_master_slave
Standard syncronization scripts for ecmc
Standard syncronization scripts for ecmc. The module provides:
* Syncronization scripts
* Generic state machine
## Syncronization scripts
Syncronazation can be done both by defining the forward adn inverse kinematcis as equations or by matrices.
The module implement 5 generic syncronazation scripts based on the matrix approach:
* axis_kin_2DoF.plc_inc : Kinematics for a 2 Dof system (slits)
* axis_kin_3DoF.plc_inc : Kinematics for a 3 Dof system (slits)
* axis_kin_4DoF.plc_inc : Kinematics for a 4 Dof system (slits)
* axis_kin_5DoF.plc_inc : Kinematics for a 5 Dof system (slits)
* axis_kin_6DoF.plc_inc : Kinematics for a 6 Dof system (slits)
and 2 dedicated:
* axis_kin_slit.plc_inc : Dedicated script for a slit system based on equations (including interlocks and softlimits)
* axis_kin_mirror.plc_inc : Dedicated script for a 5 DoF mirror based on matrices
### Generic scripts (axis_kin_xDoF.plc_inc)
The generic scripts needs a few macros:
* AX_S\<index\> : Axis index for slave axes 1..DoF
* AX_M\<index\> : Axis index for master axes 1..DoF
* FWD\<index\ : Forward kinematics array 1..DoF
* INV\<index\ : Inverse kinematics array 1..DoF
## State machine
The state machine makes sure that master and slaved axes cannot run simultaneous. However, several master axes or several slave axes can run at the same time.
The state machine is implemented in the file axis_sm.plc.inc in this module.
The state machine needs two macros:
* GRP_ID_MA : Group id for master axes (most often virtual axes)
* GRP_ID_SA : Group id for slave axes (most often real axes)
The easiest way to create a group is to just add it in the axis yaml configuration:
```
axis:
id: 5
group: realAxes
```
If the group does not exist, it will be created. The id of the group will be accessible through the environment variable "GRP_\<group_name\>_ID", in this case "GRP_realAxes_ID".
This macro can the be used when loading the PLC-code.
## Using the scripts
The easiest way is to generate a main plc file and then use include to access the kinematic and state machine scripts.
Example for a slit system:
```
/* 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.
include "axis_kin_2DoF.plc_inc"
# State machine
include "axis_sm.plc_inc"
```
The main plc file can then be loaded into ecmc with appropriate macros (also not the INC):
```
${SCRIPTEXEC} ${ecmccfg_DIR}loadPLCFile.cmd, "FILE=./cfg/axis_main.plc, PLC_ID=1, INC=.:${ecmc_master_slave_DIR}, PLC_MACROS='PLC_ID=1, AX_M1=12, AX_M2=13, AX_S1=5, AX_S2=6, GRP_ID_SA=${GRP_realAxes_ID}, GRP_ID_MA=${GRP_virtualAxes_ID}'"
```
NOTE: Macros cannot not be used in the include statement. The path to this module needs to set in the INC parameter to loadPLCFile.cmd
## Examples
See examples dir for usage:
* slits
* mirror
* slits (equations, 2DoFs matrix)
* mirror (matrix)

48
axis_sm.plc_inc
View File

@@ -12,7 +12,7 @@ if ( static.counter<0.105 ) {
if ( static.counter>=0.105) { static.VMState:=0 };
#- VMs in Internal mode, ready to listen
mc_grp_set_traj_src(${GRP_ID_VA},0);
mc_grp_set_traj_src(${GRP_ID_MA},0);
};
@@ -26,19 +26,19 @@ if ( static.counter<0.105 ) {
if(static.VMState==-1) {
#- Trigg MR sync for all virt axes
mc_grp_sync_act_set(${GRP_ID_VA},1);
mc_grp_sync_act_set(${GRP_ID_MA},1);
#- RM are no longer in motion
#- -1 -> 0
#- Trigger Condition:
#- - all RM are not busy
if( mc_grp_get_any_busy(${GRP_ID_RA})==0 ) {
if( mc_grp_get_any_busy(${GRP_ID_SA})==0 ) {
#- disable real motors
mc_grp_set_enable(${GRP_ID_RA},0);
mc_grp_set_enable(${GRP_ID_SA},0);
#- VMs in Internal mode, back to ground state
mc_grp_set_traj_src(${GRP_ID_VA},0);
mc_grp_set_traj_src(${GRP_ID_MA},0);
#- state change
println('-1 -> 0');
@@ -46,8 +46,8 @@ if(static.VMState==-1) {
};
#- If RM Motion is occuring, and user commands VM motion: simply disable the VMs to void the command.
if( mc_grp_get_any_enabled(${GRP_ID_VA})==1 and mc_grp_get_any_enable(${GRP_ID_VA})==1 ) {
mc_grp_set_enable(${GRP_ID_VA},0);
if( mc_grp_get_any_enabled(${GRP_ID_MA})==1 and mc_grp_get_any_enable(${GRP_ID_MA})==1 ) {
mc_grp_set_enable(${GRP_ID_MA},0);
};
}
@@ -60,10 +60,10 @@ else if(static.VMState==0) {
#- - all RM are in internal mode
#- - all VM are disabled
mc_grp_get_any_traj_src_ext(${GRP_ID_RA})==0
if( mc_grp_get_any_busy(${GRP_ID_RA})==1 and mc_grp_get_any_traj_src_ext(${GRP_ID_RA})==0 and mc_grp_get_any_enabled(${GRP_ID_VA})==0 ) {
mc_grp_get_any_traj_src_ext(${GRP_ID_SA})==0
if( mc_grp_get_any_busy(${GRP_ID_SA})==1 and mc_grp_get_any_traj_src_ext(${GRP_ID_SA})==0 and mc_grp_get_any_enabled(${GRP_ID_MA})==0 ) {
#- VMs in PLC mode, so no following errors occur
mc_grp_set_traj_src(${GRP_ID_VA},0);
mc_grp_set_traj_src(${GRP_ID_MA},0);
#- state change
println('0 -> -1');
@@ -73,29 +73,29 @@ else if(static.VMState==0) {
#- 0 -> 1
#- Trigger Condition:
#- - at least 1 VM is enabled
else if( mc_grp_get_any_enabled(${GRP_ID_VA})==1 ) {
else if( mc_grp_get_any_enabled(${GRP_ID_MA})==1 ) {
#- Exit Condition: all axes are on
if( mc_grp_get_enabled(${GRP_ID_RA})==1 and mc_grp_get_enabled(${GRP_ID_VA})==1 and mc_grp_get_any_busy(${GRP_ID_RA})==0 ) {
if( mc_grp_get_enabled(${GRP_ID_SA})==1 and mc_grp_get_enabled(${GRP_ID_MA})==1 and mc_grp_get_any_busy(${GRP_ID_SA})==0 ) {
if( mc_grp_get_any_busy(${GRP_ID_VA})==1 ) {
mc_grp_set_traj_src(${GRP_ID_RA},1);
if( mc_grp_get_any_busy(${GRP_ID_MA})==1 ) {
mc_grp_set_traj_src(${GRP_ID_SA},1);
#- state change
println('0 -> 1');
static.VMState:=1;
#- Disable MR sync for all virt axes
mc_grp_sync_act_set(${GRP_ID_VA},0);
mc_grp_sync_act_set(${GRP_ID_MA},0);
};
}
#- Actions: If Exit Conditions not met
else {
#- enable motors
mc_grp_set_enable(${GRP_ID_VA},1);
mc_grp_set_enable(${GRP_ID_RA},1);
mc_grp_set_enable(${GRP_ID_MA},1);
mc_grp_set_enable(${GRP_ID_SA},1);
#- halt real motors
mc_grp_halt(${GRP_ID_RA});
mc_grp_halt(${GRP_ID_SA});
};
};
}
@@ -105,18 +105,18 @@ else if(static.VMState==1) {
#- basically a done test
#- 1 -> 0
if( mc_grp_get_any_busy(${GRP_ID_VA})==0 ) {
if( mc_grp_get_any_busy(${GRP_ID_MA})==0 ) {
#- disable motors and set source
mc_grp_set_enable(${GRP_ID_VA},0);
mc_grp_set_enable(${GRP_ID_RA},0);
mc_grp_set_enable(${GRP_ID_MA},0);
mc_grp_set_enable(${GRP_ID_SA},0);
mc_grp_set_traj_src(${GRP_ID_RA},0);
mc_grp_set_traj_src(${GRP_ID_SA},0);
#- once VMs are done, RMs need a bit longer to disable
if( mc_grp_get_any_busy(${GRP_ID_RA})==0 ) {
if( mc_grp_get_any_busy(${GRP_ID_SA})==0 ) {
#- For some reason, an interlock is raised, but not sure why or what the implications are. It can be cleared this way.
mc_grp_reset_error(${GRP_ID_RA});
mc_grp_reset_error(${GRP_ID_SA});
#- state change
println('1 -> 0');

2
example/mirror/startup.script_X10SA-CPCL-ES1
View File

@@ -63,7 +63,7 @@ ${SCRIPTEXEC} ${ECMC_CONFIG_ROOT}loadYamlAxis.cmd, "FILE=cfg/ax10_ROLL.yaml"
${SCRIPTEXEC} ${ECMC_CONFIG_ROOT}loadYamlAxis.cmd, "FILE=cfg/ax11_YAW.yaml"
# load the VFM virtual axes PLC
${SCRIPTEXEC} ${ECMC_CONFIG_ROOT}loadPLCFile.cmd, "FILE=cfg/VFM.plc,PLC_ID=1,INC=.:${ecmc_master_slave_DIR},PLC_MACROS='PLC_ID=1,AX_Y1=1,AX_Y2=2,AX_Y3=3,AX_X1=4,AX_X2=5,AX_TRX=7,AX_TRY=8,AX_PITCH=9,AX_ROLL=10,AX_YAW=11,GRP_ID_RA=${GRP_VFM_REAL_ID=0},GRP_ID_VA=${GRP_VFM_VIRT_ID=0}'"
${SCRIPTEXEC} ${ECMC_CONFIG_ROOT}loadPLCFile.cmd, "FILE=cfg/VFM.plc,PLC_ID=1,INC=.:${ecmc_master_slave_DIR},PLC_MACROS='PLC_ID=1,AX_Y1=1,AX_Y2=2,AX_Y3=3,AX_X1=4,AX_X2=5,AX_TRX=7,AX_TRY=8,AX_PITCH=9,AX_ROLL=10,AX_YAW=11,GRP_ID_SA=${GRP_VFM_REAL_ID=0},GRP_ID_MA=${GRP_VFM_VIRT_ID=0}'"
#- #############################################################################
#- Temporary test

2
example/slit/slit_equations/startup.script_ecmc
View File

@@ -18,7 +18,7 @@ asynSetTraceMask(${ECMC_ASYN_PORT}, -1, 0x01)
#- #################################################################
#- PLCs with inverse kinematics (note the INC var including dirs to search for include files)
#- The group ID:s configured in yaml are stored in GRP_<axis.group>_ID
${SCRIPTEXEC} ${ecmccfg_DIR}loadPLCFile.cmd, "FILE=./cfg/axis_main.plc, PLC_ID=1, INC=.:${ecmc_master_slave_DIR}, PLC_MACROS='PLC_ID=1, AX_CEN=12, AX_GAP=13, AX_LO=5, AX_HI=6, GRP_ID_RA=${GRP_realAxes_ID}, GRP_ID_VA=${GRP_virtualAxes_ID}'"
${SCRIPTEXEC} ${ecmccfg_DIR}loadPLCFile.cmd, "FILE=./cfg/axis_main.plc, PLC_ID=1, INC=.:${ecmc_master_slave_DIR}, PLC_MACROS='PLC_ID=1, AX_CEN=12, AX_GAP=13, AX_LO=5, AX_HI=6, GRP_ID_SA=${GRP_realAxes_ID}, GRP_ID_MA=${GRP_virtualAxes_ID}'"
#- #################################################################
#- go active

2
example/slit/slit_matrix/startup.script_ecmc
View File

@@ -17,7 +17,7 @@ asynSetTraceMask(${ECMC_ASYN_PORT}, -1, 0x01)
#- #################################################################
#- PLCs with kinematics (note the INC var including dirs to search for include files)
#- The group ID:s configured in yaml are stored in GRP_<axis.group>_ID
${SCRIPTEXEC} ${ecmccfg_DIR}loadPLCFile.cmd, "FILE=./cfg/axis_main.plc, PLC_ID=1, INC=.:${ecmc_master_slave_DIR}, PLC_MACROS='PLC_ID=1, AX_M1=12, AX_M2=13, AX_S1=5, AX_S2=6, GRP_ID_RA=${GRP_realAxes_ID}, GRP_ID_VA=${GRP_virtualAxes_ID}'"
${SCRIPTEXEC} ${ecmccfg_DIR}loadPLCFile.cmd, "FILE=./cfg/axis_main.plc, PLC_ID=1, INC=.:${ecmc_master_slave_DIR}, PLC_MACROS='PLC_ID=1, AX_M1=12, AX_M2=13, AX_S1=5, AX_S2=6, GRP_ID_SA=${GRP_realAxes_ID}, GRP_ID_MA=${GRP_virtualAxes_ID}'"
#- #################################################################
#- go active