* Set Default Releases and allowed actions for robots.
* Default is resticted most, but value gets overwritten in valid operation mode *)
OperationReleased := FALSE;
UsedStopMode := Settings.StopModeMissingOperationRelease;
EStopReleased := FALSE;
SafetyDoorFree := FALSE;
EnableSwitchFree := FALSE;
PowerReleased := FALSE;
JoggingEnabled := FALSE;
ProgamExecutionEnabled := FALSE;
FullSpeedEnabled := FALSE;
HoldToRunDisabled := FALSE;
(*---------------------------------------------------------------------------
* Check bootup state, do nothing if robots are not ready yet *)
IF (NOT gRC_InitDone) THEN
Valid := FALSE;
RETURN; (******************* RETURN ******************)
END_IF;
(* Bootup finished, do work *)
mFbGetRobots(Enable := TRUE);
IF (NOT mFbGetRobots.Valid) OR (mFbGetRobots.GroupCnt < 1) THEN
Valid := FALSE;
RETURN; (******************* RETURN ******************)
END_IF;
(* read robots information valid -> ok now, do work *)
Valid := TRUE;
IF (mFbGetRobots.GroupCnt = 1) THEN
(* single kinematic system - use always first robot as active group *)
ActiveGroup := gRC_AxesGroups[0]^;
ELSIF (RcIsAxesGroupRefValid(mFbGetRobots.ActiveGroup^)) THEN
(* use active group from FB 'RCE_SystemReadStatus_001' *)
ActiveGroup := mFbGetRobots.ActiveGroup^;
ELSE
(* no handterminal or no focussed kinematic -> use first robot as active group *)
ActiveGroup := gRC_AxesGroups[0]^;
END_IF;
(*---------------------------------------------------------------------------
* Collect desired operation mode *)
IF (OpMode_Manual AND NOT OpMode_Automatic AND NOT OpMode_AutomaticExtern) THEN
(* manual mode reduced speed (T1), and no other mode *)
CurrentOperationMode := eRCE_OperationModeManualT1;
mFbOpModeChgToleranceTmr(IN := FALSE);
ELSIF (NOT OpMode_Manual AND OpMode_Automatic AND NOT OpMode_AutomaticExtern) THEN
(* automatic mode, and no other mode *)
CurrentOperationMode := eRCE_OperationModeAutomatic;
mFbOpModeChgToleranceTmr(IN := FALSE);
ELSIF (NOT OpMode_Manual AND NOT OpMode_Automatic AND OpMode_AutomaticExtern) THEN
(* automatic extern mode, and no other mode *)
CurrentOperationMode := eRCE_OperationModeAutomaticExtern;
mFbOpModeChgToleranceTmr(IN := FALSE);
ELSE
(* no or more than one operation mode given -> operation mode invalid, consider tolerance time *)
IF Settings.ModeSelectorToleranceTime <= T#0s THEN
(* no tolerance -> invalid mode immediately *)
CurrentOperationMode := eRCE_OperationModeInvalid;
ELSE
(* run operation mode change tolerance timer - during this, do not change and keep old active operation mode *)
mFbOpModeChgToleranceTmr(IN := TRUE, PT := Settings.ModeSelectorToleranceTime);
IF mFbOpModeChgToleranceTmr.Q THEN
(* tolerance time elapsed, causes invalid operation mode now *)
CurrentOperationMode := eRCE_OperationModeInvalid;
END_IF;
END_IF;
END_IF;
(*---------------------------------------------------------------------------
* update current selected valid operation mode *)
ModeManualActive := (CurrentOperationMode = eRCE_OperationModeManualT1);
ModeAutomaticActive := (CurrentOperationMode = eRCE_OperationModeAutomatic);
ModeAutomaticExternActive := (CurrentOperationMode = eRCE_OperationModeAutomaticExtern);
(*---------------------------------------------------------------------------
* update robot release options, depending on selected operation mode *)
IF (CurrentOperationMode = eRCE_OperationModeManualT1) THEN
(* manual mode reduced speed (T1) *)
OperationReleased := (Safety_EnableSwitchState);
EStopReleased := (Safety_EStopReleased);
SafetyDoorFree := TRUE;
EnableSwitchFree := (Safety_EnableSwitchState);
PowerReleased := (Safety_PowerReleased);
JoggingEnabled := TRUE; (* Jogging enabled *)
ProgamExecutionEnabled := Settings.KAIROProgExecutionAllowedInManualMode; (* Execute KAIRO programs enabled *)
FullSpeedEnabled := FALSE; (* robot can only move with reduced speed *)
HoldToRunDisabled := FALSE; (* keep "Start" button pressed for running the program - release of "Start" button interrupts the program *)
ELSIF (CurrentOperationMode = eRCE_OperationModeManualFastT2) THEN
(* manual mode fast (T2) - not used yet *)
OperationReleased := (Safety_EnableSwitchState);
EStopReleased := (Safety_EStopReleased);
SafetyDoorFree := TRUE;
EnableSwitchFree := (Safety_EnableSwitchState);
PowerReleased := (Safety_PowerReleased);
JoggingEnabled := TRUE; (* Jogging enabled *)
ProgamExecutionEnabled := TRUE; (* Execute KAIRO programs enabled *)
FullSpeedEnabled := TRUE; (* robot can move with full speed *)
HoldToRunDisabled := FALSE; (* "Start" button need not to be held *)
ELSIF (CurrentOperationMode = eRCE_OperationModeAutomatic) THEN
(* automatic mode *)
OperationReleased := TRUE;
EStopReleased := (Safety_EStopReleased);
SafetyDoorFree := (Safety_SafetyDoorClosed);
EnableSwitchFree := TRUE;
PowerReleased := (Safety_PowerReleased);
JoggingEnabled := Settings.JoggingAllowedInAutoMode; (* Jogging enabled *)
ProgamExecutionEnabled := TRUE; (* Load and execute KAIRO programs enabled *)
FullSpeedEnabled := TRUE; (* robot can move with full speed *)
HoldToRunDisabled := TRUE; (* "Start" button need not to be held *)
ELSIF (CurrentOperationMode = eRCE_OperationModeAutomaticExtern) THEN
(* automatic extern mode *)
OperationReleased := TRUE;
EStopReleased := (Safety_EStopReleased);
SafetyDoorFree := (Safety_SafetyDoorClosed);
EnableSwitchFree := TRUE;
PowerReleased := (Safety_PowerReleased);
JoggingEnabled := TRUE; (* Jogging enabled *)
ProgamExecutionEnabled := TRUE; (* Load and execute KAIRO programs enabled *)
FullSpeedEnabled := TRUE; (* robot can move with full speed *)
HoldToRunDisabled := TRUE; (* "Start" button need not to be held *)
ELSE
(* invalid or unkown main mode *)
OperationReleased := FALSE;
EStopReleased := FALSE;
SafetyDoorFree := FALSE;
EnableSwitchFree := FALSE;;
PowerReleased := FALSE;
JoggingEnabled := FALSE;
ProgamExecutionEnabled := FALSE;
FullSpeedEnabled := FALSE;
HoldToRunDisabled := FALSE;
END_IF;
(* consider general 'application robot movement enable' *)
IF (NOT RobotOperationEnable) THEN
OperationReleased := FALSE;
END_IF;
(* considter operation release for stop mode *)
IF (OperationReleased) THEN
UsedStopMode := Settings.StopModeNormal;
ELSE
UsedStopMode := Settings.StopModeMissingOperationRelease;
END_IF;
(*---------------------------------------------------------------------------
* When in manual mode (T1 and T2) ->
* -> consider enable switch for automatic power on active robot *)
IF (CurrentOperationMode = eRCE_OperationModeManualT1) OR
(CurrentOperationMode = eRCE_OperationModeManualFastT2) OR
(mbPowerOnOffState <> 0) THEN
(* readout state of used active robot *)
mFbGroupReadState(Enable := TRUE, AxisGroup := ActiveGroup);
(* check option is activated *)
IF (Settings.AutoRobotPowerInManualMode.0) THEN
(* check rising edge of enable switch - new edge is mandatory when changing to manual mode *)
IF (EStopReleased AND
NOT mbOldEnableSwitchState AND Safety_EnableSwitchState) THEN
(* switch on robot - edge of enable switch triggered *)
IF ((NOT mFbGroupReadState.GroupPower) OR (mbPowerOnOffState = 0)) THEN
(* only switch on in robot power is off *)
mbPowerOnOffState := 10; (* 10 .. start power on pending *)
END_IF;
END_IF;
END_IF;
(* check unpressed of enable switch and check also if setting is activated *)
IF (NOT Safety_EnableSwitchState AND Settings.AutoRobotPowerInManualMode.1) THEN
(* check switching off power of robot *)
IF (mbPowerOnOffState > 0) THEN
(* switch off robot - level triggered - only when robot was enabled *)
mbPowerOnOffState := 40; (* 40 .. power off *)
END_IF;
END_IF;
(* handle change of robot power state *)
CASE (mbPowerOnOffState) OF
10: (* Power on pending - wait on drives ready signal and wait time *)
mFbTrigPowOnDelayTmr(IN := Drives_PowerOnReady, PT := Settings.DelayTimeOnEnableSwitchAutoPowerOn);
(* check delay time elapsed OR no delay used *)
IF (mFbTrigPowOnDelayTmr.Q OR (Settings.DelayTimeOnEnableSwitchAutoPowerOn <= T#0s)) THEN
mbPowerOnOffState := 20;
mFbTrigPowOnDelayTmr(IN := FALSE);
END_IF;
20: (* power on in progress *)
mFbRobotPower(Enable := TRUE, AxisGroup := ActiveGroup);
IF (mFbRobotPower.Status OR mFbRobotPower.Error) THEN
mbPowerOnOffState := 30; (* power on finished *)
IF (mFbRobotPower.Error) THEN
TestOut(CONCAT_TS('ERROR during switching on robot: ', ActiveGroup.name));
END_IF;
END_IF;
40: (* power off active robot - immediately *)
mFbTrigPowOnDelayTmr(IN := FALSE);
mFbRobotPower(Enable := FALSE, AxisGroup := ActiveGroup);
IF (NOT mFbRobotPower.Status OR mFbRobotPower.Error) THEN
mbPowerOnOffState := 0; (* power off finished *)
IF (mFbRobotPower.Error) THEN
TestOut(CONCAT_TS('ERROR during switching off robot: ', ActiveGroup.name));
END_IF;
END_IF;
ELSE
(* nothing to do *)
mFbTrigPowOnDelayTmr(IN := FALSE);
END_CASE;
ELSE
(* not in a manual mode *)
mbPowerOnOffState := 0;
mFbTrigPowOnDelayTmr(IN := FALSE);
END_IF;
(*---------------------------------------------------------------------------
* On each change of operation mode ->
* -> change also the icon on the teachpendant (do for all robots the same) *)
IF (miLastOpMode <> CurrentOperationMode) THEN
(* whenever operation mode gets changed, reset FB with Execute = FALSE first *)
mFbSetRobotOpModeIcon(Execute := FALSE, AxisGroup := ALLROBOTS);
mFbSetAxisOpModeIcon(Execute := FALSE, SingleAxis := ALLAXES);
(* start update operation mode icon with new current operation mode *)
mFbSetRobotOpModeIcon(Execute := TRUE, OpModeIndex := CurrentOperationMode, AxisGroup := ALLROBOTS);
mFbSetAxisOpModeIcon(Execute := TRUE, OpModeIndex := CurrentOperationMode, SingleAxis := ALLAXES);
(* check automatic control authority behaviour, when switching from/to automatic extern mode *)
IF (CurrentOperationMode = eRCE_OperationModeAutomaticExtern) AND
(miLastOpMode <> eRCE_OperationModeAutomaticExtern) THEN
(* switch to automatic extern *)
IF (Settings.AutoControlAuthorityMode.0) THEN
(* automatic control authority to PLC *)
mbGiveControlAuthToPLC := TRUE;
mReleaseControlAuthFromPLC := FALSE;
END_IF;
ELSIF (CurrentOperationMode <> eRCE_OperationModeAutomaticExtern) AND
(miLastOpMode = eRCE_OperationModeAutomaticExtern) THEN
(* switch from automatic extern mode to another operating mode *)
IF (Settings.AutoControlAuthorityMode.1) THEN
(* automatic release control authority from plc *)
mReleaseControlAuthFromPLC := TRUE;
mbGiveControlAuthToPLC := FALSE;
END_IF;
END_IF;
ELSIF mFbSetRobotOpModeIcon.Execute OR mFbSetAxisOpModeIcon.Execute THEN
(* FB for update operation mode icon still in progress
-> contine polling until FB finished *)
mFbSetRobotOpModeIcon(Execute := TRUE, AxisGroup := ALLROBOTS);
mFbSetAxisOpModeIcon(Execute := TRUE, SingleAxis := ALLAXES);
IF (mFbSetRobotOpModeIcon.Done OR mFbSetRobotOpModeIcon.Error) OR (mFbSetAxisOpModeIcon.Done OR mFbSetAxisOpModeIcon.Error) THEN
(* reset catch flag for continue polling
-> FB has finished or has an error *)
IF mFbSetRobotOpModeIcon.Error THEN
Error := TRUE;
ErrorID := mFbSetRobotOpModeIcon.ErrorID;
ELSIF mFbSetAxisOpModeIcon.Error THEN
Error := TRUE;
ErrorID := mFbSetAxisOpModeIcon.ErrorID;
END_IF;
mFbSetRobotOpModeIcon(Execute := FALSE, AxisGroup := ALLROBOTS);
mFbSetAxisOpModeIcon(Execute := FALSE, SingleAxis := ALLAXES);
END_IF;
ELSE
(* no change, reset FB for settings OpMode icon *)
mFbSetRobotOpModeIcon(Execute := FALSE, AxisGroup := ALLROBOTS);
mFbSetAxisOpModeIcon(Execute := FALSE, SingleAxis := ALLAXES);
END_IF;
(*---------------------------------------------------------------------------
* Store old values for edge or has changed detection of operation mode manager *)
miLastOpMode := CurrentOperationMode;
mbOldEnableSwitchState := Safety_EnableSwitchState;
(*---------------------------------------------------------------------------
* Automatic Control Authority handling depending on operation mode *)
CASE miStep OF
0: (* first start - read authority state *)
mFbReadControlAuthority(Execute := TRUE);
IF mFbReadControlAuthority.Done THEN
(* update value if PLC has control authority *)
ControlAuthorityAtPLC := mFbReadControlAuthority.AuthorityGranted;
miStep := 2;
ELSIF mFbReadControlAuthority.Error THEN
TestOut('ERROR on reading control authority state!');
miStep := 999; (* error state *)
END_IF;
2: (* check authority changed *)
mFbReadControlAuthority(Execute := FALSE);
mFbRequestAuthority(Execute := FALSE, RequestAuthority := FALSE, ForceRequest := FALSE);
(* check control authority change *)
IF mReleaseControlAuthFromPLC THEN
(* Release control authority from PLC *)
miStep := 20; (* Release control authority *)
mReleaseControlAuthFromPLC := FALSE;
ELSIF mbGiveControlAuthToPLC THEN
(* PLC should have control authority *)
miStep := 10; (* give control authority to PLC *)
mbGiveControlAuthToPLC := FALSE;
ELSE
(* readout state again, continue polling *)
miStep := 0;
END_IF;
10: (* get control authority *)
mFbRequestAuthority(Execute := TRUE, RequestAuthority := TRUE, ForceRequest := TRUE);
IF mFbRequestAuthority.Done THEN
IF mFbRequestAuthority.Status THEN
(* PLC have control authority -> ok *)
ControlAuthorityAtPLC := TRUE;
miStep := 0;
ELSE (* control authority not at plc -> error *)
TestOut('ERROR on requesting control authority in plc, state not given!');
miStep := 999; (* error state *)
END_IF;
ELSIF mFbRequestAuthority.Error THEN
TestOut('ERROR on requesting control authority state TRUE!');
miStep := 999; (* error state *)
END_IF;
20: (* release control authority from PLC *)
mFbRequestAuthority(Execute := TRUE, RequestAuthority := FALSE);
IF mFbRequestAuthority.Done THEN
IF NOT mFbRequestAuthority.Status THEN
(* PLC released control authority -> ok *)
ControlAuthorityAtPLC := FALSE;
miStep := 0;
ELSE (* control authority still at plc -> error *)
TestOut('ERROR on releasing control authority in PLC, state still given!');
miStep := 999; (* error state *)
END_IF;
ELSIF mFbRequestAuthority.Error THEN
TestOut('ERROR on releasing control authority from PLC!');
miStep := 999; (* error state *)
END_IF;
900: (* do nothing -> ERROR state *)
miStep := 0; (* try reset *)
mFbReadControlAuthority(Execute := FALSE);
mFbRequestAuthority(Execute := FALSE);
ELSE (* error state *)
TestOut('ERROR in control authority administration!');
miStep := 900;
END_CASE;
2240
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
