Rosserial Arduino - Serial Port failure when interrupted

edit

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-- ...