
(* 3x3 flyways 6 xbots - star wheel module demo CreateStar, DeleteStar, GetStarStatus, SendXbotToStar, StarExitClear *)
FUNCTION_BLOCK Example33_StarWheel
	(*3x3 flyways 6 xbots - star wheel module demo CreateStar, DeleteStar, GetStarStatus, SendXbotToStar, StarExitClear*)

	(*reset wheel angle*)
	IF(NOT(Setup.Done))THEN
		wheel_angle := 0;
		next_xbot := 1;
	END_IF;

	(*Initial setup*)
	Setup(Execute := Execute,PM_Controller := PM_Controller);

	(*check if xbot is in exit, if it is return it to queue and clear the star wheel's exit*)
	check_wheel_status(Execute := Setup.Done AND NOT(check_wheel_status.Done),WheelID := 1,PM_Controller := PM_Controller); 
	IF(check_wheel_status.Done AND check_wheel_status.ExitState = 1)THEN
		execute_return := TRUE;
	END_IF;
	return_xbot(Execute := execute_return,cmdLB := 0,MacroID := 128,XbotID := check_wheel_status.ExitXID,PM_Controller := PM_Controller);
	clear_exit(Execute := return_xbot.Ack,WheelID := 1,PM_Controller := PM_Controller);
	IF(clear_exit.Done)THEN
		execute_return := FALSE;
	END_IF;

	(*check if next xbot in queue is ready to go, if it is send it to the star wheel*)
	read_xbot_state(Enable := Setup.Done,XbotID := next_xbot,PM_Controller := PM_Controller);
	IF(read_xbot_state.State = 3)THEN
		next_xbot_ready := TRUE;
	END_IF;
	send_xbot_to_wheel(Execute := next_xbot_ready,WheelID := 1,XbotID := next_xbot,PM_Controller := PM_Controller);
	IF(send_xbot_to_wheel.Done)THEN
		next_xbot := next_xbot + 1;
		IF(next_xbot > 6)THEN
			next_xbot := 1;
		END_IF;
		next_xbot_ready := FALSE;
	END_IF;

	(*write wheel angle*)
	write_angle(Enable := TRUE,StmID := 1,AxisID := 6,Pos := wheel_angle,PM_Controller := PM_Controller);
	
	(*increment wheel angle*)
	wheel_angle := wheel_angle + rotation_speed*cycle_time;
	(*2PI wrap around handling*)
	IF(wheel_angle > 2*PI)THEN
		wheel_angle := wheel_angle - 2*PI;
	END_IF;
END_FUNCTION_BLOCK
