Communication Problem via UART

Hello everyone,

I’m new here and with Arduino, so I don’t know it is right place to open this topic. But I think that my problem is about programming, especially about interrupts. My problem is about communication via Serial port on Arduino Mega 2560. In my project, I use Dynamixel RX-24F Servo motor and I use Arduino to control this motor. And I use a library (Dynamixel_Serial_V2_2.zip) for this purpose. In my project I need to move the motor between two points continuously (i.e., motor moves from point A to point B, then it move from point B to point A and it goes like this). To accomplish this, I used servo() function in library and I put delay() between two servo() function to wait the motor until it reaches the desired point. However, with this code I couldn’t run the motor with the way I wanted. So I realized the caveat written in reference page of delay() function.

Then I changed my code by using millis() code instead of delay(). And it worked perfectly. However, now I am encountering a new problem. When I try to use readPosition() function in every seconds (or any time interval greater than 10 ms again by using millis() function) I cannot read the position. When I check the direction control pin (since Dynamixel uses half duplex communication we need to use a control pin to control TX/RX direction) with the oscilloscope I realized that control pin goes high and goes low immediately when I am using readPosition() function with some delay. It has to remain high while Arduino transmits data. And it remains high when I use servo() function with some delay. And if I don’t put any delay between two readPosition() functions control pin remains high while transmission is continuing and I can read the position. And I think it doesn’t make any sense because both function use the same private function transmitInstructionPacket() to transmit the packet via serial channel. Functions other than read functions (servo(), ping() etc.) works very well with some delay, i.e., control pin remains high during transmission.

I’m putting transmitInstructionPacket() and readStatusPacket() functions, which are used by other functions, servo() and readPosition() functions at the end. I am sorry about long post and complex explanation but I tried to be specific as much as I can. And I am sorry about possible grammatical errors since I am not native in English. I will be grateful if you can help me to figure out this problem.

Regards,
Mansur

transmitInstructionPacket() and readStatusPacket() functions.

void DynamixelClass::transmitInstructionPacket(void) // Transmit instruction packet to Dynamixel
{ 
 unsigned char Counter;
 Counter = 0; 
 
 digitalWrite(Direction_Pin,HIGH); // Set TX Buffer pin to HIGH 
 
 Serial1.write(HEADER); // Write Header (0xFF) data 1 to serial                     
 Serial1.write(HEADER); // Write Header (0xFF) data 2 to serial
 Serial1.write(Instruction_Packet_Array[0]); // Write Dynamixal ID to serial 
 Serial1.write(Instruction_Packet_Array[1]); // Write packet length to serial 
 
 do // Write Instuction & Parameters (if there is any) to serial
 { 
 Serial1.write(Instruction_Packet_Array[Counter + 2]); 
 Counter++;
 }while((Instruction_Packet_Array[1] - 2) >= Counter);
 Serial1.write(Instruction_Packet_Array[Counter + 2]); // Write check sum to serial
 
 if ((UCSR0A & B01100000) != B01100000) // Wait for TX data to be sent
 { 
 Serial1.flush();
 } 

 digitalWrite(Direction_Pin,LOW); //Set TX Buffer pin to LOW after data has been sent
 
}


unsigned int DynamixelClass::readStatusPacket(void)
{
 unsigned char Counter = 0x00;
 unsigned char First_Header = 0x00;
 
 Status_Packet_Array[0] = 0x00;
 Status_Packet_Array[1] = 0x00;
 Status_Packet_Array[2] = 0x00; 
 Status_Packet_Array[3] = 0x00;
 

 Time_Counter = STATUS_PACKET_TIMEOUT + millis(); // Setup time out error
 
 while(STATUS_FRAME_BUFFER >= Serial1.available()) // Wait for " header + header + frame length + error " RX data
 {
 if (millis() >= Time_Counter)
 {
 return Status_Packet_Array[2] = B10000000; // Return with Error if Serial data not received with in time limit
 }
 } 
 
 if (Serial1.peek() == 0xFF && First_Header != 0xFF)
 {
 First_Header = Serial1.read(); // Clear 1st header from RX buffer
 }
 else if (Serial1.peek() == -1)
 {
 return Status_Packet_Array[2] = B10000000; // Return with Error if two headers are not found
 }
 
 if(Serial1.peek() == 0xFF && First_Header == 0xFF)
 {
 Serial1.read(); // Clear 2nd header from RX buffer
 Status_Packet_Array[0] = Serial1.read();                 // ID sent from Dynamixel
 Status_Packet_Array[1] = Serial1.read(); // Frame Length of status packet
 Status_Packet_Array[2] = Serial1.read(); // Error byte 
 
 Time_Counter = STATUS_PACKET_TIMEOUT + millis();
 while(Status_Packet_Array[1] - 2 >= Serial1.available()) // Wait for wait for "Para1 + ... Para X" received data
 { 
 if (millis() >= Time_Counter)
 {
 return Status_Packet_Array[2] = B10000000; // Return with Error if Serial data not received with in time limit
 }
 } 
 
 do
 {
 Status_Packet_Array[3 + Counter] = Serial1.read();
 Counter++; 
 }while(Status_Packet_Array[1]-2 > Counter); // Read Parameter(s) into array
 
 Status_Packet_Array[Counter + 3] = Serial1.read(); // Read Check sum 
 }
 else
 {
 return Status_Packet_Array[2] = B10000000; // Return with Error if two headers are not found
 }
}

