127 lines
3.3 KiB
Plaintext
Executable File
127 lines
3.3 KiB
Plaintext
Executable File
|
|
#- Initial PLC values
|
|
if(${SELF}.firstscan) {
|
|
static.counter:=0;
|
|
static.VMState:=-2;
|
|
};
|
|
|
|
#- this is needed because initially upon IocInit, motor busy flags are briefly enabled, and the first motorRecord poll at 100ms might cause issues
|
|
if ( static.counter<0.105 ) {
|
|
static.counter += ${SELF}.scantime;
|
|
|
|
if ( static.counter>=0.105) { static.VMState:=0 };
|
|
|
|
#- VMs in Internal mode, ready to listen
|
|
mc_grp_set_traj_src(${GRP_ID_MA},0);
|
|
};
|
|
|
|
|
|
# STATES
|
|
# State -1: at least 1 RM is in motion, RM in charge.
|
|
# State 0: all RM and VM are off.
|
|
# State 1: at least 1 VM is in motion, VM in charge.
|
|
|
|
|
|
#- State -1
|
|
if(static.VMState==-1) {
|
|
|
|
#- Trigg MR sync for all virt axes
|
|
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_SA})==0 ) {
|
|
|
|
#- disable real motors
|
|
mc_grp_set_enable(${GRP_ID_SA},0);
|
|
|
|
#- VMs in Internal mode, back to ground state
|
|
mc_grp_set_traj_src(${GRP_ID_MA},0);
|
|
|
|
#- state change
|
|
println('-1 -> 0');
|
|
static.VMState:=0;
|
|
};
|
|
|
|
#- 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_MA})==1 and mc_grp_get_any_enable(${GRP_ID_MA})==1 ) {
|
|
mc_grp_set_enable(${GRP_ID_MA},0);
|
|
};
|
|
}
|
|
|
|
#- State 0
|
|
else if(static.VMState==0) {
|
|
|
|
#- 0 -> -1
|
|
#- Trigger Condition:
|
|
#- - at least 1 RM is running
|
|
#- - all RM are in internal mode
|
|
#- - all VM are disabled
|
|
|
|
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_MA},0);
|
|
|
|
#- state change
|
|
println('0 -> -1');
|
|
static.VMState:=-1;
|
|
}
|
|
|
|
#- 0 -> 1
|
|
#- Trigger Condition:
|
|
#- - at least 1 VM is enabled
|
|
else if( mc_grp_get_any_enabled(${GRP_ID_MA})==1 ) {
|
|
|
|
#- Exit Condition: all axes are on
|
|
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_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_MA},0);
|
|
};
|
|
}
|
|
|
|
#- Actions: If Exit Conditions not met
|
|
else {
|
|
#- enable motors
|
|
mc_grp_set_enable(${GRP_ID_MA},1);
|
|
mc_grp_set_enable(${GRP_ID_SA},1);
|
|
|
|
#- halt real motors
|
|
mc_grp_halt(${GRP_ID_SA});
|
|
};
|
|
};
|
|
}
|
|
|
|
#- State 1
|
|
else if(static.VMState==1) {
|
|
|
|
#- basically a done test
|
|
#- 1 -> 0
|
|
if( mc_grp_get_any_busy(${GRP_ID_MA})==0 ) {
|
|
|
|
#- disable motors and set source
|
|
mc_grp_set_enable(${GRP_ID_MA},0);
|
|
mc_grp_set_enable(${GRP_ID_SA},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_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_SA});
|
|
|
|
#- state change
|
|
println('1 -> 0');
|
|
static.VMState:=0;
|
|
};
|
|
};
|
|
};
|