(*********************************************************************************
* Copyright: Bernecker+Rainer
* Author:    Mikael Regard
* Created:   May 13, 2019/8:21 AM 
* Updated:   Aug 19, 2019/09:00 AM
 *********************************************************************************
 * Description:
 * FuB to Set mover properties.
 * 
 *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.
 * NumOfSettings (Number of Xbots to configure)
 * XbotID[0..13] (XbotID)
 * PropertyValue (Property value, depending on the PropertyID)
 * PropertyID	(Property ID: 0 = Mover Type, 1 = User configured payload, 2 = User Configured CG in Z, 3 = Mover X Dimension, 4 = Reserved, 5 = Mover Y Dimension, 6  = Reserved, 7 = Acceleration Limit)
 * 
 * 
 * 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_SetMoverProperties		
	// Check Reply
	IF(Busy)THEN
		brsmemcpy(ADR(CmdLbl),ADR(ReadFrame[3]),2);
		IF(ReadFrame[0] = CmdCount AND
			ReadFrame[1] 	= 29  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] 	:= 29;
			MsgFrame[2] 	:= 11;
			brsmemcpy(ADR(MsgFrame[3]), ADR(CmdLabel), 2);
			MsgFrame[5]		:= 1;
			MsgFrame[10]	:= NumOfSettings;
		
			FOR count := 0 TO NumOfSettings DO
				MsgFrame[11+(WRITESETTING_BYTE_SEQUNCE*count)] := XbotID[count];
				MsgFrame[12+(WRITESETTING_BYTE_SEQUNCE*count)] := PropertyID[count];
				//Convert mm to m
				IF((PropertyID[count]>0) AND (PropertyID[count]<7))THEN
					PropertyValmm2m[count]	:= PropertyValue[count]/1000.0;
					brsmemcpy(ADR(MsgFrame[13+(WRITESETTING_BYTE_SEQUNCE*count)]),ADR(PropertyValmm2m[count]),4);
				ELSE
					brsmemcpy(ADR(MsgFrame[13+(WRITESETTING_BYTE_SEQUNCE*count)]),ADR(PropertyValue[count]),4);
				END_IF;
			END_FOR;
			Busy := TRUE;	
		END_IF;
		
	//Reset output status	
	ELSIF(NOT(Execute))THEN
		Busy := FALSE;
		Done := FALSE;
		Error := FALSE;
		ErrorID := 0;
	END_IF
	
	
	
END_FUNCTION_BLOCK