servo() function (I changed the name of it but remain is the same with the original)

unsigned int DynamixelClass::joint(unsigned char ID, unsigned int position, unsigned int speed)
{
   Instruction_Packet_Array[0] = ID;
 Instruction_Packet_Array[1] = JOINT_LENGTH;
 Instruction_Packet_Array[2] = COMMAND_WRITE_DATA;
 Instruction_Packet_Array[3] = RAM_GOAL_POSITION_L;
 Instruction_Packet_Array[4] = byte(position);
 Instruction_Packet_Array[5] = byte((position & 0x0F00) >> 8);
 Instruction_Packet_Array[6] = byte(speed);
 Instruction_Packet_Array[7] = byte(speed >> 8);
 Instruction_Packet_Array[8] = ~(ID + JOINT_LENGTH + COMMAND_WRITE_DATA + RAM_GOAL_POSITION_L + byte(position) + byte((position & 0x0F00) >> 8) + byte(speed) + byte(speed >> 8)); 
 
 while (Serial1.read() != -1); // Clear RX buffer;

 transmitInstructionPacket(); 
 
 if (ID == 0xFE || Status_Return_Value != ALL ) // If ID of FE is used no status packets are returned so we do not need to check it
 {
 return (0x00);
 }
 else
 {
 readStatusPacket();
 if (Status_Packet_Array[2] == 0) // If there is no status packet error return value
 {
 return (Status_Packet_Array[0]); // Return SERVO ID
 }
 else
 {
 return (Status_Packet_Array[2] | 0xF000);   // If there is a error Returns error value
 }
 }
}

readPosition() function

unsigned int DynamixelClass::readPosition(unsigned char ID)
{ 
 Instruction_Packet_Array[0] = ID;
 Instruction_Packet_Array[1] = READ_LENGTH;
 Instruction_Packet_Array[2] = COMMAND_READ_DATA;
 Instruction_Packet_Array[3] = RAM_PRESENT_POSITION_L;
 Instruction_Packet_Array[4] = READ_TWO_BYTE_LENGTH;
 Instruction_Packet_Array[5] = ~(ID + READ_LENGTH + COMMAND_READ_DATA + RAM_PRESENT_POSITION_L + READ_TWO_BYTE_LENGTH); 
 
 while (Serial1.read() != -1); // Clear RX buffer;

 transmitInstructionPacket(); 
 readStatusPacket();
 
 if (Status_Packet_Array[2] == 0)
 { 
 return Status_Packet_Array[4] << 8 | Status_Packet_Array[3]; // Return present position value
 }
 else
 {
 return (Status_Packet_Array[2] | 0xF000);             // If there is a error Returns error value
 }
}

I will be grateful if you can help me to figure out this problem.

Without seeing ALL of your code? Not a snowball's chance in hell.

PaulS:
Without seeing ALL of your code? Not a snowball’s chance in hell.

Oh! You’re right! I forgot to put my Arduino code sorry :confused: I’m trying PART 1 and PART 2 separately but I put them together here.

#include <Dynamixel_Serial.h>
unsigned long time;
unsigned long timePrint;
unsigned int positionMotor;
unsigned int speedMotor = 200;
boolean directionMotor = 1;

void setup(){  
  Serial.begin(57600);                                       // Set up Software Serial
  delay(1000);                                               // Give time for Dynamixel to start on power-up
  Dynamixel.begin(SERVO_SET_Baudrate, SERVO_ControlPin);     // Set up Arduino to communicate to Dynamixel
  time = millis();
  timePrint = millis();
}

void loop(){ 
// PART 1: In this part control pin goes high and goes low immediately
if(millis() - timePrint >= 1000){
  positionMotor = Dynamixel.readPosition(SERVO_ID);
  Serial.print(millis());Serial.print(": ");  Serial.println(positionMotor);
  timePrint = millis();
}


// PART 2: But in this part control pin remains high during transmission
if (millis() - time >= 2000 & directionMotor)
{
	Dynamixel.joint(SERVO_ID,600,speedMotor); 
	time = millis();
        directionMotor = !directionMotor;
}

if (millis() - time >= 2000 & !directionMotor)
{
	Dynamixel.joint(SERVO_ID,900,speedMotor); 
	time = millis();
        directionMotor = !directionMotor;
}
}

