(*********************************************************************************
* Copyright: Bernecker+Rainer
* Author:    Mikael Regard
* Created:   Sep 25, 2019/8:21 AM 
 *********************************************************************************
 * Description:

 * 
 *Input:

 *
 *
 * Output:

 *********************************************************************************)


FUNCTION_BLOCK PM_AsyncMotion_16
	
	StandStill := TRUE;
	FOR i := 0 TO USINT_TO_INT(NumOfXbot) DO
		IF(PM_ReadState_0.XbotStatus[XbotID[i]] = STATE_MOTION)THEN
			StandStill := FALSE;
		END_IF;
	END_FOR;
	
	TON_0.PT := T#3s;
	IF(NOT(Execute) AND Busy)THEN
		Async16State := ASYNC16_WAIT;
		brsmemset(ADR(Side[0].XInit),0,8*4);
		brsmemset(ADR(Side[1].XInit),0,8*4);
		brsmemset(ADR(Side[0].YInit),0,8*4);
		brsmemset(ADR(Side[1].YInit),0,8*4);
		brsmemset(ADR(Side[0].InitID),0,8);
		brsmemset(ADR(Side[1].InitID),0,8);
		Side[0].NumOfXbot := 0;
		Side[1].NumOfXbot := 0;
		Side[0].InPosition := 0;
		Side[1].InPosition := 0;
		PM_Shuffle_0.Execute := FALSE;
		PM_AsyncMotion_0.Execute := FALSE;
		PM_Shuffle_0();
	END_IF;
	
	CASE Async16State OF
		
		ASYNC16_WAIT:
			IF(Execute)THEN
				Async16State := ASYNC16_CHECK_TARGET_POS;
			END_IF;
						
		ASYNC16_CHECK_TARGET_POS:
			//			FOR i := 0 TO NumOfXbot-1 DO
			//				IF(XTarget[i] < 60)THEN
			//					Async16State := ASYNC16_ERROR;
			//					ErrorID := 1;
			//					EXIT;
			//				ELSIF(YTarget[i] <60)THEN
			//					Async16State := ASYNC16_ERROR;
			//					ErrorID := 1;
			//					EXIT;
			//				ELSIF(XbotID[i] = 0)THEN
			//					Async16State := ASYNC16_ERROR;
			//					ErrorID := 1;
			//					EXIT;
			//				END_IF;
			//				//				FOR k := 0 TO NumOfXbot-1 DO   // Check invalid target position
			//				//					IF((ABS(XTarget[i]-XTarget[k])+YTarget[i]
			//			END_FOR
			
			SortedXPosition := XTarget;
			SortedYPosition := YTarget;
			SortedXbotID 	:= XbotID;
			
			
			IF(Async16State <> ASYNC16_ERROR)THEN	
				FOR i := NumOfXbot TO ASYNC16_NUM_XBOT DO
					SortedXPosition[i] 	:= 10000*i;				// Simplify Sorting by assigning an incrementing large value in the end.
					SortedYPosition[i]	:= 10000*i;
					SortedXbotID[i]   	:= INT_TO_USINT(150+i);
				END_FOR;
				
				IF(NumOfXbot >8)THEN
					Async16State := ASYNC16_SORT_X_POS;
				ELSE
					Async16State := ASYNC16_MOVE_TARGET_POS;
				END_IF;
			END_IF;
		
		ASYNC16_SORT_X_POS:
			FOR i := 0 TO USINT_TO_INT(NumOfXbot)-2 DO
				swapped := FALSE;
				FOR k := 0 TO USINT_TO_INT(NumOfXbot)-2 DO
					IF(SortedXPosition[k]>SortedXPosition[k+1])THEN
						temp := SortedXPosition[k];
						SortedXPosition[k] := SortedXPosition[k+1];
						SortedXPosition[k+1] := temp;					
						//						temp := SortedYPosition[k];
						//						SortedYPosition[k] := SortedYPosition[k+1];
						//						SortedYPosition[k+1] := temp;	
						//						temp := SortedXbotID[k];
						//						SortedXbotID[k] := SortedXbotID[k+1];
						//						SortedXbotID[k+1] := REAL_TO_USINT(temp);
						swapped := TRUE;
					END_IF;
				END_FOR;
				IF(NOT(swapped))THEN
					EXIT;
				END_IF;
			END_FOR;					
			Async16State := ASYNC16_SORT_Y_POS;
						
		ASYNC16_SORT_Y_POS:
			FOR i := 0 TO USINT_TO_INT(NumOfXbot)-2 DO
				swapped := FALSE;
				FOR k := 0 TO USINT_TO_INT(NumOfXbot)-2 DO
					IF((SortedYPosition[k]>SortedYPosition[k+1]))THEN
						temp := SortedYPosition[k];
						SortedYPosition[k] := SortedYPosition[k+1];
						SortedYPosition[k+1] := temp;	
						//						temp := SortedXbotID[k];
						//						SortedXbotID[k] := SortedXbotID[k+1];
						//						SortedXbotID[k+1] := REAL_TO_USINT(temp);
						swapped := TRUE;
					END_IF;
				END_FOR;
				IF(NOT(swapped))THEN
					EXIT;
				END_IF;
			END_FOR;
			Async16State := ASYNC16_FIND_LIMIT;
			LimitState := 0;				
		
		ASYNC16_FIND_LIMIT:		
			CASE LimitState OF
				0: // FIND X limit
					FOR i := 0 TO USINT_TO_INT(NumOfXbot)-1 DO
						Side[0].NumOfXbot := 0;
						Side[1].NumOfXbot := 0;
						//brsmemset(ADR(Side[0].InitID),0,8);
						//brsmemset(ADR(Side[1].InitID),0,8);
						XLimit := SortedXPosition[i]+60;
						FOR k := 0 TO USINT_TO_INT(NumOfXbot)-1 DO
							IF(XLimit>=XTarget[k])THEN
								Side[0].InitID[Side[0].NumOfXbot] := XbotID[k];	
								Side[0].NumOfXbot := Side[0].NumOfXbot+1;
							ELSE
								Side[1].InitID[Side[1].NumOfXbot] := XbotID[k];	
								Side[1].NumOfXbot := Side[1].NumOfXbot+1;				
							END_IF;
							
							//IF(Side[0].NumOfXbot = ASYNC16_NUM_XBOT_SIDE  OR Side[1].NumOfXbot = ASYNC16_NUM_XBOT_SIDE)THEN
							//	EXIT;
							//END_IF;
						END_FOR;
						
						// Check result.
						// exactly 8 shuttles found behind target line
						IF(Side[0].NumOfXbot = ASYNC16_NUM_XBOT_SIDE+1)THEN
							LimitAxis := 0;
							Async16State := ASYNC16_FIND_INIT_POS_0;
							EXIT;
							// Smaller than 8 shuttles found BUT remaining shuttles fits into 1 cmd
							// Example 2 shuttles found, total 10 shuttles.	
						ELSIF((Side[0].NumOfXbot < (ASYNC16_NUM_XBOT_SIDE+1)) AND (Side[1].NumOfXbot<=(ASYNC16_NUM_XBOT_SIDE+1)))THEN
							LimitAxis := 0;
							Async16State := ASYNC16_FIND_INIT_POS_0;
							EXIT;
						ELSE
							LimitState := 1;			
						END_IF;				
					END_FOR;
				
				1: //Find Y limit,
					FOR i := 0 TO USINT_TO_INT(NumOfXbot)-1 DO
						YLimit := SortedYPosition[i]+60;
						Side[0].NumOfXbot := 0;
						Side[1].NumOfXbot := 0;
						//brsmemset(ADR(Side[0].InitID),0,8);
						//brsmemset(ADR(Side[1].InitID),0,8);
						FOR k := 0 TO UINT_TO_INT(NumOfXbot)-1 DO
							IF(YLimit>=YTarget[k])THEN
								Side[0].InitID[Side[0].NumOfXbot] := XbotID[k];							
								Side[0].NumOfXbot := Side[0].NumOfXbot+1;
							ELSE
								Side[1].InitID[Side[1].NumOfXbot] := XbotID[k];
								Side[1].NumOfXbot := Side[1].NumOfXbot+1;		
							END_IF;
							
							//IF(Side[0].NumOfXbot = ASYNC16_NUM_XBOT_SIDE  OR Side[1].NumOfXbot = ASYNC16_NUM_XBOT_SIDE)THEN
							//	EXIT;
							//END_IF;
						END_FOR;
						// Check result.
						// exactly 8 shuttles found behind target line
						IF(Side[0].NumOfXbot = (ASYNC16_NUM_XBOT_SIDE+1))THEN
							LimitAxis := 1;
							Async16State := ASYNC16_FIND_INIT_POS_0;
							EXIT;
							// Smaller than 8 shuttles found BUT remaining shuttles fits into 1 cmd
							// Example 2 shuttles found, total 10 shuttles.	
						ELSIF((Side[0].NumOfXbot < (ASYNC16_NUM_XBOT_SIDE+1)) AND (Side[1].NumOfXbot<= (ASYNC16_NUM_XBOT_SIDE+1)))THEN
							LimitAxis := 1;
							Async16State := ASYNC16_FIND_INIT_POS_0;
							EXIT;
						ELSE
							LimitState := 2;			
						END_IF;				
					END_FOR;		
									
				2: // No limit found - Change target position.
					Async16State := ASYNC16_ERROR;
					ErrorID := 2;				
			END_CASE;
			
			Index 			:= 0;
			incMain			:= 0;
			incSlave 		:= 0;
			
			
		ASYNC16_FIND_INIT_POS_0:

			IF((LimitAxis = 0))THEN // X is the limit
				IF((REAL_TO_INT(incSlave) MOD 2) = 0)THEN 
					InitXParam := 85 + 135*incMain; // Decrease X init param
				ELSE
					InitXParam := 60 + 135*incMain;
				END_IF;
				
					
					InitYParam := 60 + 120*incSlave;
				IF(InitXParam>=(XLimit-60))THEN // check above limit						
					Async16State := ASYNC16_ERROR;
					ErrorID := 44;
				END_IF;
			ELSIF((LimitAxis = 1))THEN // Y is the limit
				InitYParam := 60 + 120*incMain;
				InitXParam := 60 + 120*incSlave;
				IF(InitYParam>=(YLimit-60))THEN
					Async16State := ASYNC16_FIND_INIT_POS_1;
					ErrorID := 45;
				END_IF;
			END_IF;
			
			InitParOK := TRUE;
			FOR h := 0 TO 7 DO // Check for alredy taken positions.
				IF(InitXParam = Side[0].XInit[h] AND
					InitYParam = Side[0].YInit[h])THEN
					InitParOK := FALSE;
				END_IF;
			END_FOR;
				
									
			IF(InitParOK)THEN
				FOR j := 0 TO UINT_TO_INT(NumOfXbot)-1 DO
					IF((TRUNC(InitXParam/240.0) 	= TRUNC(XTarget[j]/240.0)) AND
						(TRUNC(InitYParam/240.0) 	= TRUNC(YTarget[j]/240.0)))THEN															
						Side[0].XInit[Index] := InitXParam;
						Side[0].YInit[Index] := InitYParam;
						//Side[1].InitID[Index] := XbotID[j];
						Index := Index+1;
						EXIT;
					END_IF;
				END_FOR;
			END_IF;
			IF(Index >= UINT_TO_INT(Side[0].NumOfXbot))THEN
				Async16State := ASYNC16_FIND_INIT_POS_1;
				Index := 0;
				incMain := 0;
				incSlave := 0;
			ELSE
				incSlave := incSlave+1;
				IF(incSlave = USINT_TO_REAL(NumOfXbot))THEN
					incMain := incMain+1;
					incSlave := 0;				
				END_IF;
			END_IF;
	
		ASYNC16_FIND_INIT_POS_1:
											
			IF((LimitAxis = 0))THEN // X is the limit
				IF((REAL_TO_INT(incSlave) MOD 2) = 0)THEN 
					InitXParam := SortedXPosition[NumOfXbot-1]-(135*incMain+25); // Decrease X init param
				ELSE
					InitXParam := SortedXPosition[NumOfXbot-1]-135*incMain;
				END_IF;				
				InitYParam := 60 + 120*incSlave;
				IF(InitXParam<=(XLimit+60))THEN // check above limit						
					Async16State := ASYNC16_ERROR;
					ErrorID := 55;
				END_IF;			
			ELSIF((LimitAxis = 1))THEN
				InitYParam := SortedYPosition[NumOfXbot-1]-120*incMain;
				InitXParam := 60 + 120*incSlave;
				IF(InitYParam<=(YLimit+60))THEN
					Async16State := ASYNC16_ERROR;
					ErrorID := 56;
				END_IF;
			END_IF;
			
			InitParOK := TRUE;
			FOR h := 0 TO 7 DO // Check for alredy taken positions.
				IF(InitXParam = Side[1].XInit[h] AND
					InitYParam = Side[1].YInit[h])THEN
					InitParOK := FALSE;
				END_IF;
			END_FOR;
				
									
			IF(InitParOK)THEN
				FOR j := 0 TO USINT_TO_INT(NumOfXbot)-1 DO
					IF((TRUNC(InitXParam/240.0) 	= TRUNC(XTarget[j]/240.0)) AND
						(TRUNC(InitYParam/240.0) 	= TRUNC(YTarget[j]/240.0)))THEN															
						Side[1].XInit[Index] := InitXParam;
						Side[1].YInit[Index] := InitYParam;
						//Side[1].InitID[Index] := XbotID[j];
						Index := Index+1;
						EXIT;
					END_IF;
				END_FOR;
			END_IF;
			IF(Index >= USINT_TO_INT(Side[1].NumOfXbot))THEN
				Async16State := ASYNC16_SHUFFLE;
				Index := 0;
				incMain := 0;
				incSlave := 0;
			ELSE
				incSlave := incSlave+1;
				IF(incSlave = USINT_TO_INT(NumOfXbot))THEN
					incMain := incMain+1;
					incSlave := 0;
				END_IF;
			END_IF;					
								
		ASYNC16_COUNT_MISPLACED_SHUTTLES:
			
			Side[0].NumOfLostXbot := 0;	
			Side[1].NumOfLostXbot := 0;		
			brsmemset(ADR(Side[0].XbotList),0,8);
			brsmemset(ADR(Side[1].XbotList),0,8);
			
			FOR i := 0 TO USINT_TO_INT(NumOfXbot)-1 DO
				IF(LimitAxis = 1)THEN
					IF((PM_ReadState_0.YPos[XbotID[i]-1]>YLimit) AND (YTarget[i]<YLimit))THEN
						Side[0].XbotList[Side[0].NumOfLostXbot] := XbotID[i];
						Side[0].NumOfLostXbot := Side[0].NumOfLostXbot+1;
					ELSIF((PM_ReadState_0.YPos[XbotID[i]-1]<=YLimit) AND (YTarget[i]>=YLimit))THEN
						Side[1].XbotList[Side[1].NumOfLostXbot] := XbotID[i];
						Side[1].NumOfLostXbot := Side[1].NumOfLostXbot+1;
					END_IF;	
				ELSIF(LimitAxis = 0)THEN
					IF((PM_ReadState_0.XPos[XbotID[i]-1]>XLimit) AND (XTarget[i]<XLimit))THEN
						Side[0].XbotList[Side[0].NumOfLostXbot] := XbotID[i];
						Side[0].NumOfLostXbot := Side[0].NumOfLostXbot+1;
					ELSIF((PM_ReadState_0.XPos[XbotID[i]-1]<=XLimit) AND (XTarget[i]>=XLimit))THEN
						Side[1].XbotList[Side[1].NumOfLostXbot] := XbotID[i];
						Side[1].NumOfLostXbot := Side[1].NumOfLostXbot+1;
					END_IF;				
				END_IF;
			END_FOR;
			IF(Side[0].NumOfLostXbot>0 AND Side[1].NumOfLostXbot>0)THEN
				Async16State := ASYNC16_SWAP_SHUTTLES;
			ELSIF(NOT(Side[0].InPosition) OR NOT(Side[1].InPosition))THEN
				Async16State := ASYNC16_MOVE_INIT_POS;
				Index := 0;
			ELSIF(Side[0].InPosition AND Side[1].InPosition)THEN
				Async16State := ASYNC16_MOVE_TARGET_POS;
				Index := 0;
			END_IF;
				
		ASYNC16_SWAP_SHUTTLES:
			
			Side[0].InPosition := FALSE;
			Side[1].InPosition := FALSE;
			IF(Side[0].NumOfLostXbot = Side[1].NumOfLostXbot)THEN
				k := Side[0].NumOfLostXbot*2;
			ELSIF(Side[0].NumOfLostXbot > Side[1].NumOfLostXbot)THEN
				k := Side[1].NumOfLostXbot*2;
			ELSIF(Side[0].NumOfLostXbot < Side[1].NumOfLostXbot)THEN
				k := Side[0].NumOfLostXbot*2;
			END_IF;
							
			Index := 0; // Index
			// k = LoopLimit
			IF(k>8)THEN
				k := 8;
			END_IF;
							
			PM_AsyncMotion_0.NumOfXbot := INT_TO_USINT(k);
			FOR i := 0 TO k-1 BY 2 DO
				PM_AsyncMotion_0.XbotID[i] 	:= UINT_TO_USINT(Side[0].XbotList[Index]);
				PM_AsyncMotion_0.TargetX[i]	:= PM_ReadState_0.XPos[Side[1].XbotList[Index]-1];
				PM_AsyncMotion_0.TargetY[i]	:= PM_ReadState_0.YPos[Side[1].XbotList[Index]-1];
								
				PM_AsyncMotion_0.XbotID[i+1] 	:=  UINT_TO_USINT(Side[1].XbotList[Index]);
				PM_AsyncMotion_0.TargetX[i+1]	:= PM_ReadState_0.XPos[Side[0].XbotList[Index]-1];
				PM_AsyncMotion_0.TargetY[i+1]	:= PM_ReadState_0.YPos[Side[0].XbotList[Index]-1];
				Index := Index +1;
			END_FOR;
							
			PM_AsyncMotion_0.Execute 		:= TRUE;
			TON_0.IN 						:= TRUE;
			
			IF(PM_AsyncMotion_0.Done AND StandStill AND TON_0.Q)THEN
				PM_AsyncMotion_0.Execute 	:= FALSE;
				Async16State := ASYNC16_COUNT_MISPLACED_SHUTTLES;
				TON_0.IN					:= FALSE;
					
			ELSIF(PM_AsyncMotion_0.Error)THEN  // TODO - ERROR HANDLING. WHAT TO HERE?
				PM_AsyncMotion_0.Execute 	:= FALSE;
				TON_0.IN					:= FALSE;
				ErrorCountShuffle := ErrorCountShuffle +1;
				IF(PM_AsyncMotion_0.ErrorID = PM_ERR_INVALID_PARAMETERS)THEN
					Async16State := ASYNC16_COUNT_MISPLACED_SHUTTLES;
					IF(ErrorCountShuffle >= 5)THEN
						ErrorCountShuffle := 0;
						Async16State := ASYNC16_SHUFFLE;
					END_IF;				
				ELSIF(PM_AsyncMotion_0.ErrorID = PM_ERR_INVALID_PARAMETERS OR PM_AsyncMotion_0.ErrorID = PM_ERR_WRONG_XBOTSTATE)THEN
					Async16State := ASYNC16_COUNT_MISPLACED_SHUTTLES;;
					IF(ErrorCountShuffle = 10)THEN
						Async16State := ASYNC16_ERROR;
						ErrorID :=PM_AsyncMotion_0.ErrorID;
					END_IF;
				ELSE
					Async16State := ASYNC16_ERROR;
					ErrorID := PM_AsyncMotion_0.ErrorID;
				END_IF;
			END_IF;
						
		
		ASYNC16_MOVE_INIT_POS:			
			PM_AsyncMotion_0.NumOfXbot := 0;
			// Move side 0 or side 1
			IF(Side[0].NumOfLostXbot>0 AND Side[1].NumOfLostXbot = 0)THEN		
				Index := 0;
			ELSIF(Side[1].NumOfLostXbot>0 AND Side[0].NumOfLostXbot = 0)THEN
				Index := 1;
			ELSIF(Side[0].NumOfLostXbot = 0 AND NOT(Side[0].InPosition))THEN
				Index := 0;
			ELSIF(Side[1].NumOfLostXbot = 0 AND NOT(Side[1].InPosition))THEN
				Index := 1;	
			END_IF;
						
						
			k := 0;
			FOR i := 0 TO USINT_TO_INT(Side[Index].NumOfXbot)-1 DO
				PM_AsyncMotion_0.XbotID[i] 	:= Side[Index].InitID[i];
				PM_AsyncMotion_0.TargetX[i] := Side[Index].XInit[i];
				PM_AsyncMotion_0.TargetY[i] := Side[Index].YInit[i];
				PM_AsyncMotion_0.NumOfXbot  := Side[Index].NumOfXbot;

			END_FOR;
			
			IF(PM_AsyncMotion_0.NumOfXbot = 0)THEN
				Async16State := ASYNC16_COUNT_MISPLACED_SHUTTLES;
				Side[Index].InPosition := TRUE;
				PM_AsyncMotion_0.Execute 		:= FALSE;
			ELSE
				PM_AsyncMotion_0.Execute 		:= TRUE;
			END_IF;
			TON_0.IN := TRUE;						
			IF(PM_AsyncMotion_0.Done AND StandStill AND TON_0.Q)THEN
				PM_AsyncMotion_0.Execute 	:= FALSE;
				Side[Index].InPosition := TRUE;
				TON_0.IN := FALSE;
				Async16State := ASYNC16_COUNT_MISPLACED_SHUTTLES;	
			ELSIF(PM_AsyncMotion_0.Error)THEN
				PM_AsyncMotion_0.Execute 	:= FALSE;
				TON_0.IN := FALSE;
				ErrorCountShuffle := ErrorCountShuffle +1;
				IF(PM_AsyncMotion_0.ErrorID = PM_ERR_INVALID_PARAMETERS OR PM_AsyncMotion_0.ErrorID = PM_ERR_WRONG_XBOTSTATE)THEN
					IF(ErrorCountShuffle >= 5)THEN
						ErrorCountShuffle := 0;
						Async16State := ASYNC16_SHUFFLE;
					END_IF;
				ELSIF(ErrorCountShuffle = 10)THEN
					Async16State := ASYNC16_COUNT_MISPLACED_SHUTTLES;
					ErrorCountShuffle := 0;
				END_IF;
			END_IF;
		
		ASYNC16_MOVE_TARGET_POS:
			PM_AsyncMotion_0.NumOfXbot := 0;
			IF(NumOfXbot>8)THEN
				FOR i := 0 TO USINT_TO_INT(Side[Index].NumOfXbot)-1 DO
					FOR k := 0 TO USINT_TO_INT(NumOfXbot)-1 DO
						IF((Side[Index].InitID[i] = XbotID[k]))THEN
							PM_AsyncMotion_0.XbotID[PM_AsyncMotion_0.NumOfXbot] := XbotID[k];
							PM_AsyncMotion_0.TargetX[PM_AsyncMotion_0.NumOfXbot] := XTarget[k];
							PM_AsyncMotion_0.TargetY[PM_AsyncMotion_0.NumOfXbot] := YTarget[k];
							PM_AsyncMotion_0.NumOfXbot := PM_AsyncMotion_0.NumOfXbot+1;
						END_IF;
					END_FOR;
				END_FOR;
			ELSE
				FOR k := 0 TO USINT_TO_INT(NumOfXbot)-1 DO
					PM_AsyncMotion_0.XbotID[PM_AsyncMotion_0.NumOfXbot] := XbotID[k];
					PM_AsyncMotion_0.TargetX[PM_AsyncMotion_0.NumOfXbot] := XTarget[k];
					PM_AsyncMotion_0.TargetY[PM_AsyncMotion_0.NumOfXbot] := YTarget[k];
					PM_AsyncMotion_0.NumOfXbot := PM_AsyncMotion_0.NumOfXbot+1;
				END_FOR;
				Index := 1;
			END_IF;
			
			IF(PM_AsyncMotion_0.NumOfXbot = 0)THEN
				Index := Index+1;
				PM_AsyncMotion_0.Execute 		:= FALSE;
			ELSE
				PM_AsyncMotion_0.Execute 		:= TRUE;
			END_IF;	
			TON_0.IN := TRUE;
			IF(PM_AsyncMotion_0.Done AND StandStill AND TON_0.Q)THEN
				PM_AsyncMotion_0.Execute 	:= FALSE;
				Index := Index+1;
				TON_0.IN := FALSE;
			ELSIF(PM_AsyncMotion_0.Error)THEN
				PM_AsyncMotion_0.Execute 	:= FALSE;
				TON_0.IN := FALSE;
				ErrorCountShuffle := ErrorCountShuffle +1;
				IF(PM_AsyncMotion_0.ErrorID = PM_ERR_INVALID_PARAMETERS OR PM_AsyncMotion_0.ErrorID = PM_ERR_WRONG_XBOTSTATE)THEN
					IF(ErrorCountShuffle >= 5)THEN
						ErrorCountShuffle := 0;
						Async16State := ASYNC16_SHUFFLE;
					END_IF;
				ELSIF(ErrorCountShuffle = 10)THEN
					Async16State := ASYNC16_COUNT_MISPLACED_SHUTTLES;
					ErrorCountShuffle := 0;
				END_IF;
			END_IF;
			
			IF(Index >= 2)THEN
				Async16State := ASYNC16_DONE;
			END_IF;
				
						
						
		ASYNC16_SHUFFLE:
			PM_Shuffle_0.NumOfMoves := 10;
			PM_Shuffle_0.MsgFrame := MsgFrame;
			PM_Shuffle_0.ReadFrame := ReadFrame;
			PM_Shuffle_0.DistanceLimit := 5;
			PM_Shuffle_0.XbotID := XbotID;
			PM_Shuffle_0.NumOfXbot	:= NumOfXbot;
			PM_Shuffle_0.Execute := TRUE;
			IF(PM_Shuffle_0.Done)THEN
				PM_Shuffle_0.Execute := FALSE;
				ErrorCountShuffle := 0;
				Async16State := ASYNC16_COUNT_MISPLACED_SHUTTLES;
			ELSIF(PM_Shuffle_0.Error)THEN
				PM_Shuffle_0.Execute := FALSE;
				Async16State := ASYNC16_ERROR;
				ErrorID := PM_Shuffle_0.ErrorID;	
			END_IF;
			PM_Shuffle_0();
		
		ASYNC16_DONE:
			IF(NOT(Execute))THEN
				Async16State := ASYNC16_WAIT;
				brsmemset(ADR(Side[0].XInit),0,8*4);
				brsmemset(ADR(Side[1].XInit),0,8*4);
				brsmemset(ADR(Side[0].YInit),0,8*4);
				brsmemset(ADR(Side[1].YInit),0,8*4);
				brsmemset(ADR(Side[0].InitID),0,8);
				brsmemset(ADR(Side[1].InitID),0,8);
				Side[0].InPosition := FALSE;
				Side[1].InPosition := FALSE;
				Side[0].NumOfXbot := 0;
				Side[1].NumOfXbot := 0;
			END_IF;
		
		ASYNC16_ERROR:
			IF(NOT(Execute))THEN
				Async16State := ASYNC16_WAIT;
				brsmemset(ADR(Side[0].XInit),0,8*4);
				brsmemset(ADR(Side[1].XInit),0,8*4);
				brsmemset(ADR(Side[0].YInit),0,8*4);
				brsmemset(ADR(Side[1].YInit),0,8*4);
				brsmemset(ADR(Side[0].InitID),0,8);
				brsmemset(ADR(Side[1].InitID),0,8);
				Side[0].InPosition := FALSE;
				Side[1].InPosition := FALSE;
				Side[0].NumOfXbot := 0;
				Side[1].NumOfXbot := 0;
				ErrorID := 0;
			END_IF;		
	END_CASE;
	

	TON_0();	
	Error 	:= Async16State = ASYNC16_ERROR;
	Done 	:= Async16State = ASYNC16_DONE;
	Busy 	:=  (Async16State>ASYNC16_WAIT) AND (Async16State < ASYNC16_DONE);
	PM_ReadState_0(ReadFrame := ReadFrame);	
	PM_AsyncMotion_0(MsgFrame := MsgFrame, ReadFrame := ReadFrame);
	
END_FUNCTION_BLOCK



