Lesson 5: Move control—ros_arduino_bridge

ros_arduino_bridge package includes Arduino libraray (ROSArduinoBridge) and a series of ROS package for control based on Arduino, using standard ROS message and service. Functions in the package are:

  • Supports ping Sonar and Sharp infrared sensors
  • Read data from universal analog and digital signal sensors
  • Control the output of digital signals
  • Supports PWM servo control
  • base controller

In the Diego 1 # robot we mainly use the base controller and PWM servo control, in order to adapt to Diego 1 # selected hardware environment, we need to make the necessary changes to the package, then we step by step to install And transform the package.

1.  ros_arduino_bridge installation

ros_arduino_bridge github:https://github.com/hbrobotics/ros_arduino_bridge

1.1. Download
Go to the src directory in your workspace directory

cd ~/catkin_ws/src
git clone https://github.com/hbrobotics/ros_arduino_bridge.git

1.2. Compile

compile in workspace

cd <catkin_ws>
catkin_make

1.3. Copy Arduino lib

In the ros_arduino_bridge directory there will be a ros_arduino_firmware subdirectory, it's src subdirectory is Arduino library files, and sample code, you can copy to the corresponding Arduino IDE library directory

$ cd SKETCHBOOK_PATH
$ cp -rp  `rospack find ros_arduino_firmware`/src/libraries/ROSArduinoBridge -T ROSArduinoBridge

ROSArduinoBridge can also be copied to other windows, Mac computer Arduino IDE environment, after the restart can be used
这里写图片描述

2.Development in Arduino

2.1.Create diego 1# Arduino project

Open the sample code for ROSArduinoBridge from Example and save it as your favorite project name. We only need to modify the sample code according to our own needs.。
这里写图片描述

2.2.exemple code introduction

  • ROSArduinoBridge.ino main code
  • commands.h Serial command is predefined
  • diff_controller.h PID
  • encoder_driver.h Encoder, where only for Arduino UNO, using the interrupt interface D2, D3, and analog interfaces A2, A3; so the output of the motor encoder wiring in accordance with this rules need to pay attention to the encoder to have two outputs
    The left side of the motor code output D2, D3; the right side of the motor code output A2, A3
  • encoder_driver.ino Encoder
  • motor_driver.h Motor-driven interface definition, with different motor drive board to achieve the definition of the three functions of this document
  • motor_driver.inoMotor driver to achieve the code, according to the predefined choice of different driver board library, where I use the L298P, so you need to implement a new driver library, will be introduced later

  • sensors.h Sensor implementation file
  • servos.hThe implementation of the steering gear

2.3.Code changes

2.3.1 ROSArduinoBridge.ino

The main changes are as follows:

Enable Base Controller

#define USE_BASE      // Enable the base controller code
//#undef USE_BASE     // Disable the base controller code
/* Define the motor controller and encoder library you are using */
#ifdef USE_BASE
   /* The Pololu VNH5019 dual motor driver shield */
   //#define POLOLU_VNH5019

   /* The L298P dual motor driver shield*/
   #define L298P


   /* The Pololu MC33926 dual motor driver shield */
   //#define POLOLU_MC33926

   /* The RoboGaia encoder shield */
   //#define ROBOGAIA

   /* Encoders directly attached to Arduino board */
   #define ARDUINO_ENC_COUNTER
#endif
/* Maximum PWM signal */
#define MAX_PWM        255//最大的PWM为255

2.3.2. motor_driver.ino

Increased support for L298P motor driver boards

/***************************************************************
Motor driver definitions

Add a "#elif defined" block to this file to include support
for a particular motor driver. Then add the appropriate
#define near the top of the main ROSArduinoBridge.ino file.

*************************************************************/
#ifdef USE_BASE

#if defined POLOLU_VNH5019
/* Include the Pololu library */
#include "DualVNH5019MotorShield.h"

/* Create the motor driver object */
DualVNH5019MotorShield drive;

/* Wrap the motor driver initialization */
void initMotorController() {
drive.init();
}

/* Wrap the drive motor set speed function */
void setMotorSpeed(int i, int spd) {
if (i == LEFT) drive.setM1Speed(spd);
else drive.setM2Speed(spd);
}

// A convenience function for setting both motor speeds
void setMotorSpeeds(int leftSpeed, int rightSpeed) {
setMotorSpeed(LEFT, leftSpeed);
setMotorSpeed(RIGHT, rightSpeed);
}
#elif defined POLOLU_MC33926
/* Include the Pololu library */
#include "DualMC33926MotorShield.h"

