Hello,
I want to add the Arduino Mega 2560 to a chain of Robotis Dynamixel motors (MX-106R, MX-64R and MX-28R). The Arduino will be connected through a RS-485 breakout board and Serial1 (or 2, or 3) or SoftwareSerial (http://arduino.cc/en/Reference/softwareSerial). The chain is controlled from the PC (Ubuntu 12.04 LTS, ROS Hydro) via a FTDI USB-RS485 converter. I can perfectly find the chain of Dynamixel motors, but setting up the Arduino to act as a actuator/sensor proves harder than thought.
What have I tried:
- verified communication over RS485; 2 Putty terminals, one connected to Serial COM of Arduino and one connected to USB-RS485 COM, no problems there.
- Tutorial of rosserial, all works fine, but only over Serial port of Arduino and not Serial1/2/3 or SoftwareSerial.
So, what would be the simplest method of setting up a node with an Arduino over serial communication other then Serial0? How to publish to Serial1/2/3 or SoftwareSerial?
Update: I have some communication. But i get publish before configure. So timing is wrong. I need RTS for RS485 to work. I define a pin and set i high when nh.initnode or nh.publish is called. I set to low after delay, but trial and error. Can someone give me timing diagram of ROS init and publish?
Update(2): I have successfully added RTS functionality by editing the new hardware class:
class NewHardware : public ArduinoHardware
{
public:
NewHardware():ArduinoHardware(&Serial1, 57600){};
void init(){
pinMode(RTS, OUTPUT);
digitalWrite(RTS, LOW);
iostream->begin(baud_);
}
int read(){
return iostream->read();
};
void write(uint8_t* data, int length){
for(int i=0; i<length; i++){
digitalWrite(RTS, HIGH);
iostream->write(data[i]);
}
digitalWrite(RTS, LOW);
}
};
ros::NodeHandle_<NewHardware> nh;
now, every time a write action is initiated the RTS pin is set to HIGH, and the publishing of message works (better). But while writing the RTS pin will go LOW, as if the write action is interrupted. This will cause a sync error and the communication is far from reliable... When I add a lot of delay before releasing the RTS I can get some communication working, but the delay is dependent on the message length, and as the message length is variable, the delay will sometimes not be sufficient, causing sync errors again...
Below a picture of my scope, with top waveform TX and bottom RX of Arduino. RX should be low until TX is finished...
Update(3): Solved! I fixed it by checking the flag "TXCn: USART Transmit Complete". I kept the RTS HIGH for as long as the transmit complete flag was not risen. Here is the code:
void write(uint8_t* data, int length){
digitalWrite(RTS, HIGH);
for(int i=0; i<length; i++){
iostream->write(data[i]);
}
while(!(UCSR1A & (1<<TXC1)));
digitalWrite(RTS, LOW);
}
Originally posted by FrankB on ROS Answers with karma: 11 on 2015-02-24
Post score: 1
Original comments
Comment by toyoxx on 2016-11-01:
Hi.
I'm doing something quite similar to what you achieved, and your post solves most of my doubts. However, the constructor for ArduinoHardware only accepts HardwareSerial instances. How could I modify the code so I can pass it SoftwareSerial instances?