SM.IDLE THEN IF estop.Activated THEN _state := SM.ERROR; _errorMsg(st1 := 'TRW system error: E-stop activated',st2 := '',st3 := ''); ELSIF SWC.opState = p6opstate.NOTAVAILABLE THEN _state := SM.ERROR; _errorMsg(st1 := 'TRW system error: SWC not operational',st2 := '',st3 := ''); ELSIF TRW.opState = p6opstate.NOTAVAILABLE THEN _state := SM.ERROR; _errorMsg(st1 := 'TRW system error: Tractionwinch not operational',st2 := '',st3 := ''); ELSIF SDR.opState = p6opstate.NOTAVAILABLE THEN _state := SM.ERROR; _errorMsg(st1 := 'TRW system error: Storagedrum not operational',st2 := '',st3 := ''); ELSIF brakeHPU.Error THEN _state := SM.ERROR; _errorMsg(st1 := 'TRW system error: Brake HPU Error',st2 := '',st3 := ''); END_IF END_IF // always run code for storage Drum/CU _SDRstateController(); CASE _state OF SM.IDLE: _opstate := P6OpState.STOPPED; IF TRW.OpState = P6Opstate.RUN AND splitMode = FALSE THEN _state := SM.STARTING; (* EXIT STATE TRANSITION ACTIONS *) _SWCpositionSetpoint := 0.5; // center SWC.setBacktension(_SWCtensionSetpoint); SWC.activate(); SDR.TorqueAuto := 1 ; SDR.VelocityAuto := 0; SDR_Activate := TRUE;; TRW.InhibitManualRotation := TRUE; DebugMessage('TRW system starting...','',''); END_IF SM.STARTING: //wait for winch and SlacWire to get operational _opstate := P6OpState.PREPARE; IF SWC.RegulatorAtSetpoint = TRUE AND SDR.State = p7type.UserWinchStates.AUTO_ACTIVE THEN // SWC har riktig trykk og winch er i run/auto _state := SM.POSITIONING; END_IF IF _startingTimeout.Q THEN _state := SM.STOP; IF NOT SWC.RegulatorAtSetpoint THEN DebugMessage('TRW system: Timeout waiting for SWC','',''); END_IF IF NOT (SDR.State = p7type.UserWinchStates.AUTO_ACTIVE) THEN DebugMessage('TRW system: Timeout waiting for StorageDrum','',''); END_IF END_IF SM.POSITIONING : _opstate := P6OpState.PREPARE; IF SWC.InCenter (* AND SDR.IsRotating = FALSE *) THEN // wait until SWC is in center and winch not rotating _state := SM.RUN; (* EXIT STATE TRANSITION ACTIONS *) TRW.InhibitManualRotation := FALSE; DebugMessage('TRW system ready to run','',''); END_IF SWC.setBacktension(_SWCtensionSetpoint); SDR.TorqueAuto := config.SDRmaxTorqueInAuto ; SDR.VelocityAuto := LIMIT(-Config.SDR_parkingSpeed, _SDRspeedCommand, Config.SDR_parkingSpeed); IF _positioningTimeout.Q THEN _state := SM.STOP; DebugMessage('TRW system: Timeout waiting for SWC to reach center','',''); END_IF // check rotation while in center, and stop if wire is slipping over traction winch (no load on deck) SM.RUN : _opstate := P6OpState.RUN; IF TRW.OpState = P6Opstate.STOPPED THEN // normal exit _state := SM.PARKING; (* EXIT STATE TRANSITION ACTIONS *) TRW.InhibitManualRotation := TRUE; DebugMessage('TRW system stopped by TRW stop command','',''); ELSIF splitMode THEN // normal exit _state := SM.PARKING; (* EXIT STATE TRANSITION ACTIONS *) TRW.InhibitManualRotation := TRUE; DebugMessage('TRW system stopped due to split mode activated','',''); ELSE // normal operation or error SWC.setBacktension(_SWCtensionSetpoint); SDR.TorqueAuto := config.SDRmaxTorqueInAuto ; SDR.VelocityAuto := _SDRspeedCommand; // problems? IF NOT (SDR.OpState = P6OpState.RUN) THEN _State := SM.ERROR; DebugMessage('TRW error: Stop caused by Storage Drum not operational','',''); END_IF IF NOT (SDR.OpState = P6OpState.RUN) THEN _State := SM.ERROR; DebugMessage('TRW error: Stop caused by TRW not operational','',''); END_IF END_IF SM.PARKING : _opstate := P6OpState.PAUSING; _SWCpositionSetpoint := 0.0; // Fully retracted SDR.VelocityAuto := LIMIT(-Config.SDR_parkingSpeed, _SDRspeedCommand, Config.SDR_parkingSpeed); IF swc.endSensorInnerTriggered OR SWC.position < 0.03 THEN // normal state exit _state := SM.STOP; DebugMessage('TRW system parked','',''); END_IF IF _parkingTimeout.Q THEN _state := SM.STOP; DebugMessage('TRW system: Timeout waiting for SWC to park','',''); END_IF IF NOT (SDR.OpState = P6OpState.RUN) THEN _State := SM.ERROR; DebugMessage('TRW error: Stop caused by Storage Drum not operational','',''); END_IF IF NOT (SDR.OpState = P6OpState.RUN) THEN _State := SM.ERROR; DebugMessage('TRW error: Stop caused by TRW not operational','',''); END_IF // measure SDR length to prevent too much haul in - will detect slip on TRW SM.STOP : _opstate := P6OpState.STOPPED; SWC.deactivate(); SDR.TorqueAuto := 1 ; SDR.VelocityAuto := 0; TRW.InhibitManualRotation := FALSE; SDR_Deactivate := TRUE; // Stop SDR _state := SM.IDLE; SM.ERROR : _opstate := P6OpState.NOTAVAILABLE; // Stop operation and check for problems SWC.deactivate(); SDR.TorqueAuto := 1 ; SDR.VelocityAuto := 0; TRW.InhibitManualRotation := FALSE; SDR_Deactivate := TRUE; // Stop SDR IF estop.Activated THEN _state := SM.ESTOP; END_IF IF SWC.opState = p6opstate.NOTAVAILABLE (* SlackWireSM.ERROR *) OR TRW.OpState = p6opstate.NOTAVAILABLE OR brakeHPU.Error THEN ; // still error ELSE // all OK _state := SM.IDLE; END_IF SM.ESTOP : _opstate := P6OpState.NOTAVAILABLE; IF NOT estop.Activated THEN _state := SM.IDLE; END_IF END_CASE ]]> 5 THEN state := 7; DebugMessage('SDR Controller: Cannot activate Storagedrum in Auto','',''); END_IF IF NOT SDR_controlUnit.InControl AND NOT WinchInRun THEN // winch is off (monitoring) SDR_controlUnit.IO_In.actionButton := TRUE; state :=2; ELSIF SDR_controlUnit.InControl AND NOT WinchInRun THEN // already in control state := 3; ELSIF SDR_controlUnit.InControl AND WinchInRun THEN // already in control and in run state := 5; END_IF 2: // release button and wait for inControl SDR_controlUnit.IO_In.actionButton := FALSE; IF SDR_controlUnit.InControl THEN state :=3; END_IF IF timerwaitFoInControl.Q THEN state := 1; // start over again END_IF 3: // Activate winch SDR_controlUnit.IO_In.actionButton := TRUE; state :=4; 4: // wait for winch active SDR_controlUnit.IO_In.actionButton := FALSE; IF WinchInRun THEN state :=5; END_IF IF timerwaitForRun.Q THEN state := 1; // start over again END_IF 5: IF WinchInRun = FALSE THEN state := 1; // start over again END_IF IF timerOKinRun.Q THEN // RUN is ok for 1 second state := 6; END_IF 6: // set auto mode SDR_controlUnit.AutoMode := TRUE; SDR.AutoMode := TRUE; state := 0 ; // go to idle 7: ;// stop all SDR_controlUnit.AutoMode := FALSE; SDR.AutoMode := FALSE; SDR_controlUnit.StopWinch := TRUE; SDR_controlUnit.ReleaseControl(-1); state := 0 ; // go to idle END_CASE SDR_Activate := FALSE; SDR_Deactivate := FALSE;]]>