(*********************************************************************************
* Copyright: Bernecker+Rainer
* Author:    Mikael Regard
* Created:   May 13, 2019/8:21 AM 
* Updated:   Aug 19, 2019/09:00 AM
 *********************************************************************************
 * Description:
 * FuB to configigure digital output.
 * Max 5 digital outputs can be configured per command.
 * 128 Digital outputs available. ID from 0-127.
 * 
 *Input:
 * MsgFrame  (Profinet output frame)
 * ReadFrame (Profinet reply frame)
 * Execute   (Edgepos, Execute = TRUE will run the FuB once)
 * Cmd Label (Optional 2 byte Command label. Used to identify specfic commands.
 * NumOfDo   (Number of Digital Outputs to configure. min 1 max 5)
 * PosFactorA (Position Factor A in AX+BY. Ignored if TriggerMode = 0)
 * PosFactorB (Position Factor B in AX+BY. Ignored if TriggerMode = 0)
 * TriggerXbotID[0..4]   (Any positive integer represent a particular Xbot)
 * DoCmdLabel[0..4] (Cmd Label to monitor)
 * ForceThreshold[0..4] (Force or Torque threshold (N or Nm))
 * DoID[0..4]			(Array of digital output ID. min = 0 max 127)
 * EventID[0..4]  (1 = Motion Starting, 2 = Motion Ending, 3 = CmdLabel Cmd Starting, 4 = CmdLabel Cmd Ending, 5 = Displacement rising above threshold, 6 = Displacement falling below threshold, 7 = Force rising above theshold, 8 = Force falling below threshold, 128 = Motion state, 129 = CmdLabel state, 130 = Displacement state, 131 Force state)
 * NumOfXbot (Number of Xbot to control. min 1 max 12)
 * TriggerMode[0..4] (0 = X Only, 1 = Y Only, 2 = AX+BY)
 * DisplacementTrigger (Array of Displacement trigger treshold [mm])
 * AxesID[0..4]			(Array of Streaming Axes ID. Bitmap Bit[5:0] : [RZ,RY,RX,Z,Y,X])
 * GroupID  (ID of the Group, 0 = all group, max 10 groups)
 * GroupOption (0 = Add Xbot to empty group, 1 = Clear Group, 2 = Connect all Xbot, 3 = Disconnect Group, 4 = Block motion buffers for all XBOT in group, 5 = Release motion buffer for all XBOT in group, Query status of groups)
 *
 * Output:
 * Error (Indicates if there is an error)
 * Busy  (FuB will be Busy until reply)
 * Done  (When Accepted command is received, Done = TRUE)
 * ErrorID (0x0001 = System Error, 0x2000 = Wrong PMC State, 0x2001 = No Mastership, 0x2002 = Mastership timeout,
 *		    0x2003 = Wrong group state, 0x2004 = Wrong Macro state, 0x2005 = Wrong Digital IO state,
 *          0x2006 = Wrong flyways state, 0x3000 = Wrong Xbot state, 0x4000 = Paramter Error: Invalid Parameters.
 *********************************************************************************)






FUNCTION_BLOCK PM_ConfigDigitalOutput
	// Check Reply
	IF(Busy)THEN
		brsmemcpy(ADR(CmdLbl),ADR(ReadFrame[3]),2);
		IF(ReadFrame[0] = CmdCount AND
			ReadFrame[1] 	= 27  AND
			ReadFrame[2]	= 11 AND
			CmdLbl = CmdLabel)THEN
			Done 	:= TRUE;
			Busy 	:= FALSE;
			brsmemcpy(ADR(ErrorID),ADR(ReadFrame[5]),2);
			Error 	:= UINT_TO_BOOL(ErrorID);
			Done 	:= NOT(UINT_TO_BOOL(ErrorID));
		ELSE
			Busy := FALSE;
		END_IF;
	END_IF;
	//Send Command		
	IF(Execute AND NOT(Done) AND NOT(Error))THEN
		IF(NOT(Busy))THEN
			brsmemset(ADR(MsgFrame[1]),FALSE,98); //rs frame
			CmdCount 		:= ReadFrame[0] +1;
			MsgFrame[0] 	:= CmdCount; 
			MsgFrame[1] 	:= 27;
			MsgFrame[2] 	:= 11;
			brsmemcpy(ADR(MsgFrame[3]), ADR(CmdLabel), 2);
			MsgFrame[5]		:= 1;
			MsgFrame[10]	:= NumOfDo;
			
			FOR count := 0 TO NumOfDo DO
				MsgFrame[11+(DO_BYTE_SEQUENCE*count)] := DoID[count];
				MsgFrame[12+(DO_BYTE_SEQUENCE*count)] := EventID[count];
				MsgFrame[13+(DO_BYTE_SEQUENCE*count)] := TriggerXbotID[count];
				
				IF((EventID[count] = 1) OR (EventID[count] = 2) OR (EventID[count] = 128))THEN
					// Do nothing, B[27:14] := 0;
					// Can be removed. Keeping it for overhead
					
				ELSIF((EventID[count] = 3) OR (EventID[count] = 4) OR (EventID[count] = 129))THEN
					brsmemcpy(ADR(MsgFrame[14]), ADR(DoCmdLabel[count]), 2);
					//Rest 0

				ELSIF((EventID[count] = 5) OR (EventID[count] = 6) OR (EventID[count] = 130))THEN
					MsgFrame[14+(DO_BYTE_SEQUENCE*count)] := TriggerMode[count];
					//MsgFrame[15 + ..] = 0;
					
					DispTriggmm2m[count] := DisplacementTrigger[count]/1000.0;
					//PosFacAmm2m[count]	:= PosFactorA[count]/1000.0;
					//PosFacBmm2m[count]	:= PosFactorB[count]/1000.0;
					
					brsmemcpy(ADR(MsgFrame[16]), ADR(DispTriggmm2m[count]), 4);
					IF(TriggerMode[count] = 2)THEN
						brsmemcpy(ADR(MsgFrame[20]), ADR(PosFactorA[count]), 4);
						brsmemcpy(ADR(MsgFrame[24]), ADR(PosFactorB[count]), 4);
					END_IF;
										
					
				ELSIF((EventID[count] = 7) OR (EventID[count] = 8) OR (EventID[count] = 131))THEN
					MsgFrame[14+(DO_BYTE_SEQUENCE*count)]	:= AxesID[count];
					//MsgFrame[15 + ..] = 0;
					brsmemcpy(ADR(MsgFrame[16]), ADR(ForceThreshold[count]), 4);
			
				END_IF;
			END_FOR;
			
			Busy := TRUE;	
		END_IF;
		//Reset Output
		
	ELSIF(NOT(Execute))THEN
		Busy := FALSE;
		Done := FALSE;
		Error := FALSE;
		ErrorID := 0;
	END_IF
	
	
	
END_FUNCTION_BLOCK
