140 lines
3.7 KiB
Plutus Core
140 lines
3.7 KiB
Plutus Core
|
|
#- forward kinematics
|
|
ax${AX_TRY}.enc.actpos := (ax${AX_Y1}.enc.actpos + ax${AX_Y2}.enc.actpos) / 2;
|
|
|
|
#- inverse kinematics
|
|
ax${AX_Y1}.traj.setpos := ax${AX_TRY}.traj.setpos;
|
|
ax${AX_Y2}.traj.setpos := ax${AX_TRY}.traj.setpos;
|
|
|
|
#- 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_VIRT_ID}, 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_REAL_ID}) == 0) {
|
|
|
|
#- disable real motors
|
|
mc_grp_set_enable(${GRP_REAL_ID}, 0);
|
|
|
|
#- VMs in Internal mode, back to ground state
|
|
mc_grp_set_traj_src(${GRP_VIRT_ID}, 0);
|
|
|
|
ax${AX_TRY}.traj.setpos := ax${AX_TRY}.enc.actpos
|
|
ax${AX_PITCH}.traj.setpos := ax${AX_PITCH}.enc.actpos
|
|
ax${AX_ROLL}.traj.setpos := ax${AX_ROLL}.enc.actpos
|
|
|
|
#- 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_VIRT_ID}) == 1 and mc_grp_get_any_enable(${GRP_VIRT_ID}) == 1) {
|
|
mc_grp_set_enable(${GRP_VIRT_ID}, 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
|
|
if (mc_grp_get_any_busy(${GRP_REAL_ID}) == 1 and ax${AX_Y1}.traj.source == 0 and ax${AX_Y2}.traj.source == 0 and ax${AX_Y3}.traj.source == 0 and mc_grp_get_any_enabled(${GRP_VIRT_ID}) == 0) {
|
|
|
|
#- VMs in PLC mode, so no following errors occur
|
|
mc_grp_set_traj_src(${GRP_VIRT_ID}, 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_VIRT_ID}) == 1) {
|
|
|
|
#- Exit Condition: all axes are on
|
|
if (mc_grp_get_enabled(${GRP_REAL_ID}) == 1 and mc_grp_get_enabled(${GRP_VIRT_ID}) == 1 and mc_grp_get_any_busy(${GRP_REAL_ID}) == 0) {
|
|
|
|
if (mc_grp_get_any_busy(${GRP_VIRT_ID}) == 1) {
|
|
|
|
mc_grp_set_traj_src(${GRP_REAL_ID}, 1);
|
|
|
|
#- state change
|
|
println('0 -> 1');
|
|
static.VMState := 1;
|
|
};
|
|
}
|
|
|
|
#- Actions: If Exit Conditions not met
|
|
else {
|
|
#- enable motors
|
|
mc_grp_set_enable(${GRP_VIRT_ID}, 1);
|
|
mc_grp_set_enable(${GRP_REAL_ID}, 1);
|
|
|
|
#- halt real motors
|
|
mc_grp_halt(${GRP_REAL_ID});
|
|
|
|
println('state 0: eanble all');
|
|
}
|
|
}
|
|
}
|
|
|
|
#- State 1
|
|
else if (static.VMState == 1) {
|
|
|
|
#- basically a done test
|
|
#- 1 -> 0
|
|
if (mc_grp_get_any_busy(${GRP_VIRT_ID}) == 0) {
|
|
|
|
#- disable motors and set source to internal
|
|
mc_grp_set_enable(${GRP_VIRT_ID}, 0);
|
|
mc_grp_set_enable(${GRP_REAL_ID}, 0);
|
|
|
|
#if (mc_grp_get_any_enabled(${GRP_REAL_ID}) == 0) {
|
|
|
|
mc_grp_set_traj_src(${GRP_REAL_ID}, 0);
|
|
#}
|
|
|
|
#- once VMs are done, RMs need a bit longer to disable
|
|
if (mc_grp_get_any_busy(${GRP_REAL_ID}) == 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_REAL_ID});
|
|
|
|
#- state change
|
|
println('1 -> 0');
|
|
static.VMState := 0;
|
|
};
|
|
};
|
|
}
|