FUNCTION_BLOCK PMC_StartUpPlanarMotorSystem
	CASE CmdSta OF
		(*idling*)
		0:
			(*check if the rising edge of execute*)
			IF NOT(PreExecute_local) AND Execute THEN
				(*reset outputs *)
				Done := FALSE;
				Busy := TRUE;
				Aborted := FALSE;
				Error := FALSE;
				ErrorID := 0;
				NumXbots := 0;
				FOR xbot_loop_index := 0 TO 78 - 1 DO
					Xbot_IDs[xbot_loop_index] := 0;
				END_FOR;
	                    
				(*reset internal state variables    *)
				aborted_local := FALSE;
				busy_local := FALSE;
				done_local := FALSE;
				error_local := FALSE;
				errorID_local := 0;
				execute_local := TRUE;
				ideal_loop_counter := 0;
				pmc_state := 0;
				PreExecute_local := 0;          
				prestep := 0;
				step := 0;
				xbot_loop_index := 0;
				xbot_state := 0;
				timeout_counter := 0;
				(*make sure to reset the internal states of ActivateXbots and LevitationCtrl*)
				PMC_ActivateXbots_db(FBin_pointer := ADR(FBin_pointer), FBout_pointer := ADR(FBout_pointer),Execute := FALSE);
				PMC_LevitationCtrl_db(FBin_pointer := ADR(FBin_pointer), FBout_pointer := ADR(FBout_pointer),Execute := FALSE);
				(*change state to BUSY *)
				CmdSta := 1;
			END_IF;
	        
			(*currently running*)
		1:
			timeout_counter := timeout_counter + 1;
	        
			CASE step OF
				0:  (*activate xbots*)
					PMC_ActivateXbots_db(FBin_pointer := ADR(FBin_pointer), FBout_pointer := ADR(FBout_pointer),Execute := execute_local);
					done_local := PMC_ActivateXbots_db.Done;
					busy_local := PMC_ActivateXbots_db.Busy;
					aborted_local := PMC_ActivateXbots_db.Aborted;
					error_local := PMC_ActivateXbots_db.Error;
					errorID_local := PMC_ActivateXbots_db.ErrorID;
	                
					IF busy_local THEN
						execute_local := FALSE;
					END_IF;
					IF done_local THEN
						execute_local := TRUE; 
						(*if function sent successfully*)
						IF NOT(error_local) AND NOT(aborted_local) THEN
							prestep := step;
							step := 69;
						END_IF;
						(*if function aborted*)
						IF aborted_local THEN
							(*stop execution*)
							CmdSta := 0;
							(*set outputs*)
							Done := TRUE;
							Busy := FALSE;
							Aborted := TRUE;
							Error := FALSE;
							ErrorID := 0;
						END_IF;
						(*if function timed out*)
						IF errorID_local = 8201 THEN
							(*try calling again if startup hasn't timed out else finish *)
							IF timeout_counter > timeout_limit THEN
								(*stop execution*)
								CmdSta := 0;
								(*set outputs*)
								Done := TRUE;
								Busy := FALSE;
								Aborted := FALSE;
								Error := TRUE;
								ErrorID := errorID_local;
							END_IF;
						END_IF;
						(*if function errored out (not timeout)*)
						IF error_local AND NOT(errorID_local = 8201) THEN
							(*stop execution*)
							CmdSta := 0;
							(*set outputs*)
							Done := TRUE;
							Busy := FALSE;
							Aborted := FALSE;
							Error := TRUE;
							ErrorID := errorID_local;
						END_IF;
					END_IF;
	            
				1:  (*levitate all the xbots*)
					PMC_LevitationCtrl_db(FBin_pointer := ADR(FBin_pointer), FBout_pointer := ADR(FBout_pointer),Execute := execute_local,XbotID := 0,Level := 1);
					done_local := PMC_LevitationCtrl_db.Done;
					busy_local := PMC_LevitationCtrl_db.Busy;
					aborted_local := PMC_LevitationCtrl_db.Aborted;
					error_local := PMC_LevitationCtrl_db.Error;
					errorID_local := PMC_LevitationCtrl_db.ErrorID;
	                
					IF busy_local THEN
						execute_local := FALSE;
					END_IF;
					IF done_local THEN
						execute_local := TRUE; 
						(*if function sent successfully*)
						IF NOT(error_local) AND NOT(aborted_local) THEN
							prestep := step;
							step := 69;
						END_IF;
						(*if function aborted*)
						IF aborted_local THEN
							(*stop execution*)
							CmdSta := 0;
							(*set outputs*)
							Done := TRUE;
							Busy := FALSE;
							Aborted := TRUE;
							Error := FALSE;
							ErrorID := 0;
						END_IF;
						(*if function timed out*)
						IF errorID_local = 8201 THEN
							(*try calling again if startup hasn't timed out else finish *)
							IF timeout_counter > timeout_limit THEN
								(*stop execution*)
								CmdSta := 0;
								(*set outputs*)
								Done := TRUE;
								Busy := FALSE;
								Aborted := FALSE;
								Error := TRUE;
								ErrorID := errorID_local;
							END_IF;
						END_IF;
						(*if function errored out (not timeout)*)
						IF error_local AND NOT(errorID_local = 8201) THEN
							(*stop execution*)
							CmdSta := 0;
							(*set outputs*)
							Done := TRUE;
							Busy := FALSE;
							Aborted := FALSE;
							Error := TRUE;
							ErrorID := errorID_local;
						END_IF;
					END_IF;
				2:  (*read all possible xbot statuses   *)
					xbot_array_index := 0;
					NumXbots := 0;
					(*loop through all possible xbot IDs*)
					FOR xbot_loop_index := 0 TO 78 - 1 DO
						PMC_ReadXbotState_db(FBin_pointer := ADR(FBin_pointer), FBout_pointer := ADR(FBout_pointer),Enable := TRUE,XbotID := DINT_TO_USINT(xbot_loop_index + 1));
						done_local := PMC_ReadXbotState_db.Valid;
						xbot_state := PMC_ReadXbotState_db.State;
	                    
						(*if xbot is present at that ID add it to the output array and increment the number of detected xbots*)
						IF (done_local) AND NOT(xbot_state = 15) THEN
							NumXbots := NumXbots + 1;
							Xbot_IDs[xbot_array_index] := DINT_TO_USINT(xbot_loop_index + 1);
							xbot_array_index := xbot_array_index + 1;
						END_IF;                 
					END_FOR;
					(*stop execution*)
					CmdSta := 0;
					(*set other outputs*)
					Done := TRUE;
					Busy := FALSE;
					Aborted := FALSE;
					Error := FALSE;
					ErrorID := 0;
	                        
				69:
					IF ideal_loop_counter < 100 THEN
						ideal_loop_counter := ideal_loop_counter + 1;       
					ELSE
						ideal_loop_counter := 0;                
	
						PMC_ReadPmcState_db(FBin_pointer := ADR(FBin_pointer), FBout_pointer := ADR(FBout_pointer),Enable:=TRUE);
						done_local := PMC_ReadPmcState_db.Valid;
						xbot_state := PMC_ReadPmcState_db.State;
	                    
						(*if pmc is now active move to the next state*)
						IF xbot_state = 5 THEN
							(* Statement section IF*)
							step := prestep + 1;
						END_IF;
					END_IF;
				ELSE  (* Statement section ELSE*)
					;
			END_CASE;
	END_CASE;
	
	(*if timeout expires*)
	IF timeout_counter > timeout_limit THEN
		(*stop execution*)
		CmdSta := 0;
		(*set outputs*)
		Done := TRUE;
		Busy := FALSE;
		Aborted := FALSE;
		Error := TRUE;
		ErrorID := 8201;
	END_IF;
END_FUNCTION_BLOCK
