﻿<?xml version="1.0" encoding="utf-8"?>
<TcPlcObject Version="1.1.0.1">
  <POU Name="MRUcontroller" Id="{33b0a4c3-5571-45c4-bfa1-a8fe0bf2d998}" SpecialFunc="None">
    <Declaration><![CDATA[FUNCTION_BLOCK ABSTRACT MRUcontroller IMPLEMENTS iMRUController

VAR_INPUT
   DualPositions	: BOOL ;		// if we receive to positions in same message
END_VAR

VAR CONSTANT 
    BUFFER_SIZE : DINT := 200;		// the largest sentence is less than 100 bytes in total NOTE: Must be the same as in NMEAreader!!
END_VAR
VAR
    validMRUdata 			: TOF := (PT := T#1S);	// one second timeout for MRU data
	_AHCActive				: BOOL;			// Turn on to start mixer
	sentence 				: STRING(BUFFER_SIZE);
    result 					: BOOL; 							// true if parser succeeds
    newNMEAstringAvailable 	: BOOL;			// true if new sentence is ready to parse
    Mixer 					: MRU_RampUpDown;
	SelectMRUposition2		: BOOL;			//If on, use position #2
	parseSuccess 			: BOOL;
	
	ahc1_pos : LREAL;
    ahc1_vel : LREAL;
    ahc1_acc : LREAL;
	ahc2_pos : LREAL;
    ahc2_vel : LREAL;
    ahc2_acc : LREAL;

    ahcout_pos : LREAL;
    ahcout_vel : LREAL;
    ahcout_acc : LREAL;
END_VAR

VAR	// Parsing
	NMEAmessagesParsed		: UDINT;
	NMEAparseErrorEvents 	: UDINT;
END_VAR]]></Declaration>
    <Implementation>
      <ST><![CDATA[
// Parse data in NMEA sentence
IF newNMEAstringAvailable THEN
	parseSuccess := _NMEA2Data(sentence);  
END_IF  
// valid data timeout
validMRUdata(IN:= parseSuccess);


// do stuff with parsed data
// ramp up/down when turning AHC on off

IF TRUE THEN	// don't kill AHC, let it tramp down 
	IF DualPositions AND SelectMRUposition2 THEN // CTD Crane
		Mixer(
			Active := IsActive AND validMRUdata.Q ,
			AHCPosition := ahc2_pos,
			AHCVelocity := ahc2_vel,
			AHCAcceleration := ahc2_acc);
	ELSE										// Default or A-frame
		Mixer(
			Active := IsActive AND validMRUdata.Q,
			AHCPosition := ahc1_pos,
			AHCVelocity := ahc1_vel,
			AHCAcceleration := ahc1_acc);
	END_IF
	ahcout_pos := Mixer.Position;
    ahcout_vel := Mixer.Velocity;
    ahcout_acc := Mixer.Acceleration;
	
	// just for monitoring
	//Mixer.Ready;
    
ELSE
    ahcout_pos := 0;
    ahcout_vel := 0;
    ahcout_acc := 0;
END_IF



]]></ST>
    </Implementation>
    <Method Name="_NMEA2Data" Id="{0cd09a1f-ca6d-451b-8c4b-da5031a0872a}">
      <Declaration><![CDATA[// parse NMEA string and extract MRU data. Return TRUE on success
METHOD _NMEA2Data : BOOL 
VAR_INPUT
    data : REFERENCE TO STRING(BUFFER_SIZE);
END_VAR
VAR_INST
   decoder : p6com.NMEAParser;
   started : BOOL;
   // error   : BOOL;
   timer   : TOF;
END_VAR
]]></Declaration>
      <Implementation>
        <ST><![CDATA[IF NOT started THEN
    started := TRUE;
	IF DualPositions THEN
    	decoder.setFieldsOfInterest('..RRRRRR');
	ELSE
		decoder.setFieldsOfInterest('..RRR');
	END_IF
END_IF

result := decoder.parse(data);

IF  result THEN
    ahc1_acc := decoder.getRealField(2);
    ahc1_vel := decoder.getRealField(3);
    ahc1_pos := decoder.getRealField(4);
	IF DualPositions THEN
		ahc2_acc := decoder.getRealField(5);
    	ahc2_vel := decoder.getRealField(6);
    	ahc2_pos := decoder.getRealField(7);
	ELSE
		ahc2_acc := 0;
    	ahc2_vel := 0;
    	ahc2_pos := 0;
	END_IF
	NMEAmessagesParsed := NMEAmessagesParsed +1;
 ELSE
    ahc1_acc := 0;
    ahc1_vel := 0;
    ahc1_pos := 0;
	ahc2_acc := 0;
    ahc2_vel := 0;
    ahc2_pos := 0;
	NMEAparseErrorEvents := NMEAparseErrorEvents +1;
END_IF

_NMEA2Data := result;	// not in use ?

]]></ST>
      </Implementation>
    </Method>
    <Property Name="selectMRUinput2" Id="{113fd0bf-5f97-420f-9813-f7d91d3b73c1}">
      <Declaration><![CDATA[{attribute 'monitoring':='call'}
{attribute 'TcRpcEnable'}
// 0 or 1 selects A-frame or CTD crane
PROPERTY PUBLIC selectMRUinput2 : BOOL]]></Declaration>
      <Set Name="Set" Id="{0880ad89-6359-4bc3-b147-51fb0f97a5d1}">
        <Declaration><![CDATA[VAR
END_VAR
]]></Declaration>
        <Implementation>
          <ST><![CDATA[// 0 is position 1 A-frame
SelectMRUposition2 := selectMRUinput2;]]></ST>
        </Implementation>
      </Set>
      <Get Name="Get" Id="{b61320c3-06b3-4720-8d8a-5cf4734c06c0}">
        <Declaration><![CDATA[VAR
END_VAR
]]></Declaration>
        <Implementation>
          <ST><![CDATA[selectMRUinput2 := SelectMRUposition2;]]></ST>
        </Implementation>
      </Get>
    </Property>
    <Property Name="MRUdataOK" Id="{2a4c7106-ca7e-4479-a4eb-b5b908e1436d}">
      <Declaration><![CDATA[{attribute 'monitoring':='call'}
{attribute 'TcRpcEnable'}
// true if MRU data is received and valid
PROPERTY MRUdataOK : BOOL]]></Declaration>
      <Get Name="Get" Id="{43832bd0-ae69-4772-b07a-0a51cb8360c5}">
        <Declaration><![CDATA[VAR
END_VAR
]]></Declaration>
        <Implementation>
          <ST><![CDATA[MRUdataOK := validMRUdata.Q ;
]]></ST>
        </Implementation>
      </Get>
    </Property>
    <Property Name="MRUAcceleration" Id="{41b75db1-8f76-40e2-8191-2f00eda08127}">
      <Declaration><![CDATA[{attribute 'monitoring':='call'}
{attribute 'TcRpcEnable'}
PROPERTY MRUAcceleration : LREAL]]></Declaration>
      <Get Name="Get" Id="{ca7ce0cd-3513-4c5e-9717-f942d77eaf65}">
        <Declaration><![CDATA[VAR
END_VAR
]]></Declaration>
        <Implementation>
          <ST><![CDATA[MRUAcceleration := ahcout_acc;

]]></ST>
        </Implementation>
      </Get>
    </Property>
    <Property Name="MixReady" Id="{49b0e142-b0df-4a99-8750-268969ae170c}">
      <Declaration><![CDATA[{attribute 'monitoring':='call'}
{attribute 'TcRpcEnable'}
PROPERTY MixReady : BOOL]]></Declaration>
      <Get Name="Get" Id="{d61e7089-8455-46f4-9beb-5738377e309c}">
        <Declaration><![CDATA[VAR
END_VAR
]]></Declaration>
        <Implementation>
          <ST><![CDATA[MixReady := Mixer.Ready;]]></ST>
        </Implementation>
      </Get>
    </Property>
    <Property Name="MRUPosition" Id="{5018b61f-2f73-4450-a97d-4f3b89f4e4d4}">
      <Declaration><![CDATA[{attribute 'monitoring':='call'}
{attribute 'TcRpcEnable'}
PROPERTY MRUPosition : LREAL]]></Declaration>
      <Get Name="Get" Id="{09f72e9d-fb1d-4ff8-9aaf-0a8ebb4a33f0}">
        <Declaration><![CDATA[VAR
END_VAR
]]></Declaration>
        <Implementation>
          <ST><![CDATA[MRUPosition := ahcout_pos;

]]></ST>
        </Implementation>
      </Get>
    </Property>
    <Property Name="Activate" Id="{d1f76ff4-81e3-44e3-9e8d-4e535b0c142f}">
      <Declaration><![CDATA[{attribute 'monitoring':='call'}
{attribute 'TcRpcEnable'}
// turn on mixer and output data to consumers.
PROPERTY PUBLIC Activate : bool]]></Declaration>
      <Set Name="Set" Id="{8e747ea2-648e-4fa8-ac13-0adcda2f0413}">
        <Declaration><![CDATA[VAR
END_VAR
]]></Declaration>
        <Implementation>
          <ST><![CDATA[_AHCActive := Activate;
]]></ST>
        </Implementation>
      </Set>
    </Property>
    <Property Name="IsActive" Id="{d3f5232b-5ce9-4543-b445-d1b7c5efba21}">
      <Declaration><![CDATA[{attribute 'monitoring':='call'}
{attribute 'TcRpcEnable'}
// AHC active and valid data received.
PROPERTY IsActive : BOOL]]></Declaration>
      <Get Name="Get" Id="{b08cb333-516d-4ede-b43e-faa7532e5016}">
        <Declaration><![CDATA[VAR
END_VAR
]]></Declaration>
        <Implementation>
          <ST><![CDATA[IsActive := _AHCActive aND validMRUdata.Q ; ]]></ST>
        </Implementation>
      </Get>
    </Property>
    <Property Name="MRUVelocity" Id="{f1c579ef-4397-4277-930d-010850f38187}">
      <Declaration><![CDATA[{attribute 'monitoring':='call'}
{attribute 'TcRpcEnable'}
PROPERTY MRUVelocity : LREAL]]></Declaration>
      <Get Name="Get" Id="{96bb8e8d-8985-4773-979c-fb0169f50787}">
        <Declaration><![CDATA[VAR
END_VAR
]]></Declaration>
        <Implementation>
          <ST><![CDATA[MRUVelocity := ahcout_vel;


]]></ST>
        </Implementation>
      </Get>
    </Property>
  </POU>
</TcPlcObject>