(*********************************************************************************
* Copyright: Bernecker+Rainer
* Author:    Mikael Regard
* Created:   May 13, 2019/8:21 AM 
* Updated:   Aug 19, 2019/09:00 AM
 *********************************************************************************
 * Description:
 * FuB to move Xbot arc motion.
 * 
 *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. 
 * Xbot ID      	(Xbot to Move. XbotID > 0. Can only move 1 at the time. Virtual Axis ID 100-127, Macro ID 128-191
 * ArcMode      	(0 = Target position and arc radius mode, 1 = Arc center and rotation angle (|rad|) mode)
 * TargetX      	(ArcMode = 0: Target X position [mm], ArcMode = 1 Center X position [mm])
 * TargetY      	(ArcMode = 0: Target Y position [mm], ArcMode = 1 Center Y position [mm])
 * Radius       	(ArcMode = 0: Radius = radius [m], ArcMode = 1, Radius = Angle(rad))
 * MaxTanSpeed    	(Maximum allowed speed. Will be automatically adjusted if they are below or above the configured system limit settings)
 * MaxTanAccel    	(Maximum allowed acceleration. Will be automatically adjusted if they are below or above the configured system limit settings)
 * FinalTanSpeed 	(Final speed = 0, Xbot will stop at the target position. Final speed > 0 can be used to connect multiple motion commands seamlessly.
 * PositionMode    	(0 = Absolute position, 1 = Relative position)
 * Direction		(0 = Clockwise, 1 = Counter clockwise)
 * ArcType     		(0 = Minor Arc (shortest path arc) 1 = Major arc (longest arc path)
 * 
 *
 * Output:
 * Error (Indicates if there is an error)
 * Busy  (FuB will be Busy until reply)
 * Done  (When Accepted command is received, Done = TRUE)
 * TravelTime (Expected travel time)
 * 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_ArcMotion
	// Check Reply
	IF(Busy)THEN
		brsmemcpy(ADR(CmdLbl),ADR(ReadFrame[3]),2);
		IF(ReadFrame[0] = CmdCount AND
			ReadFrame[1] 	= 10  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));
			brsmemcpy(ADR(TravelTime),ADR(ReadFrame[12]),4);
		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] 	:= 10;
			MsgFrame[2] 	:= 11;
			brsmemcpy(ADR(MsgFrame[3]), ADR(CmdLabel), 2);
			MsgFrame[5]		:= 1;	
			MsgFrame[8]		:= PositionMode;
			MsgFrame[9] 	:= XbotID;
			MsgFrame[10] 	:= ArcMode;
			MsgFrame[11] 	:= Direction;
			Xmm2m 			:= TargetX/1000.0;
			Ymm2m 			:= TargetY/1000.0;
			Radmm2m			:= Radius/1000.0;
			
			brsmemcpy(ADR(MsgFrame[12]), ADR(FinalTanSpeed), 4);
			brsmemcpy(ADR(MsgFrame[16]), ADR(MaxTanSpeed), 4);
			brsmemcpy(ADR(MsgFrame[20]), ADR(MaxAccel), 4);
			brsmemcpy(ADR(MsgFrame[24]), ADR(Xmm2m), 4);
			brsmemcpy(ADR(MsgFrame[28]), ADR(Ymm2m), 4);
			IF(ArcMode <> 0)THEN
				brsmemcpy(ADR(MsgFrame[32]), ADR(Radius), 4);
			ELSE
				brsmemcpy(ADR(MsgFrame[32]), ADR(Radmm2m), 4);
			END_IF;
			
				MsgFrame[36]	:= ArcType;
			
			
			
			
			Busy 			:= TRUE;	
		END_IF;
	//Reset output status	
	ELSIF(NOT(Execute))THEN
		Busy 	:= FALSE;
		Done 	:= FALSE;
		Error 	:= FALSE;
		ErrorID := 0;
	END_IF
	
	
	
END_FUNCTION_BLOCK
