(*********************************************************************************
* Copyright: Bernecker+Rainer
* Author:    Mikael Regard
* Created:   May 13, 2019/8:21 AM 
* Updated:   Aug 19, 2019/09:00 AM
 *********************************************************************************
 * Description:
 * FuB to read xbot status provided in every frame digital input.
 * 
 *Input:
 * Readframe  (Profinet inputS frame)
 *
 * Output:
 * PMCState (0 = Booting, 1 = Inactive, 2 = Activating, 3,4 and 6 = Reserved, 5 = Operation, 7 = Deactivating, 8 = Error handling, 9 = Error)
 * XbotStatus[0..77] (0 = Undetected, 1 = Discovering, 2 = Landed, 3 = Idling, 4 = Reserved, 5 = Motion, 6 = Wait, 7 = Stopping, 8 = Obstacle,
 * 					  9 = Hold, 10 = Stopped, 11,12,13 = Reserved, 14 = Error, 15 = Uninstalled)
 * DoStatus	[0..127] (Digital output status)
 * XPos[0..77]	(X position, multiplexed)
 * YPos[0..77] 	(Y position, multiplexed)
 * ZPos[0..77]	(Z position, multiplexed) 
 * RXPos[0..77]	(RX position, multiplexed)
 * RYPos[0..77] (RY position, multiplexed)
 * RZPos[0..77] (RZ position, multiplexed)
 * XForce[0..77] (X Force, multiplexed)
 * YForce[0..77] (Y Force, multiplexed)
 * ZForce[0..77] (Z Force, multiplexed)
 * RXForce[0..77] (RX Force, multiplexed)
 * RYForce[0..77] (RY Force, multiplexed)
 * RZForce[0..77]  (RZ Force, multiplexed)
 * XbotID(0..77] (Xbot ID)
 * NumOfXbot 	(Number of xbot on the system)
 * ReplyCmdCount (For logging purposes)
 * ReplyKeyword (Reply command keyword. For logging purposes)
 * ReplyCmdLbl (Reply command label. For logging purposes)
 * ReplyErrCode(Reply error code. For logging purposes)
 *
 *********************************************************************************)






FUNCTION_BLOCK PM_ReadState
	
	//Cmd reply
	ReplyCmdCount 	:= ReadFrame[0];
	brsmemcpy(ADR(ReplyKeyword),ADR(ReadFrame[1]),2);
	brsmemcpy(ADR(ReplyCmdLbl),ADR(ReadFrame[3]),2);
	brsmemcpy(ADR(ReplyErrCode),ADR(ReadFrame[5]),2);
	
	
	//PMC State
	PMCState := ReadFrame[39];
	
	//Xbot State
	index := 0;
	
	NumOfXbot := 0;
	
	FOR count := 0 TO READSTATE_MAX_XBOT BY 2 DO
		XbotStatus[count+1] := SHR(ReadFrame[60+index],4);
		XbotStatus[count] := ReadFrame[60+index] AND 15;
		index := index +1;
		NumOfXbot := NumOfXbot + BOOL_TO_UINT((XbotStatus[count]<STATE_UNINSTALLED) AND (XbotStatus[count]>STATE_DEACTIVATED)) + BOOL_TO_UINT((XbotStatus[count+1]<STATE_UNINSTALLED) AND (XbotStatus[count+1]>STATE_DEACTIVATED));
		
		
	END_FOR;
	
	// Xbot position
	brsmemcpy(ADR(CurrXbotID),ADR(ReadFrame[100]),2);
//	IF(CurrXbotID =1)THEN
//		NumOfXbot := OldXbotID;
//	END_IF;
	
	IF(CurrXbotID <> 0)THEN
		
		
		XbotID[CurrXbotID-1] := CurrXbotID;
		brsmemcpy(ADR(XPos[CurrXbotID-1]),ADR(ReadFrame[104]),4);
		brsmemcpy(ADR(YPos[CurrXbotID-1]),ADR(ReadFrame[108]),4);
		brsmemcpy(ADR(ZPos[CurrXbotID-1]),ADR(ReadFrame[112]),4);
		brsmemcpy(ADR(RXPos[CurrXbotID-1]),ADR(ReadFrame[116]),4);
		brsmemcpy(ADR(RYPos[CurrXbotID-1]),ADR(ReadFrame[120]),4);
		brsmemcpy(ADR(RZPos[CurrXbotID-1]),ADR(ReadFrame[124]),4);
		
		XPos[CurrXbotID-1] 	:= XPos[CurrXbotID-1]*1000.0;
		YPos[CurrXbotID-1] 	:= YPos[CurrXbotID-1]*1000.0;
		ZPos[CurrXbotID-1] 	:= ZPos[CurrXbotID-1]*1000.0;
		RXPos[CurrXbotID-1] := RXPos[CurrXbotID-1]*1000.0;
		RYPos[CurrXbotID-1] := RYPos[CurrXbotID-1]*1000.0;
		RZPos[CurrXbotID-1] := RZPos[CurrXbotID-1]*1000.0;
		//131:128 reserved
		
		brsmemcpy(ADR(XForce[CurrXbotID-1]),ADR(ReadFrame[132]),4);
		brsmemcpy(ADR(YForce[CurrXbotID-1]),ADR(ReadFrame[136]),4);
		brsmemcpy(ADR(ZForce[CurrXbotID-1]),ADR(ReadFrame[140]),4);
		brsmemcpy(ADR(RXForce[CurrXbotID-1]),ADR(ReadFrame[144]),4);
		brsmemcpy(ADR(RYForce[CurrXbotID-1]),ADR(ReadFrame[148]),4);
		brsmemcpy(ADR(RZForce[CurrXbotID-1]),ADR(ReadFrame[152]),4);
		
		//156:159 reserved
	END_IF;
	
	
	//Digital Output
	FOR count := 0 TO READSTATE_MAX_DO_BYTE-1 DO
		DoStatus[8*count]		:= USINT_TO_BOOL(ReadFrame[384+count] AND 1);
		DoStatus[1+8*count]		:= USINT_TO_BOOL(ReadFrame[384+count] AND 2);
		DoStatus[2+8*count]		:= USINT_TO_BOOL(ReadFrame[384+count] AND 4);
		DoStatus[3+8*count]		:= USINT_TO_BOOL(ReadFrame[384+count] AND 8);
		DoStatus[4+8*count]		:= USINT_TO_BOOL(ReadFrame[384+count] AND 16);
		DoStatus[5+8*count]		:= USINT_TO_BOOL(ReadFrame[384+count] AND 32);
		DoStatus[6+8*count]		:= USINT_TO_BOOL(ReadFrame[384+count] AND 64);
		DoStatus[7+8*count]		:= USINT_TO_BOOL(ReadFrame[384+count] AND 128);
	END_FOR;
	
	
	OldXbotID := CurrXbotID;
	
	
END_FUNCTION_BLOCK
