﻿<?xml version="1.0" encoding="utf-8"?>
<TcPlcObject Version="1.1.0.1">
  <POU Name="AutoController" Id="{801019e1-db92-4f8f-93dc-f19458f37038}" SpecialFunc="None">
    <Declaration><![CDATA[FUNCTION_BLOCK AutoController
VAR_INPUT
	Config  				: REFERENCE TO p6winch.ConfigElectricWinch;
    OpValues                : REFERENCE TO OpValuesAutoController;
	winch 					: IAutoWinch; 
    eWinch                  : p6winch.IWinch;
	lengthProcessValue		: LREAL;
	controlUnit				: p7type.IBaseControlUnit; 
	drive					: p7type.IDrive; 
    AHC                     : iMRUController;
	EmergencyStop			: p6com.IEmergencyStop;
	brakeHPU				: REFERENCE TO localHPUBrake;
END_VAR
VAR
    //requestHaulIn           : BOOL;
    //autoActive              : BOOL;
    state                   : AutoControllerSM;
    autoControllerMode      : p7type.WinchModes;    // MANUAL or AUTO
    _opstate                : p7type.P6OpState;
    CMD                     : AutoControllerCMD;
    
    tensionMax              : LREAL;
    velocityMax             : LREAL;
    doRun                   : BOOL;
    //autoModeActive          : BOOL;      
    PIDrun                  : BOOL;		// PID active
	TRJrun					: BOOL;		// Trajectory active 
    //stopButton              : BOOL;
    stopButtonTrig          : R_TRIG;
	AHC_pidCtrl    			: AHC_PIDController;
    trajectory              : TrajectoryGenerator;
    timer                   : p6com.CycleGenerator;
    timerPreOp              : TON;
    TorqueRelout            : LREAL; // 0-1
	SpeedRPMout				: LREAL; 
    SpeedRelOut             : LREAL; // 0-1
	WinchOpstateOutOfRun	: F_TRIG;		// cancels auto operation if winch stops
END_VAR
]]></Declaration>
    <Implementation>
      <ST><![CDATA[stopButtonTrig(CLK:= controlUnit.ActionSignal);
WinchOpstateOutOfRun (CLK := (winch.OpState = p7type.P6OpState.RUN));	// detect if winch goes out of run state

IF EmergencyStop.Activated THEN
	state := AutoControllerSM.EMERGENCY_STOP; // stop and go to emstop state
END_IF


// add this: Option_HasAHC	

CASE state OF
    AutoControllerSM.IDLE:
		autoControllerMode := p7type.WinchModes.MANUAL;
		_setAutoModeAtWinch(FALSE);
		_opstate := p6com.P6OpState.STOPPED;
		PIDrun := FALSE;
		TRJrun := FALSE;
        
		IF controlUnit.AutoAvailable AND _isAutoStartAllowed() THEN
			CASE CMD OF
				AutoControllerCMD.AUTO:    
					state := AutoControllerSM.AUTO_PREPARE;
			END_CASE
		END_IF		 
        
        
	AutoControllerSM.AUTO_PREPARE:
		IF config.AutoController.Activate_w_currentLength THEN
			OpValues.setpoint := INT_TO_LREAL( LREAL_TO_INT(lengthProcessValue * 10)) / 10.0;		// Always start at current length when activation (Mbari)
		END_IF
		autoControllerMode := p7type.WinchModes.AUTO;
		_opstate := p6com.P6OpState.PREPARE;
		PIDrun := TRUE;	
		TRJrun := TRUE;
		_setAutoModeAtWinch(TRUE);
		_setAutoSpeedAndTorque(0, 1);
		state := AutoControllerSM.AUTO_RUN;
			
	AutoControllerSM.AUTO_RUN: 	
		_setAutoModeAtWinch(TRUE);
        autoControllerMode := p7type.WinchModes.AUTO;
		_opstate := p6com.P6OpState.RUN;
		PIDrun := TRUE;	
		TRJrun := TRUE;
		_setAutoSpeedAndTorque(SpeedRelOut, 1);		// Always run om max tension, 
		
		IF  stopButtonTrig.Q  OR  _terminateAutoOperation() THEN						
			state := AutoControllerSM.AUTO_STOPPING;		
		END_IF
		
        IF cmd = AutoControllerCMD.STOP THEN						
			state := AutoControllerSM.AUTO_RAMP_DOWN;
		END_IF
		
		
	AutoControllerSM.AUTO_RAMP_DOWN:	
		// find a way to stop winch
		_setAutoModeAtWinch(TRUE);
        autoControllerMode := p7type.WinchModes.AUTO;
		_opstate := p6com.P6OpState.RUN;
		PIDrun := TRUE;	
		TRJrun := FALSE;		// ramp down
		_setAutoSpeedAndTorque(SpeedRelOut, 1);		// Always run om max tension, 
		
		IF stopButtonTrig.Q OR _terminateAutoOperation() THEN						
			state := AutoControllerSM.AUTO_STOPPING;		
		END_IF
		
		IF trajectory.Ready (* detect that winch is stopped*) THEN
			state := AutoControllerSM.AUTO_STOPPING;	
		END_IF
		
	
    AutoControllerSM.AUTO_STOPPING:
		autoControllerMode := p7type.WinchModes.MANUAL;
		PIDrun := FALSE;
		TRJrun := FALSE;
		_setAutoSpeedAndTorque(0, 1);
		_setAutoModeAtWinch(FALSE);
        state := AutoControllerSM.IDLE;
		_opstate := p6com.P6OpState.STOPPED;
		
	AutoControllerSM.EMERGENCY_STOP:
		autoControllerMode := p7type.WinchModes.MANUAL;
		PIDrun := FALSE;
		TRJrun := FALSE;	
		_setAutoSpeedAndTorque(0, 1);
		_setAutoModeAtWinch(FALSE);
        _opstate := p6com.P6OpState.NOTAVAILABLE;
		IF NOT EmergencyStop.Activated THEN
			state := AutoControllerSM.IDLE;
		END_IF
        
END_CASE

timer.update(IN:=T#50MS, PLCTask_CycleTime := T#10MS);

IF OpValues.setpoint < winch.SafetyDistance THEN
	OpValues.setpoint := winch.SafetyDistance;
END_IF

IF timer.Q THEN
 
 // PIDrun := AHC.IsActive;

//Lager utgang for posisjon og velocity som PID-regulatoren skal følge
    trajectory(
        Active := TRJrun,
        Setpoint := OpValues.setpoint,
        PV_Length := lengthProcessValue,
        PV_Velocity := ewinch.Velocity,
        PV_Tension := ewinch.Tension,	
        VelocityHaulin := opvalues.velocityHaulin,		//operatørens innstilling for hastighet
		VelocityPayout := opvalues.velocityPayout,		//operatørens innstilling for hastighet
        TensionMax := opvalues.tensionMax, 				// max tension under haul in vil redusere hastighet
        Acceleration := opvalues.acceleration,			// hvor fort skal vi rampe opp og ned (m/s*s).Setpunkt fra operatør
        AHCActive := TRUE,								// Hvis AHC er av så er AHCPosition null uansett.
        AHCPositon := AHC.MRUPosition,					// posisjon fra MRU
        ErrorMax := config.AutoController.ErrorMax, 					// bør ikke nå denne under bruk (m). Avvik mellom actual og trajectory pos. 
        Scantime := 0.05,
   );
   
    AHC_pidCtrl(
        Active := PIDrun,
        TrajectorySetpoint	:= trajectory.Position, //   trajectory.AHCPositon, // Really??
        ProcessValue 		:= lengthProcessValue,
        TrajectoryVelocity 	:= trajectory.Velocity, //  ahc.AHCVelocity, (m/s)
        AHCAcceleration 	:= ahc.MRUAcceleration * -1 ,
		AHCVelocity			:= ahc.MRUVelocity * -1 ,
        VelocityMax 		:= MAX(opvalues.velocityHaulin,opvalues.velocityPayout) ,	// max hastighet for winch: pick the highest
        Kp := config.AutoController.Kp,		// kan være null
        Ki := config.AutoController.Ki,		// kan være null
        Kd := config.AutoController.Kd,		// kan være null
        Ka := config.AutoController.Ka,		// config for testing  0.05 ?
        Kv := config.AutoController.Kv,		// config for testing  0.95 ?
        Scantime := 0.05
    );
	//VelocityRelOut := AHC_pidCtrl.Out  ;  (* m/s *)  // må konverteres til RPM (* RPM *); 
	
	//  Kv                          : LREAL := 0.9;     // Feed Forward AHV Vel
	//  Ka                          : LREAL := 0.05;    // Feed Forward AHC Acc

    // not velocity, but speed. Drum or motor RPM?
	// convert to RPM AHC_pidCtrl.Out
	
	// ADD or subtract joustick value	

	// Update values TO send TO winch:
	//  m/s divided on winch circumference * 60  should give drum RPM
	SpeedRPMout := ( AHC_pidCtrl.Out * 60 ) / (ewinch.Radius * math.PI2 ) ;
	// divided on max (drum) RPM, we get the speed we want to run the winch with.. Hopefully.
	SpeedRelOut := SpeedRPMout / ewinch.SpeedMax ;
 
END_IF

CMD := AutoControllerCMD.NONE;
]]></ST>
    </Implementation>
    <Method Name="_isOutsideSafetyRange" Id="{18f4af43-f53f-40bf-9c73-0b82708dc7d6}">
      <Declaration><![CDATA[METHOD _isOutsideSafetyRange : BOOL
VAR_INPUT
END_VAR
]]></Declaration>
      <Implementation>
        <ST><![CDATA[_isOutsideSafetyRange := winch.SafetyDistance < lengthProcessValue;]]></ST>
      </Implementation>
    </Method>
    <Property Name="Mode" Id="{1da4ffe4-a769-4297-a69a-d10dfc54e1ed}">
      <Declaration><![CDATA[{attribute 'monitoring' := 'call'}
(* This is the unified state of the state of operation *)
{attribute 'TcRpcEnable'}
// currently AUTO or MANUAL
PROPERTY Mode : p7type.WinchModes]]></Declaration>
      <Get Name="Get" Id="{11358831-4011-4b9b-82ba-a4facd63c4bf}">
        <Declaration><![CDATA[
]]></Declaration>
        <Implementation>
          <ST><![CDATA[Mode := autoControllerMode;


]]></ST>
        </Implementation>
      </Get>
    </Property>
    <Method Name="startAuto" Id="{362bfc23-c7f8-45f9-8996-4b6f3812763c}">
      <Declaration><![CDATA[{attribute 'TcRpcEnable'}
METHOD startAuto : BOOL
VAR_INPUT
END_VAR
]]></Declaration>
      <Implementation>
        <ST><![CDATA[requestOperation(CMD := AutoControllerCMD.AUTO);]]></ST>
      </Implementation>
    </Method>
    <Property Name="OpState" Id="{42c6f3c9-30e5-431e-a622-8550d5930065}">
      <Declaration><![CDATA[{attribute 'monitoring':='call'}
{attribute 'TcRpcEnable'}
PROPERTY OpState : p6com.P6OpState]]></Declaration>
      <Get Name="Get" Id="{9cc281b1-fe4e-4695-85db-77646e64bd19}">
        <Declaration><![CDATA[VAR
END_VAR
]]></Declaration>
        <Implementation>
          <ST><![CDATA[OpState := _opstate;
]]></ST>
        </Implementation>
      </Get>
    </Property>
    <Method Name="_isAutoStartAllowed" Id="{5ecd2fa4-a37f-4769-adb9-21dfca7eca4a}">
      <Declaration><![CDATA[// If winches are active, no estop, outside safety range
METHOD _isAutoStartAllowed : BOOL
VAR_INST
    winchesactive : BOOL;
END_VAR
]]></Declaration>
      <Implementation>
        <ST><![CDATA[winchesactive := (winch.OpState = p7type.P6OpState.RUN) ;

_isAutoStartAllowed  := _isOutsideSafetyRange() AND 
						(NOT emergencyStop.Activated)
						AND brakeHPU.PressureOK AND
						((state = AutoControllerSM.IDLE) OR (state = AutoControllerSM.AUTO_RUN) ) AND
						winchesactive;

                ]]></ST>
      </Implementation>
    </Method>
    <Method Name="_setAutoSpeedAndTorque" Id="{685a67bf-6680-413e-9fad-f56fdb002dab}">
      <Declaration><![CDATA[// input: Velocity, torque
METHOD PRIVATE _setAutoSpeedAndTorque : BOOL
VAR_INPUT
	Velocity		  : LREAL; 		// m/sec
	TorqueRel		  : LREAL; 
END_VAR
]]></Declaration>
      <Implementation>
        <ST><![CDATA[winch.TorqueAuto := TorqueRel;
winch.VelocityAuto := Velocity;


]]></ST>
      </Implementation>
    </Method>
    <Property Name="AutoAllowed" Id="{7310edc8-2c34-4c7b-8ada-4899974028b0}">
      <Declaration><![CDATA[// true if allowed to go to (or already in) AUTO mode. 
{attribute 'monitoring':='call'}
{attribute 'TcRpcEnable'}
PROPERTY AutoAllowed : BOOL]]></Declaration>
      <Get Name="Get" Id="{f6828426-f283-4647-870f-9332e1ca5086}">
        <Declaration><![CDATA[
VAR
END_VAR
]]></Declaration>
        <Implementation>
          <ST><![CDATA[AutoAllowed := _isAutoStartAllowed();]]></ST>
        </Implementation>
      </Get>
    </Property>
    <Method Name="_setAutoModeAtWinch" Id="{74c8c3e8-fff1-48d8-8bd1-da1623d47d2e}">
      <Declaration><![CDATA[METHOD PROTECTED _setAutoModeAtWinch : BOOL
VAR_INPUT
	AutoMode : BOOL;
END_VAR
]]></Declaration>
      <Implementation>
        <ST><![CDATA[winch.AutoMode := AutoMode;
controlUnit.AutoMode := AutoMode;

]]></ST>
      </Implementation>
    </Method>
    <Method Name="requestOperation" Id="{b5420610-614c-455b-9cc6-819734f9397f}">
      <Declaration><![CDATA[{attribute 'TcRpcEnable'}
// STOP or AUTO
METHOD requestOperation : BOOL
VAR_INPUT
    CMD : AutoControllerCMD;
END_VAR
]]></Declaration>
      <Implementation>
        <ST><![CDATA[THIS^.CMD := CMD; ]]></ST>
      </Implementation>
    </Method>
    <Method Name="stopAuto" Id="{c00ce69c-f73e-4e59-ab05-eb4a3dee3db4}">
      <Declaration><![CDATA[{attribute 'TcRpcEnable'}
METHOD stopAuto : BOOL
VAR_INPUT
END_VAR
]]></Declaration>
      <Implementation>
        <ST><![CDATA[requestOperation(CMD := AutoControllerCMD.STOP);]]></ST>
      </Implementation>
    </Method>
    <Method Name="_terminateAutoOperation" Id="{dbed375a-17a3-0e6a-3d2b-88dc55e686bc}">
      <Declaration><![CDATA[METHOD _terminateAutoOperation : BOOL
VAR_INPUT
END_VAR
]]></Declaration>
      <Implementation>
        <ST><![CDATA[_terminateAutoOperation := 	NOT _isOutsideSafetyRange() OR
							// NOT  brakeHPU.PressureOK OR		// Using brakeHPU.Error instead
							// WinchOpstateOutOfRun.Q OR			// Does not work as intended. Winch oes in and out of run when auto is activated
							brakeHPU.Error OR
							emergencyStop.Activated ;
					
                ]]></ST>
      </Implementation>
    </Method>
  </POU>
</TcPlcObject>