FUNCTION_BLOCK PMC_ReadXbotForce
	IF Enable = TRUE THEN
		(* Statement section IF*)
		(*read xbot ID*)
		memcpy(pDest := ADR(curXID),pSrc := ADR(FBin_pointer) + P2H_START + P2H_MUX_OFFSET,length := 1);
		IF curXID = XbotID THEN
			(* Statement section IF*)
			(*read X force from frame*)
			memcpy(pDest := ADR(Fx),pSrc := ADR(FBin_pointer) + P2H_START + P2H_MUX_OFFSET + 32,length := 4);
	        
			(*read Y force from frame*)
			memcpy(pDest := ADR(Fy),pSrc := ADR(FBin_pointer) + P2H_START + P2H_MUX_OFFSET + 36,length := 4);
	        
			(*read Z force from frame*)
			memcpy(pDest := ADR(Fz),pSrc := ADR(FBin_pointer) + P2H_START + P2H_MUX_OFFSET + 40,length := 4);
	        
			(*read Rx torque from frame*)
			memcpy(pDest := ADR(Tx),pSrc := ADR(FBin_pointer) + P2H_START + P2H_MUX_OFFSET + 44,length := 4);
	        
			(*read RY torque from frame*)
			memcpy(pDest := ADR(Ty),pSrc := ADR(FBin_pointer) + P2H_START + P2H_MUX_OFFSET + 48,length := 4);
	        
			(*read RZ torque from frame*)
			memcpy(pDest := ADR(Tz),pSrc := ADR(FBin_pointer) + P2H_START + P2H_MUX_OFFSET + 52,length := 4);
			Valid := TRUE;
		ELSE
			Valid := FALSE;
		END_IF;
	    
	ELSE
		Valid := FALSE;
	END_IF;

END_FUNCTION_BLOCK