Now I realized that when I comment out the following part of my code there is no problem.

Serial.print(millis());Serial.print(": "); Serial.println(positionMotor);

I guess Serial interrupts my code but I need to read the position of the motor from serial to use in my project. How can I solve this problem? And isn't it strange that Serial interrupts only when I try to read position with some delay and it doesn't interrupt when I read position continuously?

Edit: I said it wrong about minimum delay time. If I use 1 ms delay one signal is transmitting correctly while the following cannot be transmitted because of immediate fall of control pin. And if I use 10 ms delay all signals encounters with immediate fall of control pin.

That Dynamixel library was written by an idiot.

void DynamixelClass::begin(long baud, unsigned char D_Pin){ 

#if defined(__AVR_ATmega32U4__)
    Serial1.begin(baud);                    //Setup Serail
#else 
    Serial.begin(baud);                 //Setup Serail
#endif
    Direction_Pin = D_Pin;
    pinMode(Direction_Pin,OUTPUT);  

}

Hard-coding the use of the hardware serial pins (0 and 1) is stupid. It certainly interferes with your use of Serial to read data from the PC.

The indenting and random white space, and lack of white space in places, also indicate someone that doesn't care.

You'd be well off ditching that library.

Thank you for your answer. I'm using Arduino Mega 2560 therefore Dynamixel uses Serial1 ports of Arduino and I changed the library accordingly.

void DynamixelClass::begin(long baud, unsigned char D_Pin)
{   
    Serial1.begin(baud);                    
    Direction_Pin = D_Pin;
    pinMode(Direction_Pin,OUTPUT);  
}

At first I didn't realize that problem so it also gave me headache until I figured out what the problem is. Now Dynamixel doesn't interfere with my communication with PC. But when I try to use Serial.println() something goes wrong. Also I changed my code to use Serial.println() part 1 second after readPosition() function but again I encountered with the same problem.

So, you made some changes to the library, and to your code, and it still doesn't work. And, now, you'd like us to guess what the library looks like and what your code looks like, and tell you what's wrong.

I'll pass. Good luck.

Yeah you’re right! I didn’t want to throw the whole code so I just wrote some part of it. But I think by doing this way I made impossible to help me. Now will put changed library as attachment and I will write my Arduino codes and their explanations. I hope you can help me and I am sorry about inadequate information I gave before.

In library I changed #if defined(AVR_ATmega32U4) part with #if defined(AVR_ATmega2560) since I’m using Arduino Mega 2560. Thus, Dynamixel uses Serial1 ports of Arduino.

As I said before, in the following code, control pin to use TX/RX direction goes HIGH and goes LOW immediately.

#include <Dynamixel_Serial.h>
unsigned long timePrint;
unsigned int positionMotor;

void setup(){  
  Serial.begin(57600);
  delay(1000);
  Dynamixel.begin(SERVO_SET_Baudrate, SERVO_ControlPin);     
  timePrint = millis();
}

void loop(){ 
if(millis() - timePrint >= 1000){
  positionMotor = Dynamixel.readPosition(SERVO_ID);
  Serial.print(millis());Serial.print(": ");  Serial.println(positionMotor);
  timePrint = millis();
}
}

However, in the following code control pin remains HIGH while transmission.

#include <Dynamixel_Serial.h>
unsigned long time;
unsigned int speedMotor = 200;
boolean directionMotor = 1;

void setup(){  
  Serial.begin(57600);
  delay(1000);
  Dynamixel.begin(SERVO_SET_Baudrate, SERVO_ControlPin);
  time = millis();
}
void loop(){
if (millis() - time >= 2000 & directionMotor)
{
	Dynamixel.joint(SERVO_ID,600,speedMotor); 
	time = millis();
        directionMotor = !directionMotor;
}

if (millis() - time >= 2000 & !directionMotor)
{
	Dynamixel.joint(SERVO_ID,900,speedMotor); 
	time = millis();
        directionMotor = !directionMotor;
}
}

Then I realized that when I comment out the following part from my first code, immediate fall in control pin problem is solved.

Serial.print(millis());Serial.print(": "); Serial.println(positionMotor);

But I need to use Serial port of Arduino to read some data from PC. So I need this part. And also if I use the following code I don’t encounter with any problem, I can read position correctly.

