
(* Initial setup for Example33_StarWheel *)
FUNCTION_BLOCK Example33_StarWheelSetup
	(*Initial setup for Example33_StarWheel*)

	(*Reset xbot ID*)
	IF(NOT(Execute))THEN
		xbot_id := 1;
	END_IF;

	(*Stop all xbots*)
	PMC_StopXbots1(PM_Controller := PM_Controller,Execute := Execute,XbotID := 0);

	(*Delete all star wheels*)
	PMC_DeleteStarWheel1(PM_Controller := PM_Controller,Execute := PMC_StopXbots1.Done,WheelID := 0);

	(*setup star wheel module*)
	Vials[0] := 0;Vials[1] := PI/2;Vials[2] := PI;Vials[3] := 3*PI/2;
	PMC_CreateStarWheel1(PM_Controller := PM_Controller,Execute := PMC_DeleteStarWheel1.Done,WheelID := 1,Direction := 1,MasterXID := 100,DiscRadius := Disc_Radius,MaxDiscSpeed := 5,MaxXbotAcc := 10,DiscCenterX := Disc_X,DiscCenterY := Disc_Y,StartX := Disc_X + Disc_Radius,StartY := 0.06,EndX := Disc_X - Clearance_Space,EndY := Disc_Y + Disc_Radius,SyncStart := 0,SyncEnd := PI/2,NumVials := 4,VialLocations := Vials);

	(*Setup macro 128*)
	PMC_ClearMacro1(PM_Controller := PM_Controller,Execute := PMC_CreateStarWheel1.Done,MacroID := 128);
	PMC_XYMotion1(PM_Controller := PM_Controller,Execute := PMC_ClearMacro1.Done,cmdLB := 0,XbotID := 128,Mode := 0,Type_ := 0,PosX := 0.18,PosY := Disc_Y + Disc_Radius,EndVel := 1,MaxVel := 1,MaxAcc := 20);
	PMC_ArcMotionTarget1(PM_Controller := PM_Controller,Execute := PMC_XYMotion1.Done,cmdLB := 0,XbotID := 128,Mode := 0,AType := 0,Dir := 1,EndVel := 1,MaxVel := 1,MaxAcc := 20,PosX := 0.06,PosY := PMC_XYMotion1.PosY - 0.12,Radius := 0.12);
	PMC_XYMotion2(PM_Controller := PM_Controller,Execute := PMC_ArcMotionTarget1.Done,cmdLB := 0,XbotID := 128,Mode := 0,Type_ := 0,PosX := 0.06,PosY := 0.18,EndVel := 1,MaxVel := 1,MaxAcc := 20);
	PMC_ArcMotionTarget2(PM_Controller := PM_Controller,Execute := PMC_XYMotion2.Done,cmdLB := 0,XbotID := 128,Mode := 0,AType := 0,Dir := 1,EndVel := 1,MaxVel := 1,MaxAcc := 20,PosX := 0.18,PosY := 0.06,Radius := 0.12);
	PMC_XYMotion3(PM_Controller := PM_Controller,Execute := PMC_ArcMotionTarget2.Done,cmdLB := 0,XbotID := 128,Mode := 0,Type_ := 0,PosX := Disc_X + Disc_Radius,PosY := 0.06,EndVel := 0,MaxVel := 2,MaxAcc := 20);
	PMC_SaveMacro1(PM_Controller := PM_Controller,Execute := PMC_XYMotion3.Done,MacroID := 128);

	(*Move xbots to starting positions*)
	XbotIDs[0] := 1;XbotIDs[1] := 2;XbotIDs[2] := 3;XbotIDs[3] := 4;XbotIDs[4] := 5;XbotIDs[5] := 6;
	PosXs[0] := 0.06;PosXs[1] := 0.06;PosXs[2] := 0.06;PosXs[3] := 0.06;PosXs[4] := 0.06;PosXs[5] := 0.06;
	PosYs[0] := 0.06;PosYs[1] := 0.18;PosYs[2] := 0.30;PosYs[3] := 0.42;PosYs[4] := 0.54;PosYs[5] := 0.66;
	PMC_AutoDrivingMotion1(PM_Controller := PM_Controller,Execute := PMC_SaveMacro1.Done,nXbots := 6,RoutingType := 0,ZoneID := 0,XbotID := XbotIDs,PosX := PosXs,PosY := PosYs);

	(*setup xbot queueing*)
	PMC_XYMotion4(PM_Controller := PM_Controller,Execute := PMC_AutoDrivingMotion1.Done,cmdLB := 0,XbotID := xbot_id,Mode := 0,Type_ := 2,PosX := Disc_X + Disc_Radius,PosY := 0.06,EndVel := 0,MaxVel := 2,MaxAcc := 20);
	IF(PMC_XYMotion4.Ack AND xbot_id < 6)THEN
		PMC_XYMotion4(PM_Controller := PM_Controller,Execute := FALSE);
		xbot_id := xbot_id + 1;
	END_IF;

	(*setup streaming to virtual xbot 100*)
	PMC_StreamModeCtrl1.XbotID[0] := 100;
	PMC_StreamModeCtrl1.StmID[0] := 1;
	PMC_StreamModeCtrl1.StmAxis[0] := 32;
	PMC_StreamModeCtrl1(PM_Controller := PM_Controller,Execute := PMC_XYMotion4.Ack,Level := 1,nXbots := 1);
	Done := PMC_StreamModeCtrl1.Done;
END_FUNCTION_BLOCK
