
(* Command multiple xbots (1-78) to target positions. Pathing is determined by PMC *)
FUNCTION_BLOCK PMC_AutoDrivingMotion
	(*Determine whether to request an event ID*)
	requestEventID := TRUE;

	PMC_CmdProcessor(Execute, 0,requestEventID, PM_Controller, PMCFuncInfo, Ack,Done, Busy, Error, ErrorID, Aborted);

	IF PMCFuncInfo.SendToPMC THEN
		(*prevent buffer overflow*)
		IF nXbots > maxNXbot THEN
			totalXbotNum := maxNXbot;
		ELSE
			totalXbotNum := nXbots;
		END_IF;
		numFrames := totalXbotNum / maxNXbotPerFrame + 1;
		IF PMCFuncInfo.MsgPartNStatus > 0 THEN          
			(*SendToPMCDataRegion//*)
			(*Sending Command to PMC*)
			(* read current heart beat of field bus*)
			memcpy(pDest := ADR(cmdHB),pSrc := PM_Controller.P2H_START + PM_Controller.PMC_Constants.P2H_CMDC_OFFSET,length := 1);;
			(*update command heart beat*)
			cmdHB := cmdHB + 1;
			cmdHB := cmdHB AND 255;
			(*--------------write command info*)
			(*command ID*)
			memcpy(pDest := PM_Controller.H2P_START + PM_Controller.PMC_Constants.H2P_CMDID_OFFSET,pSrc := ADR(cmdID),length := 2);
			(*command label*)
			memcpy(pDest := PM_Controller.H2P_START + PM_Controller.PMC_Constants.H2P_CMDLB_OFFSET,pSrc := ADR(nXbots),length := 1);
			(*event ID*)
			memcpy(pDest := PM_Controller.H2P_START + PM_Controller.PMC_Constants.H2P_EVENTID_OFFSET,pSrc := ADR(PMCFuncInfo.EventID),length := 2);
			(*Number of xbots total:*)
			memcpy(pDest := PM_Controller.H2P_START + PM_Controller.PMC_Constants.H2P_DATA_OFFSET + 4,pSrc := ADR(totalXbotNum),length := 1);
			(*beginning index for this frame*)
			beginning_index := (DINT_TO_SINT(PMCFuncInfo.MsgPartNStatus - 1) * maxNXbotPerFrame);
			(*Number of xbots to send for this frame*)
			IF (PMCFuncInfo.MsgPartNStatus * maxNXbotPerFrame <= totalXbotNum) THEN
				numXbotsToSend := maxNXbotPerFrame;
			ELSE
				numXbotsToSend := totalXbotNum - (DINT_TO_SINT(PMCFuncInfo.MsgPartNStatus - 1) * maxNXbotPerFrame);
			END_IF;
			memcpy(pDest := PM_Controller.H2P_START + PM_Controller.PMC_Constants.H2P_DATA_OFFSET + 1,pSrc := ADR(numXbotsToSend),length := 1);
			FOR xIndex := 0 TO USINT_TO_SINT(numXbotsToSend - 1) DO
				(* Statement section FOR*)
				(*xbot ID: *)
				memcpy(pDest := PM_Controller.H2P_START + PM_Controller.PMC_Constants.H2P_DATA_OFFSET + 6 + xIndex * 9,pSrc := ADR(XbotID[xIndex + beginning_index]),length := 1);
				(*Pos X*)
				memcpy(pDest := PM_Controller.H2P_START + PM_Controller.PMC_Constants.H2P_DATA_OFFSET + 7 + xIndex * 9,pSrc := ADR(PosX[xIndex + beginning_index]),length := 4);
				(*Pos Y*)
				memcpy(pDest := PM_Controller.H2P_START + PM_Controller.PMC_Constants.H2P_DATA_OFFSET + 11 + xIndex * 9,pSrc := ADR(PosY[xIndex + beginning_index]),length := 4);
			END_FOR;
			(*write command heart beat*)
			memcpy(pDest := PM_Controller.H2P_START + PM_Controller.PMC_Constants.H2P_CMDC_OFFSET,pSrc := ADR(cmdHB),length := 1);
			(*SendToPMCDataRegionEnd//*)
		
			(*Figure out if this is the last frame*)
			IF NOT(PMCFuncInfo.MsgPartNStatus * maxNXbotPerFrame < totalXbotNum) THEN
				PMCFuncInfo.MsgPartNStatus := 0;   (*finished sending, report all done*)
			END_IF;					
		ELSE  (* Statement section ELSE*)
			PMCFuncInfo.MsgPartNStatus := -2;   (*error detected, invalid part*)
		END_IF;
	END_IF;

	IF PMCFuncInfo.ReadFromPMC THEN
		CASE PMCFuncInfo.MsgPartNStatus OF
			1:
				(*ReadFromPMCDataRegion//*)
			
				(*ReadFromPMCDataRegionEnd//*)
				PMCFuncInfo.MsgPartNStatus := 0;  (*finished sending, report all done*)
			ELSE
				PMCFuncInfo.MsgPartNStatus := -2;   (*wrong part number*)
		END_CASE;
	END_IF;

	IF PMCFuncInfo.SendToPMC OR PMCFuncInfo.ReadFromPMC THEN
		PMC_CmdProcessor(Execute, 1,requestEventID, PM_Controller, PMCFuncInfo, Ack,Done, Busy, Error, ErrorID, Aborted);
	END_IF;

	(*timeout check, overrides the cmdprocessor outputs in case it has implementation errors. Hard timeout check*)
	IF Execute = TRUE AND Busy = TRUE THEN
		timeout := timeout + 1;
		IF timeout > PM_Controller.PMC_Constants.PMC_TIMEOUT THEN
			ErrorID := 8201;    (*timeout error id*)
			Error := TRUE;
			Busy := FALSE;
			Aborted := FALSE;
			Ack := FALSE;
			Done := FALSE;
			(*reset the event ID *)
			IF PMCFuncInfo.EventID <> 0 THEN
				PM_Controller.EventMgmt.ExecutingEventID[PMCFuncInfo.EventID] := FALSE;
				PMCFuncInfo.EventID := 0;
			END_IF;  
			(*the command processor will close the ticket,*)
			PMCFuncInfo.CmdSta := 30; (*idle state, and not change any outputs such as ErrorID or Eroor*)
			PMC_CmdProcessor(Execute, 2,requestEventID, PM_Controller, PMCFuncInfo, Ack,Done, Busy, Error, ErrorID, Aborted);
		END_IF;
	ELSE
		timeout := 0;
	END_IF;

	(*if eventID was not requested, then Done shares its value with Ack*)
	IF NOT(requestEventID) THEN
		Done := Ack;
	END_IF;
END_FUNCTION_BLOCK
