trying to run a 3axes gantry in sequence mode similar to the sequence of serial data sent - serial-port

Question

i want to run my 3 axes gantry(stm32) in sequence similar to the sequence of serial data sent from the pc but this is not happening. no data is lost as all the axes perform their task but in wrong sequence.
this is the recieve complete callback loop
`void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart)
{
if(r != ',')
{
Rx[i++] = r;
}
else
{
strncpy(s1,Rx,i);
Home = strrchr(s1, 'H');
StringX = strrchr(s1, 'X');
StringY = strrchr(s1, 'Y');
StringZ = strrchr(s1, 'Z');
StringW = strrchr(s1, 'W');
if(Home[0] == 'H' && Home[1] == 'O' && Home[2] == 'M' && Home[3] == 'E')
{
homeValue = 1;
}
if(StringX[0] == 'X')
{
MoveX= substr(StringX,1,8);
MoveXvalue = (atol(MoveX))/100;
SpeedX = substr(StringX,8,12);
SpeedXvalue = atol(SpeedX);
insertintoQueue_X(MoveXvalue, SpeedXvalue);
insertintoSequenceQueue (1);
}
if(StringY[0] == 'Y')
{
MoveY= substr(StringY,1,8);
MoveYvalue = (atol(MoveY))/100;
SpeedY = substr(StringY,8,12);
SpeedYvalue = atol(SpeedY);
insertintoQueue_Y(MoveYvalue, SpeedYvalue);
insertintoSequenceQueue (2);
///goes on for other axes
/
i = 0;
}
}`
i tried sequencing these received cmds with below loop(posting only for one axis)
`void insertintoQueue_X (int j, int l)
{
uint8_t queuefull[] = "XFULL";
if(countXmove == 256 || countXspeed == 256 )
{
HAL_UART_Transmit_IT (&huart1, queuefull, sizeof (queuefull));
}
Xmovequeue[countXmove] = j ;
countXmove++;
Xspeedqueue[countXspeed] = l;
countXspeed++;
}
void removefromQueue_X()
{
//uint8_t queueEmpty[] = "XEMPTY";
/*if(countXmove == 0 || countXspeed == 0)
{
HAL_UART_Transmit_IT (&huart2, queueEmpty, sizeof (queueEmpty));
}*/
currentXmoveelement = Xmovequeue[0];
currentXspeedelement = Xspeedqueue[0];
for(int i = 0; i < countXmove - 1; i++)
{
Xmovequeue[i] = Xmovequeue[i+1];
Xspeedqueue[i] = Xspeedqueue[i+1];
}
}`
while loop is as below
` removefromSequenceQueue();//only called once in while loop
for(int p = 0; p < countXmove ; p++)
{
if(currentSequence == 1)
{
removefromQueue_X();
if(currentXmoveelement >0)
{
//uint8_t Xmovvestatus[] = "XMMM";
if(CurrentXposition != currentXmoveelement)
{
if(CurrentXposition > currentXmoveelement )
{
currentmoveX = CurrentXposition - currentXmoveelement;
motorMoveTo(currentmoveX,0,currentXspeedelement,20,SMSPR_X,SMS_X,SMDPR_X,SMD_X);
}
else if(CurrentXposition < currentXmoveelement )
{
// HAL_UART_Transmit_IT (&huart1, Xmovvestatus, sizeof (Xmovvestatus));
currentmoveX = currentXmoveelement - CurrentXposition ;
motorMoveTo(currentmoveX,1,currentXspeedelement,20,SMSPR_X,SMS_X,SMDPR_X,SMD_X);
}
CurrentXposition = currentXmoveelement;
currentXmoveelement = 0;
currentmoveX = 0;
MoveXvalue=0;
}
}
}
}