(*********************************************************************************
* Copyright: Bernecker+Rainer
* Author:    Mikael Regard
* Created:   May 13, 2019/8:21 AM 
* Updated:   Aug 19, 2019/09:00 AM
 *********************************************************************************
 * Description:
 * FuB to Read feedback stream. Feedback stream needs to be configured.
 * 
 * MAX_FEEDBACK_AXIS = (FEEDBACK_BYTE_END-FEEDBACK_BYTE_START)/8
 * FEEDBACK_BYTE_END = 384, FEEDBACK_BYTE_START = 160
 * Input:
 * ReadFrame (Profinet reply frame)
 * NumOfStream (Number of Xbots in stream)
 * StreamXbotID (Xbot Stream ID. Starting form 0..9) 
 * FeedbackMode (Feedback Mode (Bitmap) BIT structure [RZ,RY,RX,Z,Y,X])
 * CycleTime (Cycle time for velocity calc)
 * 
 *
 * Output:
 * Error (Indicates if there is an error)
 * Busy  (FuB will be Busy until reply)
 * Done  (When Accepted command is received, Done = TRUE)
 * XbotID[0..MAX_FEEDBACK_AXIS]     (XbotID)
 * XPos[0..MAX_FEEDBACK_AXIS]		(X Position [mm])
 * YPos[0..MAX_FEEDBACK_AXIS]		(Y Position [mm])
 * ZPos[0..MAX_FEEDBACK_AXIS]		(Z Position [mm])
 * RXPos[0..MAX_FEEDBACK_AXIS]		(RX Position [mrad])
 * RYPos[0..MAX_FEEDBACK_AXIS]		(RY Position [mrad])
 * RZPos[0..MAX_FEEDBACK_AXIS]		(RZ Position [mrad])
 * XForce[0..MAX_FEEDBACK_AXIS]		(X Force [N]) 
 * YForce[0..MAX_FEEDBACK_AXIS]		(Y Force [N])
 * ZForce[0..MAX_FEEDBACK_AXIS]		(Z Force [N])
 * RXForce[0..MAX_FEEDBACK_AXIS]	(RX Force [Nm])
 * RYForce[0..MAX_FEEDBACK_AXIS]	(RY Force [Nm])
 * RZForce[0..MAX_FEEDBACK_AXIS]	(RZ Force [Nm])
 * Velocity[0..MAX_FEEDBACK_AXIS]   (X, Y velocity)
 * 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_ReadFeedbackStream
	

	inc := 0; // Increment if variable is read according to the bitmap.	
	
	OldXPos := XPos;
	OldYPos := YPos;
	FOR count := 0 TO NumOfStream BY 1 DO	
		brsmemcpy(ADR(XPos[count]), ADR(ReadFrame[FEEDBACK_BYTE_START+4*inc]),4); 
			FeedbackCheck := USINT_TO_BOOL(FeedbackMode AND 1); // 1 = 000001
			XPos[count] := XPos[count]*1000.0*BOOL_TO_REAL(FeedbackCheck);
			inc := inc  + FeedbackCheck;
			
			brsmemcpy(ADR(YPos[count]), ADR(ReadFrame[FEEDBACK_BYTE_START+4*inc]),4); 
			FeedbackCheck := USINT_TO_BOOL(FeedbackMode AND 2); // 2 = 000010
			YPos[count] := YPos[count]*1000.0*BOOL_TO_REAL(FeedbackCheck);
			inc := inc  + FeedbackCheck;
			
			brsmemcpy(ADR(ZPos[count]), ADR(ReadFrame[FEEDBACK_BYTE_START+4*inc]),4); 
			FeedbackCheck := USINT_TO_BOOL(FeedbackMode AND 4); // 4 = 000100
			ZPos[count] := ZPos[count]*1000.0*BOOL_TO_REAL(FeedbackCheck);
			inc := inc  + FeedbackCheck;
			
			brsmemcpy(ADR(RXPos[count]), ADR(ReadFrame[FEEDBACK_BYTE_START+4*inc]),4); 
			FeedbackCheck := USINT_TO_BOOL(FeedbackMode AND 8); // 8 = 001000
			RXPos[count] := RXPos[count]*1000.0*BOOL_TO_REAL(FeedbackCheck);
			inc := inc  + FeedbackCheck;
			
			brsmemcpy(ADR(RYPos[count]), ADR(ReadFrame[FEEDBACK_BYTE_START+4*inc]),4); 
			FeedbackCheck := USINT_TO_BOOL(FeedbackMode AND 16); // 16 = 010000
			RYPos[count] := RYPos[count]*1000.0*BOOL_TO_REAL(FeedbackCheck);
			inc := inc  + FeedbackCheck;
			
			brsmemcpy(ADR(RZPos[count]), ADR(ReadFrame[FEEDBACK_BYTE_START+4*inc]),4); 
			FeedbackCheck := USINT_TO_BOOL(FeedbackMode AND 32); // 32 = 100000
			RZPos[count] := RZPos[count]*1000.0*BOOL_TO_REAL(FeedbackCheck);
			inc := inc  + FeedbackCheck;	
			Velocity[count] := SQRT(brmpow((XPos[count]-OldXPos[count]),2)+brmpow((YPos[count]-OldYPos[count]),2))/(CycleTime);	
			
			XbotID[count] := StreamXbotID[count];
	END_FOR;
	

	
	// Xbot position
	
	
	
	
	
	
	
END_FUNCTION_BLOCK
