Ask Your Question
0

Rosserial Arduino - Serial Port failure when interrupted

asked 2016-06-03 17:53:01 -0500

Piachnp gravatar image

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-- ...
(more)
edit retag flag offensive close merge delete

2 Answers

Sort by ยป oldest newest most voted
0

answered 2016-06-07 09:20:41 -0500

Piachnp gravatar image

Thanks for the suggestions Paul. I actually managed to fix the problem, which was a sum of many little ones. I'm leaving some tips for anyone who might encounter this in the future:

  • One of the problems was the line TCCR5B = (1 << CS51); that should be TCCR5B |= (1 << CS51); I have no idea why this is a problem, since that register should be all zero except for the bit CS51.... but it works with the pipe, and doesn't work without it.
  • Another thing to take into account for people losing sync over time, is to make sure your baud rate matches the crystal you are running on your board/arduino. For arduinos with 16Mhz clock, running at 115200 is not a good idea, since this baud rate yields a 2.1% maximum error in the communication. Sadly, the rosserial_python won't accept custom baud rates like 250000 which yield no error. For more info consult this link: http://wormfood.net/avrbaudcalc.php .
edit flag offensive delete link more
0

answered 2016-06-05 08:41:15 -0500

PaulBouchier gravatar image

Nothing jumps to mind as a problem in your code. Does Hello World run on the arduino mega? If it does, I suggest you break the system down to try & discover exactly what's breaking it, and why.

Some suggestions: - Publish "Hello" in your main loop and when you stop seeing it come out in ROS is the exact time when the system died - Turn the motor by hand (vs sending a command). If it crashes when you turn it by hand then it's the encoder interrupts, as you suspect. If not, then something else is going on. - Blink the arduino LED in your main loop. Does it keep blinking when the serial port read failure happens? If not, Arduino has crashed.

Hope this helps

Paul

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

3 followers

Stats

Asked: 2016-06-03 17:53:01 -0500

Seen: 894 times

Last updated: Jun 07 '16