﻿<?xml version="1.0" encoding="utf-8"?>
<TcPlcObject Version="1.1.0.1">
  <POU Name="SlackWireCompensator" Id="{ee714931-f5cc-4487-9af1-fd970e856850}" SpecialFunc="None">
    <Declaration><![CDATA[FUNCTION_BLOCK SlackWireCompensator //IMPLEMENTS ISlackWire
VAR_INPUT
	Config								: REFERENCE TO SlackWireConfig;
	IO_In								: REFERENCE TO SWCIO_In;
	IO_Out								: REFERENCE TO SWCIO_Out;
	EmergencyStop						: p6com.IEmergencyStop;
	supplyPressure						: LREAL;	// input pressure from system
END_VAR
VAR
	_pressurePid						: PIDController;
	_opState							: p7type.P6OpState;
	_sensorError						: BOOL; 
	_state								: SlackWireSM;
	valve								: p7Hyd.ValveEL2535;	
	_pressureSensor						: p6com.HydPressure250barAA;
	_activate							: BOOL;		// start command from outside
	_stringPosition						: LREAL;	// 0-1
	_pressure							: LREAL;	// hydraulic pressure in Bar
	_pressureRequested					: LREAL;	// We are asked to achieve this (bar)...
	_pressureRegSV						: LREAL;	// SV to Regulator	
	_valveOutputValue					: LREAL;	// output to Valve (0-1)
	_tension							: LREAL;	// wire tension in Kg
	_tensionrRequested					: LREAL;	// Requested tension in Kg
	_activationFailTimer				: TON 	 := (PT := T#30S);		// if not in run after 30 sec, turn off
	_deactivateTimer					: TON 	 := (PT := T#4S);		// time to ramp down pressure when stopping
	_atSetpoint							: BOOL;			// Regulator within pressure limit and in run state
	
	
END_VAR
VAR CONSTANT
	MINIMUM_SUPPLY_PRESSURE 			: LREAL := 30;	
END_VAR

]]></Declaration>
    <Implementation>
      <ST><![CDATA[// The SWC is only responsible for maintaining its required pressure, not position.
// Should keep track of its own position and pressure
// The control is done with methods activate() and deactivate(), in combination with setBacktension(tension)

_stringPosition := _normalizeStringInput();
_pressureSensor(IO_In := IO_In.pressureSensor);		// run the sensor code
_pressure := _pressureSensor.Value;					// get the pressure
_pressureRequested := _calculateRequestedPressure(tension := _tensionrRequested );
_tension := _calculateBacktension(cylinderPressure := _pressure);
_checkSensorErrors();

// TIMERS
_activationFailTimer(IN:= _state = SlackWireSM.ACTIVATING AND _pressure < 30);	// Timeout for activation
_deactivateTimer(in := _state = SlackWireSM.DEACTIVATE); 						// timeout for deactivation

// STATE TRANSITIONS COMMON FOR ALL STATES
IF EmergencyStop.Activated THEN					// Estop...
	_state := SlackwireSM.ESTOP;
ELSIF _sensorError THEN		// Any sensor error		
	_state := SlackwireSM.ERROR;
END_IF

//  STATE MACHINE
CASE _state OF
	SlackwireSM.IDLE:
		IF _activate THEN _state := SlackWireSM.ACTIVATING;
		END_IF
		_opstate := P6OpState.STOPPED;				// default OpState
		
	SlackwireSM.ERROR,
	SlackwireSM.ESTOP:
		_opstate := P6OpState.NOTAVAILABLE;
		_activate := FALSE;
		IF NOT EmergencyStop.Activated AND NOT _sensorError THEN
			_state := SlackWireSM.IDLE;
		END_IF
		
	SlackwireSM.ACTIVATING:
		_opstate := P6OpState.PREPARE;
		IF supplyPressure > MINIMUM_SUPPLY_PRESSURE THEN _state := SlackWireSM.RUN;
		END_IF
		IF _activationFailTimer.Q THEN _state := SlackWireSM.IDLE; 
		END_IF
		IF NOT _activate THEN _state := SlackWireSM.DEACTIVATE;
		END_IF
		
	SlackwireSM.RUN:
		_opstate := P6OpState.RUN;
		IF NOT _activate THEN _state := SlackWireSM.DEACTIVATE;
		END_IF
		
	SlackwireSM.DEACTIVATE:
		_opstate := P6OpState.PAUSING;
		IF _deactivateTimer.Q OR _pressure < 20 THEN 			//hard-coded value :-(
			_state:=SlackWireSM.IDLE;
		END_IF
		IF _activate THEN
			_state := SlackWireSM.ACTIVATING;
		END_IF
		
END_CASE

IF _state = SlackWireSM.RUN THEN
	_pressureRegSV :=  MAX(0,MIN(_pressureRequested , supplyPressure - 3));	// SV never more than supply pressure -3 bar
ELSE _pressureRegSV :=0;
END_IF

// set the valve  
_pressurePid.update(
	config := config.pidConfig,
	Active := _activate,
	Setpoint := _pressureRegSV,
	ProcessValue := _pressure,
	ProcessValueFF1 :=0,
	ProcessValueFF2 :=0,
	LowerBoundary :=0,
	UpperBoundary := 1,
	CycleTime := 0.05	// PLC scan tima
);

// regulator in run and within pressure limit
_atSetpoint := ABS(_pressurePid.Error)< Config.atSetpointWindow AND _state = SlackwireSM.RUN;

_valveOutputValue := _pressurePid.Out;
//_valveOutputValue := 0.01;

IF _activate THEN
	valve.setValve (CMD := ValveCMD.ON, value :=  _valveOutputValue );  
ELSE
	  valve.setValve (CMD := ValveCMD.OFF,value := 0);
END_IF
THIS^._updateValve(); // code to run valve object]]]]><![CDATA[>]]></ST>
    </Implementation>
    <Property Name="endSensorOuterTriggered" Id="{0ce19166-29f6-4639-b39b-d27c345c5785}">
      <Declaration><![CDATA[{attribute 'monitoring' := 'call'}
{attribute 'TcRpcEnable'}
PROPERTY endSensorOuterTriggered : bool]]></Declaration>
      <Get Name="Get" Id="{a7d6ef0e-e88b-4158-b71b-c5da1e6246ac}">
        <Declaration><![CDATA[VAR
END_VAR
]]></Declaration>
        <Implementation>
          <ST><![CDATA[endSensorOuterTriggered := NOT IO_In.endSensorOuter;]]></ST>
        </Implementation>
      </Get>
    </Property>
    <Property Name="opState" Id="{113b0412-328a-4d03-8c24-8818334d8d44}">
      <Declaration><![CDATA[{attribute 'monitoring' := 'call'}
{attribute 'TcRpcEnable'}
PROPERTY opState : p7type.P6OpState
]]></Declaration>
      <Get Name="Get" Id="{74a17069-5d5a-4232-8fc7-221c17fc51c0}">
        <Declaration><![CDATA[VAR
END_VAR
]]></Declaration>
        <Implementation>
          <ST><![CDATA[opState := _opState;]]></ST>
        </Implementation>
      </Get>
    </Property>
    <Property Name="backtension" Id="{2d20d8e4-f2d4-4d3b-b896-d1e9f7c6e773}">
      <Declaration><![CDATA[{attribute 'monitoring' := 'call'}
{attribute 'TcRpcEnable'}
// Calculated backtension in Kg based on hydraulic pressure
PROPERTY backtension : lreal]]></Declaration>
      <Get Name="Get" Id="{86813d38-a98e-4f57-b0dc-b10716a29135}">
        <Declaration><![CDATA[VAR
END_VAR
]]></Declaration>
        <Implementation>
          <ST><![CDATA[backtension := _tension ;	// setpoint in Kg]]></ST>
        </Implementation>
      </Get>
    </Property>
    <Method Name="deactivate" Id="{3e3aff88-ef67-4bda-9b26-6b0b724f3673}">
      <Declaration><![CDATA[METHOD PUBLIC deactivate : BOOL
VAR_INPUT
END_VAR
]]></Declaration>
      <Implementation>
        <ST><![CDATA[_activate := false;]]></ST>
      </Implementation>
    </Method>
    <Method Name="_calculateRequestedPressure" Id="{54ad30c4-49fd-474e-8a40-64a4bbac77ca}">
      <Declaration><![CDATA[METHOD PRIVATE _calculateRequestedPressure : LREAL 	// in bar
VAR_INPUT
	tension: LREAL;									// Kg
END_VAR

VAR_INST
	debgMsg_1				: DebugMessage_timed; 
	_pistonArea :LREAL;								// in cm2
	
END_VAR]]></Declaration>
      <Implementation>
        <ST><![CDATA[// Calculate pressure based on requested tension
IF config.pistonDiameter > 0 THEN
	_pistonArea := EXPT(config.pistonDiameter * 50,2) * PI;		// 50 = convert to cm and divide by 2 to get radius, result in cm2
	_calculateRequestedPressure := ( tension * 2)/ _pistonArea; 
	RETURN;
END_IF

debgMsg_1(st1:= 'SlackWireCompensator:',st2:='WARNING - Missing config: Piston diameter');
_calculateRequestedPressure := 0;


]]></ST>
      </Implementation>
    </Method>
    <Method Name="_checkSensorErrors" Id="{61124273-f3eb-4891-a234-85ade53f3f90}">
      <Declaration><![CDATA[METHOD PRIVATE _checkSensorErrors : BOOL
VAR_INPUT
END_VAR
VAR_INST
	debgMsg_1				: DebugMessage_timed; 
	debgMsg_2				: DebugMessage_timed;
	debgMsg_3				: DebugMessage_timed; 
END_VAR
]]></Declaration>
      <Implementation>
        <ST><![CDATA[_sensorError := FALSE;

IF gvlSlackWireCompensator.io_in.stringSensor.status.bits.Error THEN
	_sensorError := TRUE;
	debgMsg_1(st1:= 'SlackWireCompensator: ',st2:='String sensor failure');
END_IF

IF _pressureSensor.Error THEN
	_sensorError := TRUE;
	debgMsg_2(st1:= 'SlackWireCompensator: ',st2:='Pressure sensor failure');
END_IF

IF (endSensorInnerTriggered AND endSensorOuterTriggered) THEN
	_sensorError := TRUE;
	debgMsg_3(st1:= 'SlackWireCompensator:',st2:='End sensor failure');
END_IF

_checkSensorErrors := _sensorError;]]></ST>
      </Implementation>
    </Method>
    <Property Name="position" Id="{69f0f607-1016-43f0-a817-11bb81e33ecd}">
      <Declaration><![CDATA[{attribute 'monitoring' := 'call'}
{attribute 'TcRpcEnable'}
// Cylinder position 0 - 1
PROPERTY PUBLIC position : lreal]]></Declaration>
      <Get Name="Get" Id="{4e89209e-a380-4c2c-96a8-0b481be97002}">
        <Declaration><![CDATA[VAR
END_VAR
]]></Declaration>
        <Implementation>
          <ST><![CDATA[position := _stringPosition;]]></ST>
        </Implementation>
      </Get>
    </Property>
    <Property Name="pressure" Id="{6afd6954-d69f-464b-a978-053c82bde05c}">
      <Declaration><![CDATA[{attribute 'monitoring' := 'call'}
{attribute 'TcRpcEnable'}
// Measured pressure from SWC sylinder
PROPERTY pressure : lreal
]]></Declaration>
      <Get Name="Get" Id="{b1072e56-c65b-44b9-9e1b-2fbfbabe766d}">
        <Declaration><![CDATA[VAR
END_VAR
]]></Declaration>
        <Implementation>
          <ST><![CDATA[pressure := _pressure;]]></ST>
        </Implementation>
      </Get>
    </Property>
    <Property Name="InCenter" Id="{72a512dd-ce5f-47a5-81a6-38cbd3a23bb2}">
      <Declaration><![CDATA[{attribute 'monitoring' := 'call'}
{attribute 'TcRpcEnable'}
// Position within ssenter threshold
PROPERTY InCenter : bool]]></Declaration>
      <Get Name="Get" Id="{9b484455-f0a8-4a96-8379-c754225c582b}">
        <Declaration><![CDATA[VAR
END_VAR
]]></Declaration>
        <Implementation>
          <ST><![CDATA[// is string position within limits?
InCenter := _stringPosition > 0.5 - Config.InCenterWindow AND _stringPosition < 0.5 + Config.InCenterWindow;]]></ST>
        </Implementation>
      </Get>
    </Property>
    <Method Name="setBacktension" Id="{81be5df5-1c1c-411d-a2c2-5c4332d6351d}">
      <Declaration><![CDATA[// Tension in Kg
METHOD PUBLIC setBacktension : BOOL 
VAR_INPUT
	tension : LREAL;	// tension in kg
END_VAR
VAR
	
END_VAR]]></Declaration>
      <Implementation>
        <ST><![CDATA[_tensionrRequested := tension; // Kg]]></ST>
      </Implementation>
    </Method>
    <Method Name="_updateValve" Id="{8efb6755-758a-4d6f-b897-02c0071b69f7}">
      <Declaration><![CDATA[METHOD PRIVATE _updateValve : BOOL
VAR_INPUT
END_VAR
]]></Declaration>
      <Implementation>
        <ST><![CDATA[valve(
	Config				:= Config.swcValve,
	IO_In				:= IO_In.swcValve,
	IO_Out				:= IO_Out.swcValve
);]]></ST>
      </Implementation>
    </Method>
    <Property Name="RegulatorAtSetpoint" Id="{c656e1fd-238b-4d00-b30f-60a67fa66c66}">
      <Declaration><![CDATA[{attribute 'monitoring' := 'call'}
{attribute 'TcRpcEnable'}
// Regulator in operation, and error below threshold
PROPERTY RegulatorAtSetpoint : bool]]></Declaration>
      <Get Name="Get" Id="{038680a8-06ed-4e73-bfd4-e00ff0819b52}">
        <Declaration><![CDATA[VAR
END_VAR
]]></Declaration>
        <Implementation>
          <ST><![CDATA[RegulatorAtSetpoint := _atSetpoint;]]></ST>
        </Implementation>
      </Get>
    </Property>
    <Method Name="_normalizeStringInput" Id="{d2fa6957-c89f-4371-88dc-47990a6f8c92}">
      <Declaration><![CDATA[METHOD PRIVATE _normalizeStringInput : LREAL
VAR_INPUT
END_VAR
VAR
	_stringSensorRawValue	: LREAL;
	
END_VAR
VAR_INST
	debgMsg_1				: DebugMessage_timed; 
	debgMsg_2				: DebugMessage_timed; 
	_stringsensorNormalized : LREAL;
END_VAR
	


]]></Declaration>
      <Implementation>
        <ST><![CDATA[
IF gvlSlackWireCompensator.io_in.stringSensor.status.bits.Error THEN
	RETURN;
END_IF

IF Config.minStringSensorRawValue = config.maxStringSensorRawValue THEN
	debgMsg_1(st1:= 'SlackWireCompensator',st2:='WARNING - missing end position calibration');
	_stringPosition 	:= 0;
	_normalizeStringInput 	:= 0;
	RETURN;
END_IF


_stringSensorRawValue := INT_TO_LREAL(IO_In.stringSensor.input);

_stringsensorNormalized := (_stringSensorRawValue - Config.minStringSensorRawValue) / (Config.maxStringSensorRawValue - Config.minStringSensorRawValue);

IF _stringsensorNormalized > 1.03 OR _stringsensorNormalized <-0.03 THEN
	debgMsg_2(st1:= 'SlackWireCompensator',st2:='Position signal outside limit - please recalibrate');
END_IF

_normalizeStringInput := LIMIT(0,_stringsensorNormalized,1);
]]></ST>
      </Implementation>
    </Method>
    <Property Name="endSensorInnerTriggered" Id="{daa1b5e6-ff4f-485f-9679-d831468b6b59}">
      <Declaration><![CDATA[{attribute 'monitoring' := 'call'}
{attribute 'TcRpcEnable'}
PROPERTY endSensorInnerTriggered : bool]]></Declaration>
      <Get Name="Get" Id="{ed9c0318-208e-4db3-b5d3-adebb7e097ca}">
        <Declaration><![CDATA[VAR
END_VAR
]]></Declaration>
        <Implementation>
          <ST><![CDATA[endSensorInnerTriggered := NOT IO_In.endSensorInner;]]></ST>
        </Implementation>
      </Get>
    </Property>
    <Method Name="_calculateBacktension" Id="{db74b3ab-36d4-4a0a-b062-8948dfd73337}">
      <Declaration><![CDATA[METHOD PRIVATE _calculateBacktension : LREAL // in KG
VAR_INPUT
	cylinderPressure : LREAL;				 // in bar
END_VAR

VAR_INST
	_pistonArea :LREAL;	// in cm2
	
END_VAR]]></Declaration>
      <Implementation>
        <ST><![CDATA[// Calculate wire tension based on oil pressure
IF config.pistonDiameter > 0 THEN
	_pistonArea := EXPT(config.pistonDiameter * 50,2) * PI;		// 50 = convert to cm and divide by 2 to get radius, result in cm2
	_calculateBacktension := (cylinderPressure * _pistonArea )/ 2 ; 
	RETURN;
END_IF

_calculateBacktension := 0;


]]></ST>
      </Implementation>
    </Method>
    <Method Name="activate" Id="{feec195b-c35e-49e6-9f68-2feb1b3eef8b}">
      <Declaration><![CDATA[METHOD PUBLIC activate : BOOL
VAR_INPUT
END_VAR
]]></Declaration>
      <Implementation>
        <ST><![CDATA[_activate := TRUE;
]]></ST>
      </Implementation>
    </Method>
  </POU>
</TcPlcObject>