ArbotiX Differential Drive Controller

This module is part of the arbotix_python package.

Module Overview

The diff_controller enables control of differential-drive mobile bases in a way that interfaces to the ROS navigation stack and other commonly available tools. This includes taking velocity commands and publishing odometry and tf information for the navigation stack.

Subscribed Topics

/cmd_vel (geometry_msgs/Twist)
  • Movement commands.

Published Topics

/odom (nav_msgs/Odometry)
  • Odometry of base, for navigation stack.

Parameters

~controllers/<name>/rate (int, default: 10.0)
  • Rate at which to publish odometry topic.
~controllers/<name>/base_width (float)
  • Track width, in meters.
~controllers/<name>/ticks_meter (int)
  • Encoder ticks per meter of travel.
~controllers/<name>/Kp (int, default: 5)
  • Proportional parameter for PID, loaded into ArbotiX at start of node.
~controllers/<name>/Kd (int, default: 1)
  • Derivative parameter for PID, loaded into ArbotiX at start of node.
~controllers/<name>/Ki (int, default: 0)
  • Integral parameter for PID, loaded into ArbotiX at start of node.
~controllers/<name>/Ko (int, default: 50)
  • Scaling parameter for PID, loaded into ArbotiX at start of node.
~controllers/<name>/accel_limit (float, default: 1.0)
  • Limit to acceleration in m/s^2.

Provided tf Transforms

odombase_link
  • Typical transform needed by navigation stack.

PID Parameter Details

The PID loop runs on the ArbotiX, and calculates how to change the PWM output in light of encoder feedback. PWM output is modulated at approximately 10KHz, and can vary from -255 to 255 on. The PID loop runs at 30hz, and thus encoder counts are accumulated for 1/30 of a second. This works well for high-count encoders, and not so well on low-count ones -- future releases may allow the loop rate to be varied. The update to output values is calculated as:

Error = DesiredCount - ActualCount;
output_update = (Kp*Error + Kd*(Error - PreviousError) + Ki*p->Ierror)/Ko;

Where PreviousError is the error from the previous loop, and Ierror is the accumulated error over time. The default values provide good results on the Lynxmotion 12V gearmotors with 100cpr encoders. These values can be tuned to improve how the PID reacts on other robots.

YAML Configuration

ArbotiX-ROS based robots are typically configured through a YAML configuration file. A typical configuration file that includes a diff controller might be:

port: /dev/ttyUSB0
controllers: {
    base_controller: {type: diff_controller, base_width: 0.381, ticks_meter: 81807
}

Here, we specify the base_width (in meters) and the ticks per meter of travel of a wheel. We could additionally specify new K parameters:

port: /dev/ttyUSB1
controllers: {
    base_controller: {type: diff_controller, base_width: 0.381, ticks_meter: 81807, Kp: 10, Kd: 0, Ki: 0, Ko: 25}
}

Wiki: arbotix_python/diff_controller (last edited 2011-09-11 03:04:49 by MichaelFerguson)