(*********************************************************************************
* Copyright: Bernecker+Rainer
* Author:    Mikael Regard
* Created:   May 13, 2019/8:21 AM 
* Updated:   Aug 19, 2019/09:00 AM
 *********************************************************************************
 * Description:
 * FuB to Hold Xbot: Wait Until event/time.
 * 5 different waiting mode
 * 1. Time Delay - Wait a specified amount of time. Min 0.001s.
 * 2. External digital input - Wait until External digital input is triggered. Rising edge, falling edge, equal to 1, equal to 0 possible.
 * 3. Fieldbus digital input - Wait unitl fieldbus digital input is triggered. Rising edge, falling edge, equal to 1, equal to 0 possible.
 * 4. Command label - Wait until a certain command label. Start of monitored cmd, end of monitored cmd, or any time during monitored cmd execution.
 * 5. Displacement  - Wait until position of a Xbot ID is met. Specify threshold, mode (Only X, Only Y, X and Y). Less than, More than, equal. 
 *
 *
 *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)
 * XbotID    (Xbot to Move. XbotID > 0. Can only move 1 at the time. Virtual Axis ID 100-127, Macro ID 128-191)
 * DigitalID   (MacroID to assignMacro ID 128-191)
 * PosFactorA (Position Factor A in AX+BY. Ignored by PMC if WaitMode <> 2 or 4)
 * PosFactorA (Position Factor B in AX+BY. Ignored by PMC if WaitMode <> 2 or 4)
 * DisplacementTrigger (Displacement trigger treshold [mm], ignored by PMC if WaitMode <> 4)
 * TriggerCmdLabel (Trigger command label)
 * TriggerMode (0 = X Only, 1 = Y Only, 2 = AX+BY)
 * TimeDelay (WaitMode = 0, Time delay [ms])
 * TriggerType (WaitMode = 1,2  {0 = Rising Edge, 1 = Falling Edge, 2 = Equal to 1, 3 = Equal to 0}, WaitMode = 3 {0 = Staring Execution, 1= Finishing Execution, 2 0 During execution}, WaitMode = 4 { 0 = > threshold, 1 = < threhold, 2 = Rising above threshold, 3 = Falling below threshold})
 * TriggerXbotID (XbotID for trigger)
 * WaitMode (0 = Time Delay mode, 1 = External Digital input mode, 2 = Fieldbus digital input mode, 3 = Command label mode, 4 = displacement mode)
 *
 * 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_WaitUntil		
	// Check Reply
	IF(Busy)THEN
		brsmemcpy(ADR(CmdLbl),ADR(ReadFrame[3]),2);
		IF(ReadFrame[0] = CmdCount AND
			ReadFrame[1] 	= 25  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);
			CmdCount 		:= ReadFrame[0] +1;
			MsgFrame[0] 	:= CmdCount; 
			MsgFrame[1] 	:= 25;
			MsgFrame[2] 	:= 11;
			brsmemcpy(ADR(MsgFrame[3]), ADR(CmdLabel), 2);
			MsgFrame[5]		:= 1;
			MsgFrame[9]		:= XbotID;
			MsgFrame[11]	:= 1;
			
			MsgFrame[10]	:= WaitMode;
			
			IF(WaitMode = 0)THEN
				TimeDelayms2s	:= TimeDelay/1000.0;
				brsmemcpy(ADR(MsgFrame[12]), ADR(TimeDelayms2s), 4);
			
			ELSIF((WaitMode = 1) OR (WaitMode = 2))THEN
				MsgFrame[12] := DigitalID;
				MsgFrame[13] := TriggerType;
				MsgFrame[14] := 0;
				MsgFrame[15] := 0;	
			ELSIF(WaitMode = 3)THEN
				MsgFrame[12] := TriggerXbotID;
				MsgFrame[13] := TriggerType;
				brsmemcpy(ADR(MsgFrame[14]), ADR(TriggerCmdLabel), 2);
			ELSIF(WaitMode = 4)THEN
				MsgFrame[12] := TriggerXbotID;
				MsgFrame[13] := TriggerMode;
				MsgFrame[14] := TriggerType;
				MsgFrame[15] := 0;
				Xmm2m 			:= DisplacementTrigger/1000.0;
				brsmemcpy(ADR(MsgFrame[16]), ADR(Xmm2m), 4);
				brsmemcpy(ADR(MsgFrame[20]), ADR(PosFactorA), 4);
				brsmemcpy(ADR(MsgFrame[24]), ADR(PosFactorB), 4);
			END_IF;	
			Busy := TRUE;	
		END_IF;
		//Reset Output
		
	ELSIF(NOT(Execute))THEN
		Busy := FALSE;
		Done := FALSE;
		Error := FALSE;
		ErrorID := 0;
	END_IF
	
	
	
END_FUNCTION_BLOCK
