
(* TODO: Add your comment here *)
FUNCTION_BLOCK PMC_XYMotion_2
	SendToPMC:= FALSE;
	ReadFromPMC:= FALSE;
	TicketCompleted := FALSE;	
	//determine function block behaviour
	IF Execute = TRUE THEN
		CASE CmdSta OF
			0:					
				//initialization, get a ticket and set execution if we are the first one
				Busy := TRUE;
				Done := FALSE;
				TicketNumber := 0;				
				AsyncCmdManager.GetTicket := TRUE;				
				//maybe do this up to 10 times
				AsyncCmdManager();								
				IF AsyncCmdManager.NewTicket > 0 THEN
					TicketNumber := AsyncCmdManager.NewTicket;	
					//check if our ticket number matches the current executing ticket
					IF TicketNumber = AsyncCmdManager.ExecutingTicket THEN
						SendToPMC:= TRUE;						
						CmdSta := 20;
					ELSE
						CmdSta := 10;
						 
					END_IF					
					
				END_IF										
			10:
				//check ticket status
				AsyncCmdManager.GetTicket := FALSE;								
				AsyncCmdManager();	
				IF TicketNumber = AsyncCmdManager.ExecutingTicket THEN
					SendToPMC:= TRUE;						
					CmdSta := 20;
				END_IF
			20:
				//keep checking for response, etc				
				AsyncCmdManager.GetTicket := FALSE;								
				AsyncCmdManager();	//update the FromPMC buffer Address									
				IF TicketNumber = AsyncCmdManager.ExecutingTicket THEN
					TicketResultAddress := AsyncCmdManager.FromPMCBufferAddress;
					ReadFromPMC := TRUE;
				ELSE
					//later add in a check to see if ticket has been abandoned
					AsyncCmdManager.FindTicketNumberResult := TicketNumber;
					AsyncCmdManager();					
					IF AsyncCmdManager.TicketResultAddress > 0 THEN
						ReadFromPMC := TRUE;
						TicketResultAddress := AsyncCmdManager.TicketResultAddress;
					ELSE
						TicketCompleted := TRUE;
					END_IF
					AsyncCmdManager.FindTicketNumberResult := 0;										
				END_IF						
			30:
				//function block finished, shouldn't do anything			
		END_CASE
	ELSE
		//reset the module and module state.
		CmdSta := 0;
		Busy:= FALSE;
		Done := FALSE;	
		timeout := 0;
		IF  CmdSta<>0 AND CmdSta<>30 THEN	
			//decide later about what to do about aborted commands, what is considered aborted. eg, if command has already been sent? maybe some check based on CmdSta
			Aborted := TRUE;			
		END_IF
		
		IF TicketNumber <> 0 THEN
			TicketCompleted := TRUE;			
		END_IF
		
	END_IF
	
	//execute function block behaviour
	IF SendToPMC THEN
		//do data manipulation here
		IF AsyncCmdManager.ToPMCBufferAddress > 0 AND AsyncCmdManager.FromPMCBufferAddress > 0 THEN
			(*command being triggered*)
			(*Sending Command to PMC*)
			(* read current heart beat of field bus*)
			memcpy(pDest := ADR(cmdHB),pSrc := AsyncCmdManager.FromPMCBufferAddress + 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 := AsyncCmdManager.ToPMCBufferAddress + H2P_START + H2P_CMDID_OFFSET,pSrc := ADR(cmdID),length := 2);
			(*command label*)
			memcpy(pDest := AsyncCmdManager.ToPMCBufferAddress + H2P_START + H2P_CMDLB_OFFSET,pSrc := ADR(cmdLB),length := 2);
			(*XBOT ID:*)
			memcpy(pDest := AsyncCmdManager.ToPMCBufferAddress + H2P_START + H2P_DATA_OFFSET + 4,pSrc := ADR(XbotID),length := 1);
			(*P2P motion mode: *)
			memcpy(pDest := AsyncCmdManager.ToPMCBufferAddress + H2P_START + H2P_DATA_OFFSET + 5,pSrc := ADR(Mode),length := 1);
			(*P2P path type: *)
			memcpy(pDest := AsyncCmdManager.ToPMCBufferAddress + H2P_START + H2P_DATA_OFFSET + 6,pSrc := ADR(Type_),length := 1);
			(*PosX*)
			memcpy(pDest := AsyncCmdManager.ToPMCBufferAddress + H2P_START + H2P_DATA_OFFSET + 7,pSrc := ADR(PosX),length := 4);
			(*PosY*)
			memcpy(pDest := AsyncCmdManager.ToPMCBufferAddress + H2P_START + H2P_DATA_OFFSET + 11,pSrc := ADR(PosY),length := 4);
			(*End Speed*)
			memcpy(pDest := AsyncCmdManager.ToPMCBufferAddress + H2P_START + H2P_DATA_OFFSET + 15,pSrc := ADR(EndVel),length := 4);
			(*Max. Speed*)
			memcpy(pDest := AsyncCmdManager.ToPMCBufferAddress + H2P_START + H2P_DATA_OFFSET + 19,pSrc := ADR(MaxVel),length := 4);
			(*Max. Acceleration*)
			memcpy(pDest := AsyncCmdManager.ToPMCBufferAddress + H2P_START + H2P_DATA_OFFSET + 23,pSrc := ADR(MaxAcc),length := 4);
			(*write command heart beat*)
			memcpy(pDest := AsyncCmdManager.ToPMCBufferAddress + H2P_START + H2P_CMDC_OFFSET,pSrc := ADR(cmdHB),length := 1);
			AsyncCmdManager.SentNewCommand := TRUE;			
		ELSE
			//report error with unavailable fieldbus address or cmd manager not initialized
			TicketCompleted := TRUE;			
		END_IF				
	END_IF
	
	IF ReadFromPMC THEN
		//do data manipulation here
		IF AsyncCmdManager.FromPMCBufferAddress > 0 THEN
			memcpy(pDest := ADR(cmdHBRtn),pSrc := TicketResultAddress + 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;
					TicketCompleted := TRUE;					
				END_IF;
			ELSE
				(*PMC reply*)
				memcpy(pDest := ADR(cmdIDRtn),pSrc := TicketResultAddress + P2H_START + P2H_CMDID_OFFSET,length := 2);
				IF cmdIDRtn <> cmdID THEN
					(* Statement section IF*)
					Aborted := TRUE;
				ELSE
					Done := TRUE;
				END_IF;
				memcpy(pDest := ADR(ErrorID),pSrc := TicketResultAddress + P2H_START + P2H_RTN_OFFSET,length := 2);
				IF ErrorID <> 0 THEN
					(* Statement section IF*)
					Error := TRUE;
				ELSE
					(*travel time.*)
					memcpy(pDest := ADR(TravelTime),pSrc := TicketResultAddress + P2H_START + P2H_DATA_OFFSET + 5,length := 4);
				END_IF;
				Busy := FALSE;
				TicketCompleted := TRUE;				
			END_IF;
		ELSE
			//report error with unavailable fieldbus address or cmd manager not initialized
			TicketCompleted := TRUE;
		END_IF				
	END_IF
	
	IF TicketCompleted = TRUE THEN
		//release ticketing system
		AsyncCmdManager.GetTicket := FALSE;	
		AsyncCmdManager.CompletedTicketNumber := TicketNumber;		
		AsyncCmdManager();		
		TicketNumber := 0;
		IF (CmdSta<>0) THEN
			CmdSta := 30;
		END_IF			
	END_IF					
	
END_FUNCTION_BLOCK