void loop(){
positionMotor = Dynamixel.readPosition(SERVO_ID);
Serial.print(millis());Serial.print(": ");  Serial.println(positionMotor);
}

Then I tried the following code but same problem, immediate fall of control pin, is happened again

#include <Dynamixel_Serial.h>
unsigned long timePrint;
unsigned long timeSerial;
boolean printSerial = false;

void setup(){  
  Serial.begin(57600);
  delay(1000);
  Dynamixel.begin(SERVO_SET_Baudrate, SERVO_ControlPin);
  timePrint = millis();
}
void loop(){ 
if(millis() - timePrint >= 1000)
{
  positionMotor = Dynamixel.readPosition(SERVO_ID);
  timePrint = millis();
  printSerial = true;
  timeSerial = millis();
}

if(millis() - timeSerial >= 500 & printSerial)
{
  Serial.println(positionMotor);
  timeSerial = millis();
  timePrint = millis();
  printSerial = false;
}
}

Dynamixel_Serial.cpp (42 KB)

Dynamixel_Serial.h (8.05 KB)

in the following code, control pin to use TX/RX direction goes HIGH and goes LOW immediately.

I don't understand what you are trying to say here. What "control pin" are you talking about?

if(millis() - timeSerial >= 500 & printSerial)

There is a big difference between the bitwise and (&) and the logical and (&&) operators.

First, I want to thank you for answering me patiently.

Dynamixel uses half dublex communication protocol. Thus, to communicate with Dynamixel I'm using MAX485 circuit and in this circuit I need to use a pin to determine the transmission direction (TX/RX). This pin must remain HIGH during transmission in order to transmit data to Dynamixel.

There is a big difference between the bitwise and (&) and the logical and (&&) operators.

You're definitely right! I shouldn't have done such mistakes. I will correct them and try my code but I don't think it will solve the problem. But I'll let you know.

Thus, to communicate with Dynamixel I'm using MAX485 circuit and in this circuit I need to use a pin to determine the transmission direction (TX/RX). This pin must remain HIGH during transmission in order to transmit data to Dynamixel.

The code that you are posting don't show that YOU are making the Arduino change the state of this pin. Is the library supposed to do that? I can't imagine why using, or not using, Serial has any impact on the state of that control pin UNLESS it is pin 0 or 1 that you (or the library) are trying to use as the control pin.

In the library transmitInstructionPacket() does that with digitalWrite() function. And it is assigned at the beginning of the code as pin 2.

void DynamixelClass::transmitInstructionPacket(void) // Transmit instruction packet to Dynamixel
{ 
 unsigned char Counter;
 Counter = 0; 

 digitalWrite(Direction_Pin,HIGH); // Set TX Buffer pin to HIGH 

 Serial1.write(HEADER); // Write Header (0xFF) data 1 to serial                     
 Serial1.write(HEADER); // Write Header (0xFF) data 2 to serial
 Serial1.write(Instruction_Packet_Array[0]); // Write Dynamixal ID to serial 
 Serial1.write(Instruction_Packet_Array[1]); // Write packet length to serial 

 do // Write Instuction & Parameters (if there is any) to serial
 { 
 Serial1.write(Instruction_Packet_Array[Counter + 2]); 
 Counter++;
 }while((Instruction_Packet_Array[1] - 2) >= Counter);
 Serial1.write(Instruction_Packet_Array[Counter + 2]); // Write check sum to serial

 if ((UCSR0A & B01100000) != B01100000) // Wait for TX data to be sent
 { 
 Serial1.flush();
 } 

 digitalWrite(Direction_Pin,LOW); //Set TX Buffer pin to LOW after data has been sent

}

I'll be damned if I can see where SERVO_ControlPin is defined. As I said in the first response, that library is a piece of crap. Write your own, or suffer it's weaknesses.

I solved the problem by changing UCSR0A with UCSR1A in transmitInstructionPacket() function, since Dynamixel uses Serial1 to communicate.

Old version

 if ((UCSR0A & B01100000) != B01100000) // Wait for TX data to be sent
 { 
 Serial1.flush();
 }

New version

 if ((UCSR1A & B01100000) != B01100000) // Wait for TX data to be sent
 { 
 Serial1.flush();
 }

Thanks for your help and patience. And sorry for wasting your time with my incomplete explanations :confused:

I'm glad you figured it out. There is a lot of stuff that that library is doing that is unnecessary or silly. That bit with checking whether the transmit register is empty, and then calling Serial.flush() in a while loop is silly. Serial.flush() does not return until the transmit buffer is empty (the last character is in the transmit register), so calling it in a while loop is useless. The first time the statement is called, it will not return until the transmit buffer is empty and the transmit register has something in it. Calling the function again is pointless, because it will simply return immediately.