﻿<?xml version="1.0" encoding="utf-8"?>
<TcPlcObject Version="1.1.0.1">
  <POU Name="fb_Outhauler" Id="{b2bcbffa-38de-44db-b711-a45b0b7b6bbc}" SpecialFunc="None">
    <Declaration><![CDATA[{attribute 'hide_all_locals'}
FUNCTION_BLOCK PUBLIC fb_Outhauler
VAR_INPUT
	drive 		: p7type.IDrive;	
	config 		: REFERENCE TO ConfigOuthauler;
	OHC			: REFERENCE TO OHC;			// to lift/lower outhauler
	ewinch		: REFERENCE TO LocalEWinch;  //p6winch.EWinch;		// winch object to get speed from
	userWinch	: REFERENCE TO LocalBaseUserWinch; //p6winch.BaseUserWinch;	// for constant tension state.
	vfd			: REFERENCE TO p7elec.ACS880;
	estop 		: IEmergencyStop ;
END_VAR
VAR_OUTPUT
END_VAR
VAR
	
	_opState: P6OpState;
	hasStarted: BOOL;
	// demo
	ErrorTimer1 		: TON := (PT := T#15S);
	ErrorTimer2 		: TON := (PT := T#3S);
	runCommand 			: BOOL;
	//rollerRPMrequested 	: LREAL ; // rollerRPM
	state 				: INT;
	_velocitySetpoint	: LREAL ; 	// requested speed in m/s
	_maxTorque			: LREAL ; 	// max torqueRel to drive
	winchTrigger 		: R_TRIG;	// detect TRW start
	CTtrigger 			: R_TRIG;	// detect TRW CT mode
	LengthTrigger		: R_TRIG;	// detect TRW length going inder Safety length
	startSignal			: BOOL ;
	reducedTorque		: BOOL;		// torque limited due to TRW speed
			

END_VAR
]]></Declaration>
    <Implementation>
      <ST><![CDATA[

IF NOT hasStarted THEN
	hasStarted := TRUE;
	RETURN;
END_IF

winchTrigger(CLK := ewinch.OpState = P6OpState.RUN);
CTtrigger(CLK := userWinch.ManMode = p7type.WinchManualModes.CONSTANT_TORQUE);

_opState := drive.OpState; // just copy drive state

IF runCommand THEN 
	
	drive.setOperation(CMD := DriveCMD.ACTIVATE);
	IF OHC.OuthaulerEngaged = FALSE OR ewinch.OpState <> P6OpState.RUN THEN
		runCommand := FALSE;
	END_IF
	IF ABS(ewinch.Velocity) > config.maximumWireSpeedDisconnect THEN
		OHC.OuthaulerDisengage(); 
		runCommand := FALSE;
	END_IF
	
	IF config.automaticDisengageInCT AND userWinch.ManMode = p7type.WinchManualModes.CONSTANT_TORQUE THEN
		OHC.OuthaulerDisengage();
		runCommand := FALSE;
	END_IF
END_IF

IF NOT runCommand OR estop.Activated THEN 
	drive.setOperation(CMD := DriveCMD.STOP);
//	IF NOT estop.Activated AND OHC.OuthaulerEngaged AND ABS(ewinch.Velocity) > 0.01 THEN		// If roller is engaged, and someone is requesting speed, then start
//		runCommand := TRUE;
//	END_IF
	IF NOT estop.Activated AND OHC.OuthaulerEngaged AND ewinch.OpState = P6OpState.RUN THEN		// If roller is engaged, and winch is running, then start
		runCommand := TRUE;
	END_IF
	IF NOT estop.Activated AND config.autoStartOnWinchActive AND (winchTrigger.Q OR CTtrigger.Q) AND NOT gvlTractionWinch01.splitModeRequest THEN		// If roller is engaged, , then start
		OHC.OuthaulerEngage();
		runCommand := TRUE;
	END_IF
END_IF

//// only update speed if trw is running
//IF ABS (vfd.getSpeedRef()) > 2 THEN  //ABS(ewinch.Velocity) > 0.01 THEN
//	_velocitySetpoint := (vfd.getSpeedRef() * ewinch.Radius * math.PI2 )/ 6000  + config.speedOffset;
//ELSE
//	_velocitySetpoint := 0;
//END_IF

_velocitySetpoint := config.fixedSpeedSetting;	//0.45 m/s?

IF ewinch.Velocity > 1.2 OR ewinch.Velocity < -1 THEN // limit high speed braking
	_maxTorque := config.outhaulerTorqueRel * 0.4 ;
	reducedTorque := TRUE;		
ELSE
	_maxTorque :=  config.outhaulerTorqueRel ;
	reducedTorque := FALSE;
END_IF


_setOuthaulerVelocity();



]]></ST>
    </Implementation>
    <Method Name="Activate" Id="{34a2b494-f6c7-4ea6-99e5-b212eb537d62}">
      <Declaration><![CDATA[{attribute 'TcRpcEnable'}
METHOD PUBLIC Activate : BOOL
]]></Declaration>
      <Implementation>
        <ST><![CDATA[
runCommand := TRUE;
OHC.OuthaulerEngage(); // lower outhauler
]]></ST>
      </Implementation>
    </Method>
    <Method Name="_setOuthaulerVelocity" Id="{369dc090-978d-0f4e-143c-c1640afdeb0a}">
      <Declaration><![CDATA[METHOD PRIVATE _setOuthaulerVelocity : BOOL
VAR_INPUT
END_VAR
VAR_INST
	RPM_sp : LREAL;
END_VAR]]></Declaration>
      <Implementation>
        <ST><![CDATA[
RPM_sp :=  ( _velocitySetpoint /  (config.roller_diameter * math.PI)) * 60  ; // result is RPM
drive.setProcessValues( Speed := RPM_sp,_maxTorque);		// Outhauler wheel RPM and torqueRel]]></ST>
      </Implementation>
    </Method>
    <Property Name="Error" Id="{40a90038-3489-495a-9af1-8811f348085a}">
      <Declaration><![CDATA[{attribute 'monitoring' := 'call'}
{attribute 'TcRpcEnable'}
PROPERTY Error : bool
]]></Declaration>
      <Get Name="Get" Id="{f4110f12-9aac-452c-be41-2b276f4baea8}">
        <Declaration><![CDATA[VAR
END_VAR
]]></Declaration>
        <Implementation>
          <ST><![CDATA[Error := _opState = P6OpState.NOTAVAILABLE;]]></ST>
        </Implementation>
      </Get>
    </Property>
    <Property Name="opState" Id="{682fd480-26f7-4708-8fa8-2be48e3f0d09}">
      <Declaration><![CDATA[{attribute 'monitoring' := 'call'}
{attribute 'TcRpcEnable'}
PROPERTY opState : p7type.P6OpState
]]></Declaration>
      <Get Name="Get" Id="{630ec0ce-34f8-4e21-bddb-a76050479ae5}">
        <Declaration><![CDATA[VAR
END_VAR
]]></Declaration>
        <Implementation>
          <ST><![CDATA[opState := _opState;]]></ST>
        </Implementation>
      </Get>
    </Property>
    <Method Name="Deactivate" Id="{a6d850bc-6c24-4170-8005-d0e6c558f7ee}">
      <Declaration><![CDATA[{attribute 'TcRpcEnable'}
METHOD PUBLIC Deactivate : BOOL
]]></Declaration>
      <Implementation>
        <ST><![CDATA[
runCommand := FALSE;
OHC.OuthaulerDisengage(); // lift outhauler]]></ST>
      </Implementation>
    </Method>
    <Property Name="Velocity" Id="{ba7c0c01-2612-4ded-8db4-05e0df4e6cec}">
      <Declaration><![CDATA[{attribute 'monitoring' := 'call'}
{attribute 'TcRpcEnable'}
PROPERTY PUBLIC Velocity : LREAL]]></Declaration>
      <Get Name="Get" Id="{5d78ab9d-1bc6-44ab-b1e0-eacc9c79d56f}">
        <Declaration><![CDATA[VAR
END_VAR
]]></Declaration>
        <Implementation>
          <ST><![CDATA[Velocity :=  Drive.getSpeed() * config.roller_diameter * Math.PI / 60 ;]]></ST>
        </Implementation>
      </Get>
    </Property>
    <Property Name="VelocityCommand" Id="{d4acecb8-cdf0-4ae2-9493-f2170064849d}">
      <Declaration><![CDATA[{attribute 'monitoring' := 'call'}
{attribute 'TcRpcEnable'}
// speed in m/sec
PROPERTY VelocityCommand : LREAL]]></Declaration>
      <Get Name="Get" Id="{4afed89b-5c33-4cbe-8c94-1344915bd0f2}">
        <Declaration><![CDATA[PUBLIC 
VAR
END_VAR
]]></Declaration>
        <Implementation>
          <ST><![CDATA[VelocityCommand := _velocitySetpoint;

//VelocityCommand := rollerRPMrequested * config.roller_diameter *math.PI / 60 ;


//rollerRPMrequested :=  ( VelocityCommand /  config.roller_diameter) * 60  ;]]></ST>
        </Implementation>
      </Get>
    </Property>
  </POU>
</TcPlcObject>