Hi everyone, I am new to the forum. I am trying to make a DC motor controller using an arduino mega 2560 and this dual motor shield. The motors used are these. The controller interface is meant to be ROS (Robot operating system)

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. Here is my code:

```
/********************************************************************************
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
#define POWER_SUPPLY 6.0
#define MOTOR_VOLTAGE 6.0
#define PWM_LIMIT (511*(MOTOR_VOLTAGE/POWER_SUPPLY))
#define INTEGRAL_LIM 255
/********************************************************************************
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;
std_msgs::Int32 position1;
std_msgs::Int32 position2;
//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);
////ROS publishers
ros::Publisher pub_pos1("position1", &position1);
ros::Publisher pub_pos2("position2", &position2);
/********************************************************************************
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);
nh.advertise(pub_pos1);
nh.advertise(pub_pos2);
//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()
{
delay(10);
position1.data = m1.position;
pub_pos1.publish(&position1);
position2.data = m2.position;
pub_pos2.publish(&position2);
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);
//Limit the contribution of the integral term
if (m1.integral>INTEGRAL_LIM)
m1.integral=INTEGRAL_LIM;
else if (m1.integral< -INTEGRAL_LIM)
m1.integral= -INTEGRAL_LIM;
if (m2.integral>INTEGRAL_LIM)
m2.integral=INTEGRAL_LIM;
else if (m2.integral< -INTEGRAL_LIM)
m2.integral= -INTEGRAL_LIM;
//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
}
```

I have attached the cpp and header file for the class DCMotorPID that I wrote. Disregard all the ROS bits in the code if you are not familiar with it.

I was having some problems with the ROS communication breaking down, and I managed to debug the problem down to the point where I realized the issue is when I call the class methods setSpeed for both motor1 and 2. If I comment those lines out, everything works as expected.

I even moved the code from those functions inside the interruptions, so I didn't have to call the method and that also worked....

If anyone is familiar with ROS and the rosserial package, I also posted a question here.

Having that method call inside the ISR is breaking the serial communication with ROS. Does anyone have any idea why???

DCMotorPID.cpp (2.41 KB)

DCMotorPID.h (1.05 KB)