/* Create the motor driver object */
DualMC33926MotorShield drive;

/* Wrap the motor driver initialization */
void initMotorController() {
drive.init();
}

/* Wrap the drive motor set speed function */
void setMotorSpeed(int i, int spd) {
if (i == LEFT) drive.setM1Speed(spd);
else drive.setM2Speed(spd);
}

// A convenience function for setting both motor speeds
void setMotorSpeeds(int leftSpeed, int rightSpeed) {
setMotorSpeed(LEFT, leftSpeed);
setMotorSpeed(RIGHT, rightSpeed);
}
#elif defined L298P
#include "DualL298PMotorShield.h"

/* Create the motor driver object */
DualL298PMotorShield drive;
/* Wrap the motor driver initialization */
void initMotorController() {
drive.init();
}

/* Wrap the drive motor set speed function */
void setMotorSpeed(int i, int spd) {
if (i == LEFT) drive.setM1Speed(spd);
else drive.setM2Speed(spd);
}

// A convenience function for setting both motor speeds
void setMotorSpeeds(int leftSpeed, int rightSpeed) {
setMotorSpeed(LEFT, leftSpeed);
setMotorSpeed(RIGHT, rightSpeed);
}

#else
#error A motor driver must be selected!
#endif

#endif

2.3.3. ecoder_driver.h修改

/* *************************************************************
Encoder driver function definitions - by James Nugen
************************************************************ */

#ifdef ARDUINO_ENC_COUNTER
//below can be changed, but should be PORTD pins;
//otherwise additional changes in the code are required
#define LEFT_ENC_PIN_A PD2 //pin 2
#define LEFT_ENC_PIN_B PD3 //pin 3

//below can be changed, but should be PORTC pins
#define RIGHT_ENC_PIN_A PC2 //pin A2 
#define RIGHT_ENC_PIN_B PC3 //pin A3 
#endif
long readEncoder(int i);
void resetEncoder(int i);
void resetEncoders();

2.3.4. ecoder_driver.ino修改

/* Interrupt routine for RIGHT encoder, taking care of actual counting */
ISR (PCINT1_vect){
static uint8_t enc_last=0;

enc_last <<=2; //shift previous state two places
enc_last |= (PINC & (3 << 2)) >> 2; //read the current state into lowest 2 bits 

right_enc_pos += ENC_STATES[(enc_last & 0x0f)];
//right_enc_pos=enc_last;
}

2.3.5. L298P Driver

Put the .h and .cpp files in the same directory and copy them to the Arduino IDE library file directory.

DualL298PMotorShield.h代码

#ifndef DualL298PMotorShield_h
#define DualL298PMotorShield_h

#include <Arduino.h>

class DualL298PMotorShield
{
  public:  
    // CONSTRUCTORS
    DualL298PMotorShield(); // Default pin selection.
    DualL298PMotorShield(unsigned char M1DIR, unsigned char M1PWM,
                           unsigned char M2DIR, unsigned char M2PWM); // User-defined pin selection. 

    // PUBLIC METHODS
    void init(); // Initialize TIMER 1, set the PWM to 20kHZ. 
    void setM1Speed(int speed); // Set speed for M1.
    void setM2Speed(int speed); // Set speed for M2.
    void setSpeeds(int m1Speed, int m2Speed); // Set speed for both M1 and M2.

  private:
    static const unsigned char _M1DIR = 4;
    static const unsigned char _M2DIR = 7;
    static const unsigned char _M1PWM = 5;
    static const unsigned char _M2PWM = 6;
};

#endif

DualL298PMotorShield.cpp

#include "DualL298PMotorShield.h"

// Constructors ////////////////////////////////////////////////////////////////

DualL298PMotorShield::DualL298PMotorShield()
{
  //Pin map

}


// Public Methods //////////////////////////////////////////////////////////////
void DualL298PMotorShield::init()
{
// Define pinMode for the pins and set the frequency for timer1.

  pinMode(_M1DIR,OUTPUT);
  pinMode(_M1PWM,OUTPUT);
  pinMode(_M2DIR,OUTPUT);
  pinMode(_M2PWM,OUTPUT);

}
// Set speed for motor 1, speed is a number betwenn -400 and 400
void DualL298PMotorShield::setM1Speed(int speed)
{
  unsigned char reverse = 0;

  if (speed < 0)
  {
    speed = -speed;  // Make speed a positive quantity
    reverse = 1;  // Preserve the direction
  }
  if (speed > 255)  // Max PWM dutycycle
    speed = 255;
  if (reverse)
  {
    digitalWrite(_M1DIR,LOW);
    analogWrite(_M1PWM, speed);
  }
  else
  {
    digitalWrite(_M1DIR,HIGH);
    analogWrite(_M1PWM, speed);
  }    
}

