0
$\begingroup$

Rosanswers logo

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.

Originally posted by Piachnp on ROS Answers with karma: 45 on 2016-06-03

Post score: 1

Improve this question
$\endgroup$

1 Answer 1

Reset to default
0
$\begingroup$

Rosanswers logo

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.

Originally posted by Piachnp with karma: 45 on 2016-06-07

This answer was ACCEPTED on the original site

Post score: 2

Original comments

Comment by jared on 2020-11-04:
That link to the avr baud rate error calculator appears to be now dead, but here's an alternative link for anyone looking: http://ruemohr.org/~ircjunk/avr/baudcalc/avrbaudcalc-1.0.8.php

Improve this answer
$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.