Ask Your Question

Revision history [back]

click to hide/show revision 1
initial version

Rosserial Arduino - Serial Port failure when interrupted

Hello, I am trying to make a DC motor controller using an arduino mega 2560 and this dual motor shield. The motors used are these.

Ideally I want to use the rosserial library in such a way that I can send commands to both motors, and read the current position, as well as the current through the motor (the driver gives you this info). Whether this feedback will be an advertised topic or a service is still to be determined, given the problems I am having.

Here is the code running in my Arduino. I'm not a huge fan of using arduino libraries, so it is mostly microcontroller code (registers). The class DCMotorPID is my own and has been extensively tested. This whole code works perfectly on its own without the ROS communication on top.

/********************************************************************************
  Includes
********************************************************************************/
#include <Arduino.h>
#include <ArduinoHardware.h>
#define USE_USBCON 1
#include <ros.h>
#include <avr/io.h>
#include <avr/interrupt.h>
#include <std_msgs/Int32.h>
#include <std_msgs/Int16.h>
#include <DCMotorPID.h>

/********************************************************************************
  Macros and Defines
********************************************************************************/

#define BAUD_RATE 115200
#define SAMPLING_FREQ 100.00
#define DT 1/SAMPLING_FREQ

/********************************************************************************
  Global Variables
********************************************************************************/

//DcMotorPID(PWM, DIR, FB, D2, SF)
DCMotorPID m1(6,8,A0,4,12);
DCMotorPID m2(7,9,A1,4,12);

// Global variables for ROS 
ros::NodeHandle nh;

//ROS subscribers and their callbacks
void sub_cmd1_cb (const std_msgs::Int32& pos1_msg)
{
  m1.command= pos1_msg.data;
}
ros::Subscriber<std_msgs::Int32> sub_cmd1("command1", sub_cmd1_cb);

void sub_cmd2_cb (const std_msgs::Int32& pos2_msg)
{
  m2.command= pos2_msg.data;
}
ros::Subscriber<std_msgs::Int32> sub_cmd2("command2", sub_cmd2_cb);

/********************************************************************************
  Program Code
********************************************************************************/
void setup()
{
  //Initialize PID
  m1.init();
  m2.init();

  //ROS initialization
  nh.getHardware()->setBaud(BAUD_RATE);
  nh.initNode();
  nh.subscribe(sub_cmd1);
  nh.subscribe(sub_cmd2);

  //Configure External Interrupts to handle encoder signal
  EICRA |= (1 << ISC30) | (1<<ISC10);  // INT3 & INT1 (channel A) interrupts on both flanks
  EIMSK |= (1 << INT3) | (1<<INT1);   // INT3 & INT1 interruptions enabled

  //Initialize Timer5 for sampling frequency 100Hz
  TIMSK5 = (1 << TOIE5);    //enable timer overflow interrupt for Timer5
  TCNT5 = 45535;            //set counter to 45535, 20000 clicks will be 10 ms
  TCCR5B = (1 << CS51);     //start timer5 with prescaler=8
}

void loop()
{
  nh.spinOnce();
}

//Takes up about 156 us
ISR(TIMER5_OVF_vect)
{
  // Reset the timer5 count for 10ms
  TCNT5 = 45535;
  //PID routine
  //Proportional term
  long error1 = m1.getError();
  long error2 = m2.getError();       

  //Integral term
  m1.integral = m1.integral + (error1 * (float)DT);         
  m2.integral = m2.integral + (error2 * (float)DT);         

  //Derivative term
  m1.derivative = (error1 - m1.last_error) * (float)SAMPLING_FREQ;
  m2.derivative = (error2 - m2.last_error) * (float)SAMPLING_FREQ;  

  // Compute Duty Cycle
  long output1 = m1.KP*error1 + m1.KI*m1.integral + m1.KD*m1.derivative;
  long output2 = m2.KP*error2 + m2.KI*m2.integral + m2.KD*m2.derivative;
  m1.setSpeed(output1);
  m2.setSpeed(output2);

  // Remember last state
  m1.last_error = error1;
  m2.last_error = error2;
}