// Set speed for motor 2, speed is a number betwenn -400 and 400
void DualL298PMotorShield::setM2Speed(int speed)
{
  unsigned char reverse = 0;

  if (speed < 0)
  {
    speed = -speed;  // Make speed a positive quantity
    reverse = 1;  // Preserve the direction
  }
  if (speed > 255)  // Max PWM dutycycle
    speed = 255;
  if (reverse)
  {
    digitalWrite(_M2DIR,LOW);
    analogWrite(_M2PWM, speed);
  }
  else
  {
    digitalWrite(_M2DIR,HIGH);
    analogWrite(_M2PWM, speed);
  }
}

// Set speed for motor 1 and 2
void DualL298PMotorShield::setSpeeds(int m1Speed, int m2Speed)
{
  setM1Speed(m1Speed);
  setM2Speed(m2Speed);
}

修改完成后变可以编译upload到Arduino UNO上了。

3.ROS pc development

3.1.Configure your robot parameters
Go to the configuration file directory

$ roscd ros_arduino_python/config

Copy a new configuration file

$ cp arduino_params.yaml my_arduino_params.yaml

Edited by nano

sudo nano my_arduino_params.yaml

Following is my_arduino_params.yaml has been modified,The main change is to enable the base Controller, modify the PID parameters, modify the robot parameters:

# For a direct USB cable connection, the port name is typically
# /dev/ttyACM# where is # is a number such as 0, 1, 2, etc
# For a wireless connection like XBee, the port is typically
# /dev/ttyUSB# where # is a number such as 0, 1, 2, etc.

port: /dev/ttyACM0
baud: 57600
timeout: 0.1

rate: 50
sensorstate_rate: 10

use_base_controller: True
base_controller_rate: 10

# For a robot that uses base_footprint, change base_frame to base_footprint
base_frame: base_link

# === Robot drivetrain parameters
wheel_diameter: 0.02900 
wheel_track: 0.18 
encoder_resolution: 2 
gear_reduction: 75.0 
motors_reversed: True

# === PID parameters
Kp: 10
Kd: 12
Ki: 0
Ko: 50
accel_limit: 1.0

# === Sensor definitions.  Examples only - edit for your robot.
#     Sensor type can be one of the follow (case sensitive!):
#     * Ping
#     * GP2D12
#     * Analog
#     * Digital
#     * PololuMotorCurrent
#     * PhidgetsVoltage
#     * PhidgetsCurrent (20 Amp, DC)



sensors: {
  #motor_current_left:   {pin: 4, type: PololuMotorCurrent, rate: 5},
  #motor_current_right:  {pin: 7, type: PololuMotorCurrent, rate: 5},
  #ir_front_center:      {pin: 2, type: GP2D12, rate: 10},
  #sonar_front_center:   {pin: 5, type: Ping, rate: 10},
  arduino_led:          {pin: 13, type: Digital, rate: 5, direction: output}
}

we can run it ,after modified.

3.2. run

start roscore

roscore

and path to bash

. ~/catkin_ws/devel/setup.bash

start node

roslaunch ros_arduino_python arduino.launch

follow is the sheetshot
这里写图片描述

At this time we can release the Twist message to control the robot's operation, such as:

rostopic pub /cmd_vel geometry_msgs/Twist -r 1 -- '[2.0, 0.0, 0.0]' '[0.0, 0.0, 1.8]'

Run this command, the robot will be rotated in situ, with the following order to view odom information, this information will continue to change


rostopic echo /odom
这里写图片描述

At this point the robot has been able to control according to the Twist message, released for the move base to use the odom information due to the chassis used by the two-wheel drive tracked chassis, which is typical of the differential control chassis, but due to the motor characteristics, the robot's load, And other issues, will lead to the actual speed of the two motors and expectations do not match, there can not go straight line situation, the next course we will continue to modify the ros_arduino_bridge function package to achieve the two motor dual PID speed control mechanism.

Leave a Reply

Scroll to top
%d bloggers like this: