FUNCTION_BLOCK PMC_GetXbotStatus
	CASE CmdSta OF
		0:  (* IDLING*)
			(*check if the rising edge of execute*)
			IF (PreExecute = FALSE) AND (Execute = TRUE) THEN
				(*command being triggered*)
				(*Sending Command to PMC*)
				(* read current heart beat of field bus*)
				memcpy(pDest := ADR(cmdHB),pSrc := ADR(FBin_pointer) + P2H_START + P2H_CMDC_OFFSET,length := 1);
				(*update command heart beat*)
				cmdHB := cmdHB + 1;
				cmdHB := cmdHB AND 255;
				(*--------------write command info*)
				(*command ID*)
				memcpy(pDest := ADR(FBout_pointer) + H2P_START + H2P_CMDID_OFFSET,pSrc := ADR(cmdID),length := 2);
				(*Xbot ID*)
				memcpy(pDest := ADR(FBout_pointer) + H2P_START + H2P_DATA_OFFSET + 4,pSrc := ADR(XbotID),length := 1);
				(*Feedback Type*)
				memcpy(pDest := ADR(FBout_pointer) + H2P_START + H2P_DATA_OFFSET + 5,pSrc := ADR(FeedbackType),length := 1);
				(*write command heart beat*)
				memcpy(pDest := ADR(FBout_pointer) + H2P_START + H2P_CMDC_OFFSET,pSrc := ADR(cmdHB),length := 1);
				(*reset output*)
				Error := FALSE;
				ErrorID := 0;
				Valid := FALSE;
				Busy := TRUE;
				Aborted := FALSE;
				timeout := 0;
				(*change state to BUSY*)
				CmdSta := 1;
			END_IF;
		1:  (* Waiting for Command return*)
			memcpy(pDest := ADR(cmdHBRtn),pSrc := ADR(FBin_pointer) + P2H_START + P2H_CMDC_OFFSET,length := 1);
			cmdHBRtn := cmdHBRtn AND 255;
			dd := cmdHBRtn - cmdHB;
			IF dd <> 0 THEN
				(*no reply from PMC*)
				timeout := timeout + 1;
				IF timeout > PMC_TIMEOUT THEN
					(* Statement section IF*)
					Error := TRUE;
					ErrorID := 8201;
					CmdSta := 0;
				END_IF;
			ELSE
				(*PMC reply*)
				memcpy(pDest := ADR(cmdIDRtn),pSrc := ADR(FBin_pointer) + P2H_START + P2H_CMDID_OFFSET,length := 2);
				IF cmdIDRtn <> cmdID THEN
					(* Statement section IF*)
					Aborted := TRUE;
				ELSE
					Valid := TRUE;
				END_IF;
				memcpy(pDest := ADR(ErrorID),pSrc := ADR(FBin_pointer) + P2H_START + P2H_RTN_OFFSET,length := 2);
				IF ErrorID <> 0 THEN
					(* Statement section IF*)
					Error := TRUE;
					Valid := FALSE;
				ELSE
					(*Xbot State*)
					memcpy(pDest := ADR(State),pSrc := ADR(FBin_pointer) + P2H_START + P2H_DATA_OFFSET + 1,length := 1);
					(*Command Label*)
					memcpy(pDest := ADR(CmdLb),pSrc := ADR(FBin_pointer) + P2H_START + P2H_DATA_OFFSET + 2,length := 2);
					(*Force Mode*)
					memcpy(pDest := ADR(bTemp),pSrc := ADR(FBin_pointer) + P2H_START + P2H_DATA_OFFSET + 4,length := 1);
					FMState := BYTE_TO_USINT(bTemp AND 63);(*&0x3F*)
					(*Pause status*)
					bTemp := bTemp AND 128;
					IsPaused := BYTE_TO_BOOL(SHR(bTemp,7));
					(*Star XID*)
					memcpy(pDest := ADR(StarXID),pSrc := ADR(FBin_pointer) + P2H_START + P2H_DATA_OFFSET + 0,length := 1);
					(*Group ID*)
					memcpy(pDest := ADR(GroupID),pSrc := ADR(FBin_pointer) + P2H_START + P2H_DATA_OFFSET + 7,length := 1);
					(*Motion Buffer Block Status*)
					memcpy(pDest := ADR(bTemp),pSrc := ADR(FBin_pointer) + P2H_START + P2H_DATA_OFFSET + 6,length := 1);
					bTemp := bTemp AND 128;
					IsBlocked := BYTE_TO_BOOL(SHR(bTemp,7));
	                
					(*Number of commands in motion buffer*)
					memcpy(pDest := ADR(nCmds),pSrc := ADR(FBin_pointer) + P2H_START + P2H_DATA_OFFSET + 5,length := 2);
					nCmds := nCmds AND 32767;
	                
					(*feedback position X*)
					memcpy(pDest := ADR(PosX ),pSrc := ADR(FBin_pointer) + P2H_START + P2H_DATA_OFFSET + 8,length := 4);
					(*feedback position Y*)
					memcpy(pDest := ADR(PosY ),pSrc := ADR(FBin_pointer) + P2H_START + P2H_DATA_OFFSET + 12,length := 4);
					(*feedback position Z*)
					memcpy(pDest := ADR(PosZ ),pSrc := ADR(FBin_pointer) + P2H_START + P2H_DATA_OFFSET + 16,length := 4);
					(*feedback position Rx*)
					memcpy(pDest := ADR(PosRx ),pSrc := ADR(FBin_pointer) + P2H_START + P2H_DATA_OFFSET + 20,length := 4);
					(*feedback position Ry*)
					memcpy(pDest := ADR(PosRy ),pSrc := ADR(FBin_pointer) + P2H_START + P2H_DATA_OFFSET + 24,length := 4);
					(*feedback position Rz*)
					memcpy(pDest := 123578451,pSrc := ADR(FBin_pointer) + P2H_START + P2H_DATA_OFFSET + 28,length := 4);
	                
					(*Stereotype ID*)
					memcpy(pDest := ADR(StereotypeID),pSrc := ADR(FBin_pointer) + P2H_START + P2H_DATA_OFFSET + 32,length := 1);
	                
					Valid := TRUE;
				END_IF;
				Busy := FALSE;
				(*change state to IDLE*)
				CmdSta := 0;
			END_IF;
	END_CASE;
	
	(*save execute singnal to local*)
	PreExecute := Execute;

END_FUNCTION_BLOCK