//Encoder handling routine for Motor1 (channel A is PD3, channel B is PD2)
//Interrupt time is about 4us
ISR(INT3_vect)
{
  unsigned char enc_state;
  unsigned char dir;
  enc_state = ((PIND & 12) >> 2);             //read the port D pins 2 & 3
  dir = (enc_state & 1) ^ ((enc_state & 2) >> 1); //determine direction of rotation
  if (dir == 1) m1.position++; else m1.position--;        //update encoder position
}

//Encoder handling routine for Motor2 (channel A is PD1, channel B is PD0)
//Interrupt time is about 4us
ISR(INT1_vect)
{
  unsigned char enc_state;
  unsigned char dir;
  enc_state = (PIND & 3);             //read the port D pins 0 & 1
  dir = (enc_state & 1) ^ ((enc_state & 2) >> 1); //determine direction of rotation
  if (dir == 1) m2.position++; else m2.position--;        //update encoder position
}

This is a barebones version where I can only send commands to the motors through the topics /command1 and /command2. To reproduce the problem, I run roscore in a terminal, then the rosserial_python node:

rosrun rosserial_python serial_node.py _port:=/dev/ttyACM3 _baud:=115200

Initially the connection is succesfull, and the subscribers are setup. However, as soon as I command any motor to move (by typing in a terminal rostopic pub /command1 std_msgs/Int32 -- 800 ) the serial communication breaks down:

pedro@Dell-i7559:~$ rosrun rosserial_python serial_node.py _port:=/dev/ttyACM3 _baud:=115200
[INFO] [WallTime: 1464991088.633517] ROS Serial Python Node
[INFO] [WallTime: 1464991088.636234] Connecting to /dev/ttyACM3 at 115200 baud
[INFO] [WallTime: 1464991091.176181] Note: subscribe buffer size is 1024 bytes
[INFO] [WallTime: 1464991091.176375] Setup subscriber on command1 [std_msgs/Int32]
[INFO] [WallTime: 1464991091.495696] Setup subscriber on command2 [std_msgs/Int32]
[WARN] [WallTime: 1464991098.348677] Serial Port read returned short (expected 1 bytes, received 0 instead).
[WARN] [WallTime: 1464991098.348987] Serial Port read failure: 
[ERROR] [WallTime: 1464991110.824118] Lost sync with device, restarting...
[ERROR] [WallTime: 1464991125.825286] Lost sync with device, restarting...
[WARN] [WallTime: 1464991126.035587] Serial Port read returned short (expected 2 bytes, received 0 instead).
[WARN] [WallTime: 1464991126.035847] Serial Port read failure: 
[WARN] [WallTime: 1464991126.491159] Serial Port read returned short (expected 1 bytes, received 0 instead).
[WARN] [WallTime: 1464991126.491435] Serial Port read failure: 
[WARN] [WallTime: 1464991127.559198] Serial Port read returned short (expected 2 bytes, received 0 instead).

After this Serial Port failure, the communications never recover.

  • I am running ROS Jade, and rosserial version 0.7.1. I have tried running the same in ROS Indigo with the same results.
  • The lost sync message sometimes shows, sometimes it doesn't.
  • I have tried various baud rates for the serial communication, from 9600 up to 115200 with no effect
  • I have also tried adding a delay in my loop when the nh.spinOnce() is.
  • It looks like the encoder interruptions are the problem, since the communication breaks down when the motors start moving. These routines have been timed at 4us. They can't get much faster than that. However they do get called significantly, since these encoders provide 4123 pulses per revolution at X2 encoding. This means those interruptions happen every 692us in the worst case (max speed). I have tried going to X1 encoding, meaning 2061 pulses per revolution, interruptions every 1.384ms (again, worst case)

Is it possible that rosserial doesn't play nice with being interrupted? The PID loop interruptions are much longer (about 156us) and happens every 10ms but those don't seem to cause any problem somehow.

Any help will be very appreciated.