Rosserial and Arduino Software Serial Library

Hey,

I’m using the arduino software serial library to send byte signals to a motor driver (sabertooth 2x25).
I defined pin 11 as the tx output pin.

The control commands are sent to the arduino through rosserial (from ROS framework) over the usb connection.

Unfortunately the arduino seems to send some weird signals on the output pin when I launch rosserial or during the upload of a sketch. This leads to an uncontrollable behaviour, when I want to launch the robot.

Is there any way to suppress arduino from doing this?

I hope you guys can help me out with this, as I’ve been trying to find a solution to this for quite some time.

Here are the relevant parts of the code:

#include <ros.h>
#include <std_msgs/Float32.h>
#include <geometry_msgs/Vector3.h>
#include <ros/time.h>
#include <AS5045.h>
#include <PID_v1.h>
#include <SoftwareSerial.h>

// === PIN Configuration ===
#define CSpin   2
#define CSpin2  5
#define CLKpin  3
#define DOpin   4
#define SWSerialPin 11

//Note to myself: 
// Aktivieren von softwareserial, nachdem eine Verbindung zu ROS aufgebaut wurde--> workaround: nachdem einige callbacks erfolgt sind. Danach darf kein Neustart des Arduinos erfolgen

SoftwareSerial STSerial(10,SWSerialPin);
// === PID Configuration ===
double Setpoint_left, vel_est_left, Output_left;
double Setpoint_right, vel_est_right, Output_right;
double angle_enc_left_old, angle_enc_right_old;


double KpL=0.52, KiL=6.5, KdL=0.0; //old: kp = 0.58, ki = 8.3
double KpR=0.52, KiR=6.3, KdR=0.0; //kp_old = 0.8, ki_old = 8.5,KiR=6.3, KdR=0.0


PID pidLeftWheel(&vel_est_left, &Output_left, &Setpoint_left,KpL,KiL,KdL, DIRECT);
PID pidRightWheel(&vel_est_right, &Output_right, &Setpoint_right,KpR,KiR,KdR, DIRECT);

// === ROSSerial Subscriber / Publisher / Callbacks ===
ros::NodeHandle  nh;

// publish velocity information. Odometry is computed on the host machine
geometry_msgs::Vector3 vel_msg; 
ros::Publisher cpl_robot_vel("cpl_robot/vel_est", &vel_msg); 

void setpointLeftWheelCb( const std_msgs::Float32& left_vel){
  Setpoint_left = left_vel.data; 
 }
void setpointRightWheelCb( const std_msgs::Float32& right_vel){
  Setpoint_right = right_vel.data;   
 }

ros::Subscriber<std_msgs::Float32> sub_left("cpl_robot/left_wheel_vel_sp", &setpointLeftWheelCb );
ros::Subscriber<std_msgs::Float32> sub_right("cpl_robot/right_wheel_vel_sp", &setpointRightWheelCb );

double dt; 

bool init_vel_est = true;
bool motor_1_act = true;  
 
unsigned long start_millis_vel, start_millis_ctrl; 
//unsigned long start_millis_sim_acc, start_millis_sim_deacc; 
unsigned long current_millis; 
const int sampling_period = 100; 
const int sampling_sim_period = 20;

AS5045 enc (CSpin,CSpin2, CLKpin, DOpin) ;

// === Setpoint Simulation ===
//double acc, v_max; 
//bool start_acc, time_reached;
//int counter; 


void setup ()
{
  //Initializing Variables
  nh.getHardware()->setBaud(115200);
  nh.initNode();
  
  nh.subscribe(sub_left);
  nh.subscribe(sub_right);
  
  nh.advertise(cpl_robot_vel);
  
  vel_est_left = 0;
  vel_est_right = 0; 
 
  Setpoint_left = 0;
  Setpoint_right = 0; 

  Output_left = 0;
  Output_right = 0; 
  
  dt = 0.1; 
 
/*setpoint simulation
v_max = 2; 
acc = 4; 
time_reached = false;
start_acc = true; 
counter = 0; 
start_millis_sim_acc   = millis();  
start_millis_sim_deacc = millis(); 
*/
  
  start_millis_vel = millis();
  start_millis_ctrl = millis(); 

  // pid Config
  pidLeftWheel.SetMode(AUTOMATIC);
  pidRightWheel.SetMode(AUTOMATIC);
  pidLeftWheel.SetTunings(KpL,KiL,KdL);
  pidRightWheel.SetTunings(KpR,KiR,KdR);
  pidLeftWheel.SetOutputLimits(-25,25);
  pidRightWheel.SetOutputLimits(-25,25);

  //Serial.begin (19200) ;
  STSerial.begin(19200);

  if (!enc.begin ())
    vel_est_right = 0; 
}

