122 lines
3.1 KiB
Plaintext
Executable File
122 lines
3.1 KiB
Plaintext
Executable File
|
|
#- Initial PLC values
|
|
if(plc${PLC_ID}.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<105 ) {
|
|
static.counter+=1;
|
|
|
|
if ( static.counter==105) { static.VMState:=0 };
|
|
|
|
#- VMs in Internal mode, ready to listen
|
|
mc_grp_set_traj_src(${GRP_ID_VA},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) {
|
|
|
|
#- 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 ) {
|
|
|
|
#- disable real motors
|
|
mc_grp_set_enable(${GRP_ID_RA},0);
|
|
|
|
#- VMs in Internal mode, back to ground state
|
|
mc_grp_set_traj_src(${GRP_ID_VA},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_VA})==1 and mc_grp_get_any_enable(${GRP_ID_VA})==1 ) {
|
|
mc_grp_set_enable(${GRP_ID_VA},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_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 ) {
|
|
#- VMs in PLC mode, so no following errors occur
|
|
mc_grp_set_traj_src(${GRP_ID_VA},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_VA})==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_any_busy(${GRP_ID_VA})==1 ) {
|
|
mc_grp_set_traj_src(${GRP_ID_RA},1);
|
|
#- state change
|
|
println('0 -> 1');
|
|
static.VMState:=1;
|
|
};
|
|
}
|
|
|
|
#- 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);
|
|
|
|
#- halt real motors
|
|
mc_grp_halt(${GRP_ID_RA});
|
|
};
|
|
};
|
|
}
|
|
|
|
#- State 1
|
|
else if(static.VMState==1) {
|
|
|
|
#- basically a done test
|
|
#- 1 -> 0
|
|
if( mc_grp_get_any_busy(${GRP_ID_VA})==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_traj_src(${GRP_ID_RA},0);
|
|
|
|
#- once VMs are done, RMs need a bit longer to disable
|
|
if( mc_grp_get_any_busy(${GRP_ID_RA})==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});
|
|
|
|
#- state change
|
|
println('1 -> 0');
|
|
static.VMState:=0;
|
|
};
|
|
};
|
|
};
|