0 THEN _cooperatingWinchRotating := _controlUnitInControl.CooperatingWinchIsRotating; _potmeterReference := _controlUnitInControl.PotmeterReference; _joystickInCentre := _controlUnitInControl.JoystickInCentre; _activateWinch := _controlUnitInControl.ActivateWinch; ELSE _cooperatingWinchRotating := 0; _potmeterReference := 1; _joystickInCentre := FALSE; _activateWinch := FALSE; END_IF man_timer(IN := _State = p7type.UserWinchStates.MANUAL ,PT:=T#50MS); // Handle timeout of winch _activeAndNotRotating := ((_State = p7type.UserWinchStates.MANUAL_ACTIVE OR _State = p7type.UserWinchStates.MANUAL_ACTIVE_REDUCED) AND NOT _winch.IsRotating); _noVelocityReference := (_velocityReference < 0.01 AND _velocityReference > -0.01); _timeoutActive(IN := (NOT _activateWinch AND _activeAndNotRotating AND _noVelocityReference AND NOT _preventTimeOut AND NOT _cooperatingWinchRotating AND (_State<>p7type.UserWinchStates.AUTO)), PT := configUserWinch.activeTimeOut); CASE _State OF p7type.UserWinchStates.INACTIVE: _opstate := p6com.P6OpState.STOPPED; _winchMode := p7type.WinchModes.IDLE; _newControlUnitAssigned := FALSE; _winchCMD := WinchCMD.STOP; _velocityReference := 0; _torqueReference := 1; _winch.setOperation(_winchCMD, _velocityReference, _torqueReference, 0, 0, 0 , 0); IF _controlUnitInControl <> 0 THEN _controlUnitInControl.StopWinch := FALSE; IF _activateWinch AND _winch.isReadyToActivate THEN CASE _winchManualMode OF WinchManualModes.NORMAL, WinchManualModes.MANUAL_TORQUE: _State := p7type.UserWinchStates.MANUAL; _opstate := p6com.P6OpState.PREPARE; WinchManualModes.CONSTANT_TORQUE: _State := p7type.UserWinchStates.CONST_TORQUE; _opstate := p6com.P6OpState.PREPARE; END_CASE END_IF END_IF p7type.UserWinchStates.MANUAL: _winchMode := p7type.WinchModes.MANUAL; _opstate := p6com.P6OpState.PREPARE; _velocityReference := 0; _torqueReference := 1; _winchCMD := WinchCMD.MANUAL; _winch.setOperation(_winchCMD, _velocityReference, _torqueReference, 0, 0, 0 , 0); IF man_timer.Q THEN _State := p7type.UserWinchStates.MANUAL_PREPARE; END_IF p7type.UserWinchStates.MANUAL_PREPARE: _winchMode := p7type.WinchModes.MANUAL; _opstate := p6com.P6OpState.PREPARE; IF winchDriverStopped() THEN _State := p7type.UserWinchStates.INACTIVE; ELSE _State := p7type.UserWinchStates.MANUAL_PREOP; END_IF p7type.UserWinchStates.MANUAL_PREOP: _winchMode := p7type.WinchModes.MANUAL; _opstate := p6com.P6OpState.PREPARE; _winchCMD := WinchCMD.MANUAL; _velocityReference := 0; _torqueReference := 1; _winch.setOperation(_winchCMD, _velocityReference, _torqueReference, 0, 0, 0 , 0); IF winchDriverStopped() THEN _State := p7type.UserWinchStates.INACTIVE; // Something wrong with inverters, user needs to reset inverters from HMI. ELSIF _winch.OpState = p6com.P6OpState.RUN THEN _State := p7type.UserWinchStates.MANUAL_ACTIVE; END_IF p7type.UserWinchStates.MANUAL_ACTIVE: _winchMode := p7type.WinchModes.MANUAL; _opstate := p6com.P6OpState.RUN; IF _coolingNoOPTimer.Q THEN _State := p7type.UserWinchStates.InActive; RETURN; ELSIF _controlUnitInControl = 0 THEN _State := p7type.UserWinchStates.InActive; RETURN; END_IF IF _controlUnitInControl.TurtleMode THEN _velocityReference := _controlUnitInControl.VelocityReference/configUserWinch.turtleModeSpeedDivident; // When in turtle mode speed is reduced by a factor of 5 ELSE _velocityReference := _controlUnitInControl.VelocityReference; END_IF IF (_blockStop OR IO_in.blockStopInput ) AND _velocityReference < 0 THEN // block Stop only in haul in. _velocityReference := 0; END_IF // normal operation _winchCMD := WinchCMD.MANUAL; CASE _winchManualMode OF WinchManualModes.NORMAL: _velocityReference := _velocityReference; _torqueReference := 1.0; WinchManualModes.MANUAL_TORQUE: _winchCMD := WinchCMD.MANUAL; _velocityReference := _velocityReference; _torqueReference := _potmeterReference; END_CASE IF _requestAutoMode THEN _State := p7type.UserWinchStates.AUTO; END_IF IF _inhibitManualRotation THEN // don't let winch rotate (external command from SDR controller) _velocityReference := 0; _torqueReference := 1; END_IF _winch.setOperation(CMD := _winchCMD, Setpoint := _velocityReference, parameter1 := _torqueReference, parameter2 := 0, parameter3 := 0, parameter4 := 0, AHC := 0); IF _timeoutActive.Q OR (NOT Enable) OR _newControlUnitAssigned OR winchDriverStopped() OR _controlUnitInControl.StopWinch THEN _State := p7type.UserWinchStates.INACTIVE; ELSIF _winchManualMode = WinchManualModes.CONSTANT_TORQUE THEN _State := p7type.UserWinchStates.CONST_TORQUE; ELSIF _requestAutoMode THEN _State := p7type.UserWinchStates.AUTO; ELSIF THIS^._reducedSpeedAndTension THEN _State := p7type.UserWinchStates.MANUAL_ACTIVE_REDUCED; END_IF // This mode forces winch to move slow and with reduced tension. Ex. when clutch in/out. p7type.UserWinchStates.MANUAL_ACTIVE_REDUCED: _winchMode := p7type.WinchModes.MANUAL; _opstate := p6com.P6OpState.RUN; IF _coolingNoOPTimer.Q THEN _State := p7type.UserWinchStates.InActive; RETURN; ELSIF _controlUnitInControl = 0 THEN _State := p7type.UserWinchStates.InActive; RETURN; END_IF _velocityReference := _controlUnitInControl.VelocityReference/configUserWinch.turtleModeSpeedDivident; // When in turtle mode speed is reduced by a factor of 5 _winchCMD := WinchCMD.MANUAL; _velocityReference := _velocityReference; _torqueReference := configUserWinch.reducedTensionLevel; IF _requestAutoMode THEN _State := p7type.UserWinchStates.AUTO; END_IF _winch.setOperation(CMD := _winchCMD, Setpoint := _velocityReference, parameter1 := _torqueReference, parameter2 := 0, parameter3 := 0, parameter4 := 0, AHC := 0); IF _timeoutActive.Q OR (NOT Enable) OR _newControlUnitAssigned OR winchDriverStopped() THEN _State := p7type.UserWinchStates.INACTIVE; ELSIF _requestAutoMode THEN _State := p7type.UserWinchStates.AUTO; // If criteria for reducing speed and tension is lost go back to normal mode. Only allowed when winch is not moving. // This makes sure that winch doesnt start to move fast suddenly. ELSIF NOT THIS^._reducedSpeedAndTension AND NOT _winch.IsRotating THEN _State := p7type.UserWinchStates.MANUAL_ACTIVE; END_IF p7type.UserWinchStates.CONST_TORQUE: _winchMode := p7type.WinchModes.TORQUE; _opstate := p6com.P6OpState.PREPARE; CT_initSpeed := _velocityReference; // store current speed. _velocityReference := 0; // maybe skip this? _torqueReference := 1; _winchCMD := WinchCMD.TORQUE; _winch.setOperation(_winchCMD, _torqueReference, _velocityReference, 0, 0, 0 , 0); _State := p7type.UserWinchStates.CONST_TORQUE_PREPARE; p7type.UserWinchStates.CONST_TORQUE_PREPARE: _winchMode := p7type.WinchModes.TORQUE; _opstate := p6com.P6OpState.PREPARE; IF winchDriverStopped() THEN _State := p7type.UserWinchStates.INACTIVE; ELSE _State := p7type.UserWinchStates.CONST_TORQUE_PREOP; END_IF p7type.UserWinchStates.CONST_TORQUE_PREOP: _winchMode := p7type.WinchModes.TORQUE; _opstate := p6com.P6OpState.PREPARE; _winchCMD := WinchCMD.TORQUE; _velocityReference := 0; // skip this too? _torqueReference := 1; _winch.setOperation(_winchCMD, _torqueReference, _velocityReference, 0, 0, 0 , 0); IF winchDriverStopped() THEN _State := p7type.UserWinchStates.INACTIVE; // Something wrong with inverters, user needs to reset inverters from HMI. ELSIF _winch.OpState = p6com.P6OpState.RUN THEN _State := p7type.UserWinchStates.CONST_TORQUE_ACTIVE; CT_rampUpInit := TRUE; // END_IF p7type.UserWinchStates.CONST_TORQUE_ACTIVE: CT_rampdownInit := TRUE; // prepare for rampdown init _winchMode := p7type.WinchModes.TORQUE; _opstate := p6com.P6OpState.RUN; IF _coolingNoOPTimer.Q THEN _State := p7type.UserWinchStates.InActive; RETURN; ELSIF _controlUnitInControl = 0 THEN _State := p7type.UserWinchStates.InActive; RETURN; END_IF // If user has enabled use of safety distance // the winch will get 0 RPM in speed reference if within safety range. // This is used by net sounder winch. IF (configUserWinch.optionSafetyDistanceEnabled AND (_winch.Length < configUserWinch.safetyDistance)) OR (_blockStop OR IO_in.blockStopInput ) THEN _velocityReference := 0; // Velocity ELSE _velocityReference := configUserWinch.speedConstantTorque; // Velocity END_IF IF configUserWinch.optionConstTorqueFromConfig THEN // use reduced level if docking head is half compressed _torqueReference := SEL((_reduceConstantTorque OR IO_In.reduceConstantTorqueInput),configUserWinch.constTorqueRel , configUserWinch.constTorqueRelReduced); ELSE // use potmeter signal _torqueReference := _potmeterReference; END_IF // *************** SuperCT start **************************** IF configUserWinch.CT_config.PIDactive AND CT.isSensorsOK THEN IF CT_rampUpInit THEN CT_rampedLimitpay := MIN(ABS(CT_initSpeed),2); CT_rampedLimitHaul := MIN(ABS(CT_initSpeed),2); CT_rampUpInit := FALSE; END_IF // ******ramp up/down ******** CT_rampChangePerScan := configUserWinch.CT_config.SpeedRampRate / 100.0 ; // 100 scans per second IF CT_rampedLimitpay < configUserWinch.CT_config.TorqueModePIDspeedLimitPay THEN CT_rampedLimitpay := CT_rampedLimitpay + CT_rampChangePerScan; IF CT_rampedLimitpay > configUserWinch.CT_config.TorqueModePIDspeedLimitPay THEN CT_rampedLimitpay := configUserWinch.CT_config.TorqueModePIDspeedLimitPay; END_IF; ELSIF CT_rampedLimitpay > configUserWinch.CT_config.TorqueModePIDspeedLimitPay THEN CT_rampedLimitpay := CT_rampedLimitpay - CT_rampChangePerScan; IF CT_rampedLimitpay < configUserWinch.CT_config.TorqueModePIDspeedLimitPay THEN CT_rampedLimitpay := configUserWinch.CT_config.TorqueModePIDspeedLimitPay; END_IF; END_IF; IF CT_rampedLimitHaul < configUserWinch.CT_config.TorqueModePIDspeedLimitHaul THEN CT_rampedLimitHaul := CT_rampedLimitHaul + CT_rampChangePerScan; IF CT_rampedLimitHaul > configUserWinch.CT_config.TorqueModePIDspeedLimitHaul THEN CT_rampedLimitHaul := configUserWinch.CT_config.TorqueModePIDspeedLimitHaul; END_IF; ELSIF CT_rampedLimitHaul > configUserWinch.CT_config.TorqueModePIDspeedLimitHaul THEN CT_rampedLimitHaul := CT_rampedLimitHaul - CT_rampChangePerScan; IF CT_rampedLimitHaul < configUserWinch.CT_config.TorqueModePIDspeedLimitHaul THEN CT_rampedLimitHaul := configUserWinch.CT_config.TorqueModePIDspeedLimitHaul; END_IF; END_IF; // UDP logging: CT.processData.speedRampPay := CT_rampedLimitpay; CT.processData.speedRampHaul := CT_rampedLimitHaul; IF configUserWinch.CT_config.selectedPIDtype = 3 THEN // automatic tension step configUserWinch.CT_config.TargetTension_kg := SEL(CT.s_20_toggle, configUserWinch.CT_config.simTensionLowValue,configUserWinch.CT_config.simTensionHighValue); END_IF // Torque PID (default mode) IF configUserWinch.CT_config.selectedPIDtype = 1 OR configUserWinch.CT_config.selectedPIDtype = 3 THEN IF CT.isIndoors THEN CT.torquePID.SetCmdLimits( minimum := LIMIT (configUserWinch.CT_config.indoorMinTorqueLimit_Nm , configUserWinch.CT_config.kN_Speed_limit_factor *( CT.processData.TractionWinchActualRPM - CT_rampedLimitpay), configUserWinch.CT_config.indoorMaxTorqueLimit_Nm), maximum := LIMIT (configUserWinch.CT_config.indoorMinTorqueLimit_Nm , configUserWinch.CT_config.kN_Speed_limit_factor *( CT.processData.TractionWinchActualRPM + CT_rampedLimitHaul), configUserWinch.CT_config.indoorMaxTorqueLimit_Nm)); ELSE // outside CT.torquePID.SetCmdLimits( minimum := LIMIT (configUserWinch.CT_config.outdoorMinTorqueLimit_Nm , configUserWinch.CT_config.kN_Speed_limit_factor *( CT.processData.TractionWinchActualRPM - CT_rampedLimitpay), configUserWinch.CT_config.outdoorMaxTorqueLimit_Nm), maximum := LIMIT (configUserWinch.CT_config.outdoorMinTorqueLimit_Nm , configUserWinch.CT_config.kN_Speed_limit_factor *( CT.processData.TractionWinchActualRPM + CT_rampedLimitHaul), configUserWinch.CT_config.outdoorMaxTorqueLimit_Nm)); END_IF CT.torquePID.SetCoeffs( loopTime := 0.01, Kp := configUserWinch.CT_PIDconfig.Kp, Ki := configUserWinch.CT_PIDconfig.Ki, Kd := configUserWinch.CT_PIDconfig.Kd, Tf := configUserWinch.CT_PIDconfig.Tf); // To take away some of the proportional error, we're subtracting the minimum backtension setting from the target tension. Backtension will usually be higher than this, but the regulator takes care of this. ff_torque_Nm := (configUserWinch.CT_config.TargetTension_kg - gvlStorageDrum01.SDR_controller.config.minimumBackTension) * 9.81 * (gvlTractionWinch01.eWinch.Config.CoreDiameter/2) / (gvlTractionWinch01.Drive.getGearRatio()* 2 ); CT.torquePID.SetFeedForward(FF := ff_torque_Nm ); // Nm CT.torqueCommand := CT.torquePID.CommandSignal(referenceInput := configUserWinch.CT_config.TargetTension_kg , measurement := CT.processData.LoadcellTension); _velocityReference := configUserWinch.CT_config.VFD_drum_RPM_limit; // positive torque hauls in. (we invert this later in the process) _torqueReference := (CT.torqueCommand / configUserWinch.CT_config.motorNominalTorque_Nm ) ; // divide by motor nominal torque Nm to get normalized value END_IF // Direct torque step simulation IF configUserWinch.CT_config.selectedPIDtype = 2 THEN _velocityReference := -1.0; // SLOW TESTING _torqueReference := SEL(CT.s_10_toggle, configUserWinch.CT_config.simTorqueLowValue,configUserWinch.CT_config.simTorqueHighValue); END_IF END_IF // *************** SuperCT end **************************** _winch.setOperation( CMD := _winchCMD, Setpoint := _torqueReference, parameter1 := _velocityReference, parameter2 := 0, parameter3 := 0, parameter4 := 0, AHC := 0); IF _timeoutActive.Q OR (NOT Enable) OR _newControlUnitAssigned OR winchDriverStopped() OR _controlUnitInControl.StopWinch THEN _State := p7type.UserWinchStates.INACTIVE; ELSIF (_winchManualMode = WinchManualModes.NORMAL) OR (_winchManualMode = WinchManualModes.MANUAL_TORQUE) THEN _State := p7type.UserWinchStates.CONST_TORQUE_DEACTIVATE ; // Cool down speed before returning to normal operation ELSIF _requestAutoMode THEN _State := p7type.UserWinchStates.AUTO; END_IF p7type.UserWinchStates.CONST_TORQUE_DEACTIVATE: IF configUserWinch.CT_config.PIDactive AND CT.isSensorsOK THEN IF CT_rampdownInit THEN CT_rampdownInit := FALSE; // (Motor RPM * 8 / 20000) gives time in seconds, multiply by 1000 to get ms CT_rampdownTime := LREAL_TO_TIME(( ABS(configUserWinch.CT_config.VFD_drum_RPM_limit) * driveGearRatio * (* VFD ramp time *) 8 * 1000 )/ (* VFD Speed scaling *) 20000 ) ; END_IF ELSE CT_rampdownTime := T#1S; END_IF constTorqueDeactivateTimer(IN := TRUE, PT := CT_rampdownTime); _winchMode := p7type.WinchModes.TORQUE; _opstate := p6com.P6OpState.RUN; _winchCMD := WinchCMD.TORQUE; IF _controlUnitInControl = 0 THEN _State := p7type.UserWinchStates.InActive; RETURN; END_IF IF configUserWinch.CT_config.PIDactive AND CT.isSensorsOK THEN // We came from CT PID.... _torqueReference := ff_torque_Nm / configUserWinch.CT_config.motorNominalTorque_Nm ; // CHECK! ELSIF configUserWinch.optionConstTorqueFromConfig THEN // use reduced level if docking head is half compressed _torqueReference := SEL((_reduceConstantTorque OR IO_In.reduceConstantTorqueInput),configUserWinch.constTorqueRel , configUserWinch.constTorqueRelReduced); ELSE _torqueReference := _potmeterReference; END_IF _velocityReference := 0; _winch.setOperation( CMD := _winchCMD, Setpoint := _torqueReference, parameter1 := _velocityReference, parameter2 := 0, parameter3 := 0, parameter4 := 0, AHC := 0); IF constTorqueDeactivateTimer.Q THEN // turns on after the specified time constTorqueDeactivateTimer(IN := FALSE); _State := p7type.UserWinchStates.MANUAL_PREPARE; END_IF // Auto states: velocity and torque references received from TrawlControl via TrawlWinch.SetAutoOperation(...); p7type.UserWinchStates.AUTO: // if this is the state we always go into, should it go directly to AUTO_ACTIVE _winchMode := p7type.WinchModes.AUTO; _opstate := p6com.P6OpState.PREPARE; _velocityReference := 0; // was _velocityReferenceAuto; _torqueReference := _torqueReferenceAuto; _State := p7type.UserWinchStates.AUTO_PREPARE; // was AUTO_ACTIVE p7type.UserWinchStates.AUTO_PREPARE: _winchMode := p7type.WinchModes.AUTO; _opstate := p6com.P6OpState.PREPARE; IF winchDriverStopped() THEN _State := p7type.UserWinchStates.MANUAL_ACTIVE; ELSE _State := p7type.UserWinchStates.AUTO_PREOP; END_IF p7type.UserWinchStates.AUTO_PREOP: _winchMode := p7type.WinchModes.AUTO; _opstate := p6com.P6OpState.PREPARE; _velocityReference := 0 ; //_velocityReferenceAuto; _torqueReference := _torqueReferenceAuto; _winchCMD := WinchCMD.SLAVE; _winch.setOperation(_winchCMD, _velocityReference, _torqueReference, 0, 0, 0 , 0); IF winchDriverStopped() THEN _State := p7type.UserWinchStates.INACTIVE; // Something wrong with inverters, user needs to reset inverters from HMI. ELSIF _winch.OpState = p6com.P6OpState.RUN THEN _State := p7type.UserWinchStates.AUTO_ACTIVE; END_IF p7type.UserWinchStates.AUTO_ACTIVE: _winchMode := p7type.WinchModes.AUTO; _opstate := p6com.P6OpState.RUN; _velocityReference := _velocityReferenceAuto; _torqueReference := _torqueReferenceAuto; _winchCMD := WinchCMD.SLAVE; _winchManualMode := WinchManualModes.NORMAL; // make sure we're not in any costant or manual torque moed. _winch.setOperation(_winchCMD, _velocityReference, _torqueReference, 0, 0, 0 , 0); IF _coolingNoOPTimer.Q THEN _State := p7type.UserWinchStates.InActive; RETURN; ELSIF winch.State <> p7type.P6State.OP THEN _State := p7type.UserWinchStates.INACTIVE; ELSIF NOT _requestAutoMode THEN _State := p7type.UserWinchStates.MANUAL_ACTIVE; _winchCMD := WinchCMD.MANUAL; _winch.setOperation(_winchCMD, _velocityReference, _torqueReference, 0, 0, 0 , 0); END_IF p7type.UserWinchStates.EMERGENCYSTOP: _winchMode := p7type.WinchModes.ESTOP; _opstate := p6com.P6OpState.NOTAVAILABLE; _winchCMD := WinchCMD.FASTSTOP; _velocityReference := 0; _torqueReference := 1; _winch.setOperation(_winchCMD, _velocityReference, _torqueReference, 0, 0, 0 , 0); IF _emergencyStop_Falling.Q THEN _winchCMD := WinchCMD.FASTSTOP_OFF; _velocityReference := 0; _torqueReference := 1; _winch.setOperation(_winchCMD, _velocityReference, _torqueReference, 0, 0, 0 , 0); // Exit emergency stop when clear. Enter inactive state to make sure winch needs to be activated to move again. _State := p7type.UserWinchStates.INACTIVE; END_IF END_CASE ]]> 0 THEN winchDriverStopped := _winch.OpState = p6com.P6OpState.STOPPED OR _winch.OpState = p6com.P6OpState.NOTAVAILABLE ; ELSE winchDriverStopped := FALSE; END_IF]]> 0 THEN getVelocityReference := _controlUnitInControl.VelocityReference; ELSE getVelocityReference := 0.0; END_IF]]> 0 THEN //IF NOT THIS^._winch.IsRotating THEN THIS^._winchManualMode := THIS^._controlUnitInControl.RequestedWinchMode; //END_IF ELSE THIS^._winchManualMode := WinchManualModes.NORMAL; END_IF ]]> p7type.P6OpState.RUN; ]]> 0 THEN IF ControlUnitInControl = _controlUnitInControl THEN AssignControlUnitInControl := FALSE; RETURN; ELSIF (__QUERYINTERFACE(ControlUnitInControl, _controlUnitInControl)) THEN AssignControlUnitInControl := TRUE; _newControlUnitAssigned := TRUE; ELSE AssignControlUnitInControl := FALSE; END_IF ELSE _controlUnitInControl := 0; AssignControlUnitInControl := TRUE; END_IF ]]>