void loop ()
{    
  current_millis = millis();
  
  // estimate wheel velocities
  if( current_millis - start_millis_vel >= sampling_period )
  {   
    if(init_vel_est) 
    {
      initialize_control();
    }
    else
    {
      double angle_enc_left  = enc.read(1) * 0.08789; // left wheel
      double angle_enc_right = enc.read(2) * 0.08789; // right wheel
      
      vel_est_left  = enc.estimateVelocity(0, angle_enc_left, angle_enc_left_old, dt);
      vel_est_right = enc.estimateVelocity(1, angle_enc_right, angle_enc_right_old, dt);

      pidLeftWheel.Compute();
      pidRightWheel.Compute();

      // reset working PID values (especially important when changing wheel direction)
      if(abs(Setpoint_left) <= 0.02){
        reset_pid_left();
        } 
      if(abs(Setpoint_right) <= 0.02){
        reset_pid_right();
        }

      //Publish Data
      vel_msg.x = vel_est_left;
      vel_msg.y = vel_est_right;
      cpl_robot_vel.publish( & vel_msg ); 

      start_millis_vel = current_millis; 

      //save last angle values
      angle_enc_left_old = angle_enc_left;
      angle_enc_right_old = angle_enc_right; 
   }     
 } 
    

  if( (current_millis - start_millis_ctrl >= 10) )
  {
    if(motor_1_act)
    {
      int offset = 64; 
      sendControlCommand(1, Output_right, offset);
      motor_1_act = false; 
    }
    else
    {
      int offset = 192;
      sendControlCommand(0, Output_left, offset);
      motor_1_act = true;
    }
    start_millis_ctrl = current_millis; 
  }  
  nh.spinOnce();
 }


void initialize_control()
{
  angle_enc_left_old  = enc.read(1) * 0.08789, DEC; // left wheel
  angle_enc_right_old = enc.read(2) * 0.08789, DEC; // right wheel
  Output_left = 0;
  Output_right = 0; 
  init_vel_est = false;
  //int zero = 0; 
  //sendControlCommand(0, zero, zero);
  //sendControlCommand(1, zero, zero);
}



void reset_pid_left(){
  pidLeftWheel.SetOutputLimits(0.0, 1.0);  // Forces minimum up to 0.0
  pidLeftWheel.SetOutputLimits(-1.0, 0.0);  // Forces maximum down to 0.0
  pidLeftWheel.SetOutputLimits(-25 , 25);  // Set the limits back to normal
}
void reset_pid_right(){
  pidRightWheel.SetOutputLimits(0.0, 1.0);  // Forces minimum up to 0.0
  pidRightWheel.SetOutputLimits(-1.0, 0.0);  // Forces maximum down to 0.0
  pidRightWheel.SetOutputLimits(-25 , 25);  // Set the limits back to normal
}


void sendControlCommand(double ang_vel, int offset)
{
/*
Simplified Serial, 38400 Baud: 
A TTL level 8N1 serial data stream is connected to terminal S1. Control is with single byte commands.
Motor 1: 1 is full reverse, 
         64 is stop and
         127 is full forward.
Motor 2: 128 is full reverse, 
         192 is stop
         255 is full forward. 
--> Resolution in each direction: 63, max_angular_velocity of robot: 10,61 rad/s, --> 10,61 / 63 = 0,16841
*/

  int ctrl_command = ang_vel / 0.16841;
  
  if(ctrl_command > 62)
    ctrl_command = 62;
  else if(ctrl_command < (-62))
    ctrl_command = -62;
  else if(abs(ctrl_command) == 1)
    ctrl_command = 0; 

STSerial.write(offset + ctrl_command); 
}

Further information:
I noticed, that when I assign the value zero to the following variables “offset, Output_right, Output_left”
I can upload the sketch or launch rosserial without any problems.

Here are the relevant parts of the code:

Here is the (IMHO) relevant part of my reply: Your problem is simple. Stop

If you think that the relevant part might be in the part I didn't show, you'll need to post ALL of your code, and I'll post all of my answer.

you are probably right. I didnt want to show the code, as it is a mess after I've changed so much in order to find a solution :)