Error when trying to read information from Arduino Mega

Hello, I am having some problems while trying to read data from my arduino mega. I am using Serial1 to send information and Serial0 to receive it. The arduino is conected to a motor, from which I am trying to read its current position using the code bellow.

int Sensor_data;
void loop(){
Sensor_data = Dynamixel.readPosition(0x05); //Function returns the motor position
Serial.println(Sensor_data);
delay(5);
return;
}

Everything works perfectly until I set the delay unequal to zero, like shown above. Without the delay the arduino prints the actual position in the Serial Monitor, but with the delay the arduino prints an error.
The error corresponds to the following while loop (which is in the same script as Dynamixel.readPosition and runs when Dynamixel.readPosition is called). The idea behind the while loop was to let it run until the 4 first bytes of information were in the buffer and to return an error if the running time exceeded the time limit (which appears to happen if the delay is unequal to zero).

int Time_Counter;
Time_Counter = 50 + millis();
while(4 >= Serial1.available()){
    if (millis() >= Time_Counter){
 return B10000000;
 }
}

I do not know why the delay has such an effect on the while loop, because the while loop runs when the Dynamixel.readPosition function is called, therefore making it independant of the delay that comes afterwards.

I hope you can help me, I dont know if I have a thought or a code error.

Thank you very much

[EDIT]

The hardware that I am using is called a Dynamixel MX64-R, you can find its Data Sheet in the following adress http://support.robotis.com/en/product/dynamixel/mx_series/mx-64.htm.
The programm (2 c++ scripts and the ArduinoIDE code) can be found in the attachment. The documents Dynamixel_Serial.h and Dynamixel_Serial.cpp contain all the functions and variables necessary to send and receive information. I saved them in the Arduino libraries using the following path Arduino\libraries\Dynamixel. I use the ArduinoIDE file readpos to control the motors and retrieve information from them.

The function Dynamixel.readPosition to read the current position of the motor can be found in line 582 of Dynamixel_Serial.cpp. The while loop that returns the error, when the delay is set unequal to zero, is found in line 129 of the same document.

An example of a correct result in the Serial window for a delay equal to zero is shown bellow:

2086
2080
2074
2067
2061
2056

As it can be seen the position changes with time.

An example in the Serial window of an error when the delay is unequal to zero is shown bellow:
3754
128
128
128
128
128
128
128
128

The value remains constant and equal to the return value of the while loop.

I hope this information is helpfull.

Thank you very much

Arduino_Code_Forum.zip (7.91 KB)

Please do us and yourself a favour and post your whole program or a shorter one that demonstrates the problem. Links to any libraries and hardware used would also be helpful.