yikestone/ros_arduino_bridge
1
0
Fork 0
mirror of https://github.com/YikeStone/ros_arduino_bridge.git synced 2025-08-04 03:34:08 +05:30
Code Issues Projects Releases Wiki Activity

master branch is history

Browse Source
This commit is contained in:
Michael Ferguson 2013-11-25 14:16:07 -08:00
parent 481760bd42
commit 79e8ab1f52
47 changed files with 5 additions and 2979 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
CMakeLists.txt
View File

@ -1,17 +0,0 @@
cmake_minimum_required(VERSION 2.4.6)
include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
# Append to CPACK_SOURCE_IGNORE_FILES a semicolon-separated list of
# directories (or patterns, but directories should suffice) that should
# be excluded from the distro. This is not the place to put things that
# should be ignored everywhere, like "build" directories; that happens in
# rosbuild/rosbuild.cmake. Here should be listed packages that aren't
# ready for inclusion in a distro.
#
# This list is combined with the list in rosbuild/rosbuild.cmake. Note
# that CMake 2.6 may be required to ensure that the two lists are combined
# properly. CMake 2.4 seems to have unpredictable scoping rules for such
# variables.
#list(APPEND CPACK_SOURCE_IGNORE_FILES /core/experimental)
rosbuild_make_distribution(0.1.0)

1
Makefile
View File

@ -1 +0,0 @@
include $(shell rospack find mk)/cmake_stack.mk

471
README.md
View File

@ -1,469 +1,8 @@
This ROS stack includes an Arduino library (called ROSArduinoBridge) and a collection of ROS packages for controlling an Arduino-based robot using standard ROS messages and services. The stack does **not** depend on ROS Serial. This is an old branch, please update to one of:
Features of the stack include: * groovy-devel - uses rosmake
* hydro-devel - uses catkin, intended for ROS Hydro and later, may work on Groovy.
* Direct support for Ping sonar and Sharp infrared (GP2D12) sensors To checkout a branch
* Can also read data from generic analog and digital sensors git checkout hydro-devel
* Can control digital outputs (e.g. turn a switch or LED on and off)
* Support for PWM servos
* Configurable base controller if using the required hardware
The stack includes a base controller for a differential drive
robot that accepts ROS Twist messages and publishes odometry data back to
the PC. The base controller requires the use of a motor controller and encoders for reading odometry data. The current version of the stack provides support for the following base controller hardware:
* Pololu VNH5019 dual motor controller shield (http://www.pololu.com/catalog/product/2502) or Pololu MC33926 dual motor shield (http://www.pololu.com/catalog/product/2503).
* Robogaia Mega Encoder shield
(http://www.robogaia.com/two-axis-encoder-counter-mega-shield-version-2.html).
**NOTE:** The Robogaia Mega Encoder shield can only be used with an Arduino Mega.
* The library can be easily extended to include support for other motor controllers and encoder hardware or libraries.
Official ROS Documentation
--------------------------
A standard ROS-style version of this documentation can be found on the ROS wiki at:
http://www.ros.org/wiki/ros_arduino_bridge
System Requirements
-------------------
**Python Serial:** To install the python-serial package under Ubuntu, use the command:
$ sudo apt-get install python-serial
On non-Ubuntu systems, use either:
$ sudo pip install --upgrade pyserial
or
$ sudo easy_install -U pyserial
The stack should work with any Arduino-compatible controller for reading sensors and controlling PWM servos. However, to use the base controller, you will need a supported motor controller and encoder hardware as described above. If you do not have this hardware, you can still try the package for reading sensors and controlling servos. See the NOTES section at the end of this document for instructions on how to do this.
To use the base controller you must also install the appropriate libraries for your motor controller and encoders. For the Pololu VNH5019 Dual Motor Shield, the library can be found at:
https://github.com/pololu/Dual-VNH5019-Motor-Shield
For the Pololu MC33926 Dual Motor Shield, the library can be found at:
https://github.com/pololu/dual-mc33926-motor-shield
The Robogaia Mega Encoder library can be found at:
http://www.robogaia.com/uploads/6/8/0/9/6809982/__megaencodercounter-1.3.tar.gz
These libraries should be installed in your standard Arduino
sketchbook/libraries directory.
Finally, it is assumed you are using version 1.0 or greater of the
Arduino IDE.
Preparing your Serial Port under Linux
--------------------------------------
Your Arduino will likely connect to your Linux computer as port /dev/ttyACM# or /dev/ttyUSB# where # is a number like 0, 1, 2, etc., depending on how many other devices are connected. The easiest way to make the determination is to unplug all other USB devices, plug in your Arduino, then run the command:
$ ls /dev/ttyACM*
or
$ ls /dev/ttyUSB*
Hopefully, one of these two commands will return the result you're looking for (e.g. /dev/ttyACM0) and the other will return the error "No such file or directory".
Next you need to make sure you have read/write access to the port. Assuming your Arduino is connected on /dev/ttyACM0, run the command:
$ ls -l /dev/ttyACM0
and you should see an output similar to the following:
crw-rw---- 1 root dialout 166, 0 2013-02-24 08:31 /dev/ttyACM0
Note that only root and the "dialout" group have read/write access. Therefore, you need to be a member of the dialout group. You only have to do this once and it should then work for all USB devices you plug in later on.
To add yourself to the dialout group, run the command:
$ sudo usermod -a -G dialout your_user_name
where your\_user\_name is your Linux login name. You will likely have to log out of your X-window session then log in again, or simply reboot your machine if you want to be sure.
When you log back in again, try the command:
$ groups
and you should see a list of groups you belong to including dialout.
Installation of the ros\_arduino\_bridge Stack
----------------------------------------------
$ cd ~/ros_workspace
$ git clone https://github.com/hbrobotics/ros_arduino_bridge.git
$ cd ros_arduino_bridge
$ rosmake
The provided Arduino library is called ROSArduinoBridge and is
located in the ros\_arduino\_firmware package. This sketch is
specific to the hardware requirements above but it can also be used
with other Arduino-type boards (e.g. Uno) by turning off the base
controller as described in the NOTES section at the end of this
document.
To install the ROSArduinoBridge library, follow these steps:
$ cd SKETCHBOOK_PATH
where SKETCHBOOK_PATH is the path to your Arduino sketchbook directory.
$ cp -rp `rospack find ros_arduino_firmware`/src/libraries/ROSArduinoBridge ROSArduinoBridge
This last command copies the ROSArduinoBridge sketch files into your sketchbook folder. The next section describes how to configure, compile and upload this sketch.
Loading the ROSArduinoBridge Sketch
-----------------------------------
* If you are using the base controller, make sure you have already installed the appropriate motor controller and encoder libraries into your Arduino sketchbook/librariesfolder.
* Launch the Arduino 1.0 IDE and load the ROSArduinoBridge sketch.
You should be able to find it by going to:
File->Sketchbook->ROSArduinoBridge
NOTE: If you don't have the required base controller hardware but
still want to try the code, see the notes at the end of the file.
Choose one of the supported motor controllers by uncommenting its #define statement and commenting out any others. By default, the Pololu VNH5019 driver is chosen.
Choose a supported encoder library by by uncommenting its #define statement and commenting out any others. At the moment, only the Robogaia Mega Encoder shield is supported and it is chosen by default.
If you want to control PWM servos attached to your controller, change
the two lines that look like this:
<pre>
//#define USE_SERVOS
#undef USE_SERVOS
</pre>
to this:
<pre>
#define USE_SERVOS
//#undef USE_SERVOS
</pre>
You must then edit the include file servos.h and change the N_SERVOS
parameter as well as the pin numbers for the servos you have attached.
* Compile and upload the sketch to your Arduino.
Firmware Commands
-----------------
The ROSArduinoLibrary accepts single-letter commands over the serial port for polling sensors, controlling servos, driving the robot, and reading encoders. These commands can be sent to the Arduino over any serial interface, including the Serial Monitor in the Arduino IDE.
**NOTE:** Before trying these commands, set the Serial Monitor baudrate to 57600 and the line terminator to "Carriage return" or "Both NL & CR" using the two pulldown menus on the lower right of the Serial Monitor window.
The list of commands can be found in the file commands.h. The current list includes:
<pre>
#define ANALOG_READ 'a'
#define GET_BAUDRATE 'b'
#define PIN_MODE 'c'
#define DIGITAL_READ 'd'
#define READ_ENCODERS 'e'
#define MOTOR_SPEEDS 'm'
#define PING 'p'
#define RESET_ENCODERS 'r'
#define SERVO_WRITE 's'
#define SERVO_READ 't'
#define UPDATE_PID 'u'
#define DIGITAL_WRITE 'w'
#define ANALOG_WRITE 'x'
</pre>
For example, to get the analog reading on pin 3, use the command:
a 3
To change the mode of digital pin 3 to OUTPUT, send the command:
c 3 1
To get the current encoder counts:
e
To move the robot forward at 20 encoder ticks per second:
m 20 20
Testing your Wiring Connections
-------------------------------
On a differential drive robot, the motors are connected to the motor controller terminals with opposite polarities to each other. Similarly, the A/B leads from the encoders are connected in the reverse sense to each other. However, you still need to make sure that (a) the wheels move forward when given a positive motor speed and (b) that the encoder counts increase when the wheels move forward.
After **placing your robot on blocks**, you can use the Serial Monitor in the Arduino IDE to test both requirements. Use the 'm' command to activate the motors, the 'e' command to get the encoder counts, and the 'r' command to reset the encoders to 0. Remember that at the firmware level, motor speeds are given in encoder ticks per second so that for an encoder resolution of, say 4000 counts per wheel revolution, a command such as 'm 20 20' should move the wheels fairly slowly. (The wheels will only move for 2 seconds which is the default setting for the AUTO\_STOP\_INTERVAL.) Also remember that the first argument is the left motor speed and the second argument is the right motor speed. Similarly, when using the 'e' command, the first number returned is the left encoder count and the second number is the right encoder count.
Finally, you can use the 'r' and 'e' commands to verify the expected encoder counts by rotating the wheels by hand roughly one full turn and checking the reported counts.
Configuring the ros\_arduino\_python Node
-----------------------------------------
Now that your Arduino is running the required sketch, you can
configure the ROS side of things on your PC. You define your robot's
dimensions, PID parameters, and sensor configuration by editing the
YAML file in the directory ros\_arduino\_python/config. So first move
into that directory:
$ roscd ros_arduino_python/config
Now copy the provided config file to one you can modify:
$ cp arduino_params.yaml my_arduino_params.yaml
Bring up your copy of the params file (my\_arduino\_params.yaml) in
your favorite text editor. It should start off looking like this:
<pre>
port: /dev/ttyUSB0
baud: 57600
timeout: 0.1
rate: 50
sensorstate_rate: 10
use_base_controller: False
base_controller_rate: 10
# === Robot drivetrain parameters
#wheel_diameter: 0.146
#wheel_track: 0.2969
#encoder_resolution: 8384 # from Pololu for 131:1 motors
#gear_reduction: 1.0
#motors_reversed: True
# === PID parameters
#Kp: 20
#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: 0, type: PololuMotorCurrent, rate: 5},
#motor_current_right: {pin: 1, 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}
}
</pre>
**NOTE**: Do not use tabs in your .yaml file or the parser will barf it back out when it tries to load it. Always use spaces instead. **ALSO**: When defining your sensor parameters, the last sensor in the list does **not** get a comma (,) at the end of the line but all the rest **must** have a comma.
Let's now look at each section of this file.
_Port Settings_
The port will likely be either /dev/ttyACM0 or /dev/ttyUSB0. Set accordingly.
The MegaRobogaiaPololu Arudino sketch connects at 57600 baud by default.
_Polling Rates_
The main *rate* parameter (50 Hz by default) determines how fast the
outside ROS loop runs. The default should suffice in most cases. In
any event, it should be at least as fast as your fastest sensor rate
(defined below).
The *sensorstate\_rate* determines how often to publish an aggregated
list of all sensor readings. Each sensor also publishes on its own
topic and rate.
The *use\_base\_controller* parameter is set to False by default. Set it to True to use base control (assuming you have the required hardware.) You will also have to set the PID paramters that follow.
The *base\_controller\_rate* determines how often to publish odometry readings.
_Defining Sensors_
The *sensors* parameter defines a dictionary of sensor names and
sensor parameters. (You can name each sensor whatever you like but
remember that the name for a sensor will also become the topic name
for that sensor.)
The four most important parameters are *pin*, *type*, *rate* and *direction*.
The *rate* defines how many times per second you want to poll that
sensor. For example, a voltage sensor might only be polled once a
second (or even once every 2 seconds: rate=0.5), whereas a sonar
sensor might be polled at 20 times per second. The *type* must be one
of those listed (case sensitive!). The default *direction* is input so
to define an output pin, set the direction explicitly to output. In
the example above, the Arduino LED (pin 13) will be turned on and off
at a rate of 2 times per second.
_Setting Drivetrain and PID Parameters_
To use the base controller, you will have to uncomment and set the
robot drivetrain and PID parameters. The sample drivetrain parameters
are for 6" drive wheels that are 11.5" apart. Note that ROS uses
meters for distance so convert accordingly. The sample encoder
resolution (ticks per revolution) is from the specs for the Pololu
131:1 motor. Set the appropriate number for your motor/encoder
combination. Set the motors_reversed to True if you find your wheels
are turning backward, otherwise set to False.
The PID parameters are trickier to set. You can start with the sample
values but be sure to place your robot on blocks before sending it
your first Twist command.
Launching the ros\_arduino\_python Node
---------------------------------------
Take a look at the launch file arduino.launch in the
ros\_arduino\_python/launch directory. As you can see, it points to a
config file called my\_arduino\_params.yaml. If you named your config
file something different, change the name in the launch file.
With your Arduino connected and running the MegaRobogaiaPololu sketch,
launch the ros\_arduino\_python node with your parameters:
$ roslaunch ros_arduino_python arduino.launch
You should see something like the following output:
<pre>
process[arduino-1]: started with pid [6098]
Connecting to Arduino on port /dev/ttyUSB0 ...
Connected at 57600
Arduino is ready.
[INFO] [WallTime: 1355498525.954491] Connected to Arduino on port /dev/ttyUSB0 at 57600 baud
[INFO] [WallTime: 1355498525.966825] motor_current_right {'rate': 5, 'type': 'PololuMotorCurrent', 'pin': 1}
[INFO]
etc
</pre>
If you have any Ping sonar sensors on your robot and you defined them
in your config file, they should start flashing to indicate you have
made the connection.
Viewing Sensor Data
-------------------
To see the aggregated sensor data, echo the sensor state topic:
$ rostopic echo /arduino/sensor_state
To see the data on any particular sensor, echo its topic name:
$ rostopic echo /arduino/sensor/sensor_name
For example, if you have a sensor called ir\_front\_center, you can see
its data using:
$ rostopic echo /arduino/sensor/ir_front_center
You can also graph the range data using rxplot:
$ rxplot -p 60 /arduino/sensor/ir_front_center/range
Sending Twist Commands and Viewing Odometry Data
------------------------------------------------
Place your robot on blocks, then try publishing a Twist command:
$ rostopic pub -1 /cmd_vel geometry_msgs/Twist '{ angular: {z: 0.5} }'
The wheels should turn in a direction consistent with a
counter-clockwise rotation (right wheel forward, left wheel backward).
If they turn in the opposite direction, set the motors_reversed
parameter in your config file to the opposite of its current setting,
then kill and restart the arduino.launch file.
Stop the robot with the command:
$ rostopic pub -1 /cmd_vel geometry_msgs/Twist '{}'
To view odometry data:
$ rostopic echo /odom
or
$ rxplot -p 60 /odom/pose/pose/position/x:y, /odom/twist/twist/linear/x, /odom/twist/twist/angular/z
ROS Services
------------
The ros\_arduino\_python package also defines a few ROS services as follows:
**digital\_set\_direction** - set the direction of a digital pin
$ rosservice call /arduino/digital_set_direction pin direction
where pin is the pin number and direction is 0 for input and 1 for output.
**digital\_write** - send a LOW (0) or HIGH (1) signal to a digital pin
$ rosservice call /arduino/digital_write pin value
where pin is the pin number and value is 0 for LOW and 1 for HIGH.
**servo\_write** - set the position of a servo
$ rosservice call /arduino/servo_write id pos
where id is the index of the servo as defined in the Arduino sketch (servos.h) and pos is the position in degrees (0 - 180).
**servo\_read** - read the position of a servo
$ rosservice call /arduino/servo_read id
where id is the index of the servo as defined in the Arduino sketch (servos.h)
NOTES
-----
If you do not have the hardware required to run the base controller,
follow the instructions below so that you can still use your
Arduino-compatible controller to read sensors and control PWM servos.
First, you need to edit the ROSArduinoBridge sketch. At the top of
the file, change the two lines that look like this:
<pre>
#define USE_BASE
//#undef USE_BASE
</pre>
to this:
<pre>
//#define USE_BASE
#undef USE_BASE
</pre>
**NOTE:** You also need to comment out the line that looks like this in the file encoder_driver.ino:
#include "MegaEncoderCounter.h"
so it looks like this:
//#include "MegaEncoderCounter.h"
Compile the changes and upload to your controller.
Next, edit your my\_arduino_params.yaml file and make sure the
use\_base\_controller parameter is set to False. That's all there is to it.

30
ros_arduino_firmware/CMakeLists.txt
View File

@ -1,30 +0,0 @@
cmake_minimum_required(VERSION 2.4.6)
include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
# Set the build type. Options are:
# Coverage : w/ debug symbols, w/o optimization, w/ code-coverage
# Debug : w/ debug symbols, w/o optimization
# Release : w/o debug symbols, w/ optimization
# RelWithDebInfo : w/ debug symbols, w/ optimization
# MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries
#set(ROS_BUILD_TYPE RelWithDebInfo)
rosbuild_init()
#set the default path for built executables to the "bin" directory
set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
#set the default path for built libraries to the "lib" directory
set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
#uncomment if you have defined messages
#rosbuild_genmsg()
#uncomment if you have defined services
#rosbuild_gensrv()
#common commands for building c++ executables and libraries
#rosbuild_add_library(${PROJECT_NAME} src/example.cpp)
#target_link_libraries(${PROJECT_NAME} another_library)
#rosbuild_add_boost_directories()
#rosbuild_link_boost(${PROJECT_NAME} thread)
#rosbuild_add_executable(example examples/example.cpp)
#target_link_libraries(example ${PROJECT_NAME})

1
ros_arduino_firmware/Makefile
View File

@ -1 +0,0 @@
include $(shell rospack find mk)/cmake.mk

26
ros_arduino_firmware/mainpage.dox
View File

@ -1,26 +0,0 @@
/**
\mainpage
\htmlinclude manifest.html
\b ros_arduino_firmware is ...
<!--
Provide an overview of your package.
-->
\section codeapi Code API
<!--
Provide links to specific auto-generated API documentation within your
package that is of particular interest to a reader. Doxygen will
document pretty much every part of your code, so do your best here to
point the reader to the actual API.
If your codebase is fairly large or has different sets of APIs, you
should use the doxygen 'group' tag to keep these APIs together. For
example, the roscpp documentation has 'libros' group.
-->
*/

14
ros_arduino_firmware/manifest.xml
View File

@ -1,14 +0,0 @@
<package>
<description brief="ros_arduino_firmware">
ROS Arduino Firmware
</description>
<author>Patrick Goebel</author>
<license>BSD</license>
<review status="unreviewed" notes=""/>
<url>http://ros.org/wiki/ros_arduino_firmware</url>
</package>

341
ros_arduino_firmware/src/libraries/MegaRobogaiaPololu/MegaRobogaiaPololu.ino
View File

@ -1,341 +0,0 @@
/*********************************************************************
* ROSArduinoBridge
A set of simple serial commands to control a differential drive
robot and receive back sensor and odometry data. Default
configuration assumes use of an Arduino Mega + Pololu motor
controller shield + Robogaia Mega Encoder shield. Edit the
readEncoder() and setMotorSpeed() wrapper functions if using
different motor controller or encoder method.
Created for the Pi Robot Project: http://www.pirobot.org
Inspired and modeled after the ArbotiX driver by Michael Ferguson
Software License Agreement (BSD License)
Copyright (c) 2012, Patrick Goebel.
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions
are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above
copyright notice, this list of conditions and the following
disclaimer in the documentation and/or other materials provided
with the distribution.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
#define USE_BASE // Enable the base controller code
//#undef USE_BASE // Disable the base controller code
//#define USE_SERVOS // Enable use of PWM servos as defined in servos.h
#undef USE_SERVOS // Disable use of PWM servos
/* Serial port baud rate */
#define BAUDRATE 57600
/* Maximum PWM signal */
#define MAX_PWM 255
#if defined(ARDUINO) && ARDUINO >= 100
#include "Arduino.h"
#else
#include "WProgram.h"
#endif
/* Include definition of serial commands */
#include "commands.h"
/* Sensor functions */
#include "sensors.h"
/* Include servo support if required */
#ifdef USE_SERVOS
#include <Servo.h>
#include "servos.h"
#endif
#ifdef USE_BASE
/* The Pololu motor driver shield */
#include "DualVNH5019MotorShield.h"
/* The Robogaia Mega Encoder shield */
#include "MegaEncoderCounter.h"
/* Create the motor driver object */
DualVNH5019MotorShield drive;
/* Create the encoder shield object */
MegaEncoderCounter encoders(4); // Initializes the Mega Encoder Counter in the 4X Count mode
/* PID parameters and functions */
#include "diff_controller.h"
/* Run the PID loop at 30 times per second */
#define PID_RATE 30 // Hz
/* Convert the rate into an interval */
const int PID_INTERVAL = 1000 / PID_RATE;
/* Track the next time we make a PID calculation */
unsigned long nextPID = PID_INTERVAL;
/* Stop the robot if it hasn't received a movement command
in this number of milliseconds */
#define AUTO_STOP_INTERVAL 2000
long lastMotorCommand = AUTO_STOP_INTERVAL;
/* Wrap the encoder reading function */
long readEncoder(int i) {
if (i == LEFT) return encoders.YAxisGetCount();
else return encoders.XAxisGetCount();
}
/* Wrap the encoder reset function */
void resetEncoder(int i) {
if (i == LEFT) return encoders.YAxisReset();
else return encoders.XAxisReset();
}
/* Wrap the encoder reset function */
void resetEncoders() {
resetEncoder(LEFT);
resetEncoder(RIGHT);
}
/* 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);
}
#endif
/* Variable initialization */
// A pair of varibles to help parse serial commands (thanks Fergs)
int arg = 0;
int index = 0;
// Variable to hold an input character
char chr;
// Variable to hold the current single-character command
char cmd;
// Character arrays to hold the first and second arguments
char argv1[16];
char argv2[16];
// The arguments converted to integers
long arg1;
long arg2;
/* Clear the current command parameters */
void resetCommand() {
cmd = NULL;
memset(argv1, 0, sizeof(argv1));
memset(argv2, 0, sizeof(argv2));
arg1 = 0;
arg2 = 0;
arg = 0;
index = 0;
}
/* Run a command. Commands are defined in commands.h */
int runCommand() {
int i = 0;
char *p = argv1;
char *str;
int pid_args[4];
arg1 = atoi(argv1);
arg2 = atoi(argv2);
switch(cmd) {
case GET_BAUDRATE:
Serial.println(BAUDRATE);
break;
case ANALOG_READ:
Serial.println(analogRead(arg1));
break;
case DIGITAL_READ:
Serial.println(digitalRead(arg1));
break;
case ANALOG_WRITE:
analogWrite(arg1, arg2);
Serial.println("OK");
break;
case DIGITAL_WRITE:
if (arg2 == 0) digitalWrite(arg1, LOW);
else if (arg2 == 1) digitalWrite(arg1, HIGH);
Serial.println("OK");
break;
case PIN_MODE:
if (arg2 == 0) pinMode(arg1, INPUT);
else if (arg2 == 1) pinMode(arg1, OUTPUT);
Serial.println("OK");
break;
case PING:
Serial.println(Ping(arg1));
break;
#ifdef USE_SERVOS
case SERVO_WRITE:
servos[arg1].write(arg2);
Serial.println("OK");
break;
case SERVO_READ:
Serial.println(servos[arg1].read());
break;
#endif
#ifdef USE_BASE
case READ_ENCODERS:
Serial.print(readEncoder(LEFT));
Serial.print(" ");
Serial.println(readEncoder(RIGHT));
break;
case RESET_ENCODERS:
resetEncoders();
Serial.println("OK");
break;
case MOTOR_SPEEDS:
/* Reset the auto stop timer */
lastMotorCommand = millis();
if (arg1 == 0 && arg2 == 0) {
setMotorSpeeds(0, 0);
moving = 0;
}
else moving = 1;
leftPID.TargetTicksPerFrame = arg1;
rightPID.TargetTicksPerFrame = arg2;
Serial.println("OK");
break;
case UPDATE_PID:
while ((str = strtok_r(p, ":", &p)) != '\0') {
pid_args[i] = atoi(str);
i++;
}
Kp = pid_args[0];
Kd = pid_args[1];
Ki = pid_args[2];
Ko = pid_args[3];
Serial.println("OK");
break;
#endif
default:
Serial.println("Invalid Command");
break;
}
}
/* Setup function--runs once at startup. */
void setup() {
Serial.begin(BAUDRATE);
// Initialize the motor controller if used */
#ifdef USE_BASE
initMotorController();
#endif
/* Attach servos if used */
#ifdef USE_SERVOS
int i;
for (i = 0; i < N_SERVOS; i++) {
servos[i].attach(servoPins[i]);
}
#endif
}
/* Enter the main loop. Read and parse input from the serial port
and run any valid commands. Run a PID calculation at the target
interval and check for auto-stop conditions.
*/
void loop() {
while (Serial.available() > 0) {
// Read the next character
chr = Serial.read();
// Terminate a command with a CR
if (chr == 13) {
if (arg == 1) argv1[index] = NULL;
else if (arg == 2) argv2[index] = NULL;
runCommand();
resetCommand();
}
// Use spaces to delimit parts of the command
else if (chr == ' ') {
// Step through the arguments
if (arg == 0) arg = 1;
else if (arg == 1) {
argv1[index] = NULL;
arg = 2;
index = 0;
}
continue;
}
else {
if (arg == 0) {
// The first arg is the single-letter command
cmd = chr;
}
else if (arg == 1) {
// Subsequent arguments can be more than one character
argv1[index] = chr;
index++;
}
else if (arg == 2) {
argv2[index] = chr;
index++;
}
}
}
// If we are using base control, run a PID calculation at the appropriate intervals
#ifdef USE_BASE
if (millis() > nextPID) {
updatePID();
nextPID += PID_INTERVAL;
}
// Check to see if we have exceeded the auto-stop interval
if ((millis() - lastMotorCommand) > AUTO_STOP_INTERVAL) {;
setMotorSpeeds(0, 0);
moving = 0;
}
#endif
}

22
ros_arduino_firmware/src/libraries/MegaRobogaiaPololu/commands.h
View File

@ -1,22 +0,0 @@
/* Define single-letter commands that will be sent by the PC over the
serial link.
*/
#define ANALOG_READ 'a'
#define GET_BAUDRATE 'b'
#define PIN_MODE 'c'
#define DIGITAL_READ 'd'
#define READ_ENCODERS 'e'
#define MOTOR_SPEEDS 'm'
#define PING 'p'
#define RESET_ENCODERS 'r'
#define SERVO_WRITE 's'
#define SERVO_READ 't'
#define UPDATE_PID 'u'
#define DIGITAL_WRITE 'w'
#define ANALOG_WRITE 'x'
#define LEFT 0
#define RIGHT 1

71
ros_arduino_firmware/src/libraries/MegaRobogaiaPololu/diff_controller.h
View File

@ -1,71 +0,0 @@
/* Functions and type-defs for PID control.
Taken mostly from Mike Ferguson's ArbotiX code which lives at:
http://vanadium-ros-pkg.googlecode.com/svn/trunk/arbotix/
*/
/* PID setpoint info For a Motor */
typedef struct {
double TargetTicksPerFrame; // target speed in ticks per frame
long Encoder; // encoder count
long PrevEnc; // last encoder count
int PrevErr; // last error
int Ierror; // integrated error
int output; // last motor setting
}
SetPointInfo;
SetPointInfo leftPID, rightPID;
/* PID Parameters */
int Kp = 20;
int Kd = 12;
int Ki = 0;
int Ko = 50;
unsigned char moving = 0; // is the base in motion?
/* PID routine to compute the next motor commands */
void doPID(SetPointInfo * p) {
long Perror;
long output;
Perror = p->TargetTicksPerFrame - (p->Encoder - p->PrevEnc);
// Derivative error is the delta Perror
output = (Kp * Perror + Kd * (Perror - p->PrevErr) + Ki * p->Ierror) / Ko;
p->PrevErr = Perror;
p->PrevEnc = p->Encoder;
output += p->output;
// Accumulate Integral error *or* Limit output.
// Stop accumulating when output saturates
if (output >= MAX_PWM)
output = MAX_PWM;
else if (output <= -MAX_PWM)
output = -MAX_PWM;
else
p->Ierror += Perror;
p->output = output;
}
/* Read the encoder values and call the PID routine */
void updatePID() {
/* Read the encoders */
leftPID.Encoder = readEncoder(0);
rightPID.Encoder = readEncoder(1);
/* If we're not moving there is nothing more to do */
if (!moving)
return;
/* Compute PID update for each motor */
doPID(&rightPID);
doPID(&leftPID);
/* Set the motor speeds accordingly */
setMotorSpeeds(leftPID.output, rightPID.output);
}

34
ros_arduino_firmware/src/libraries/MegaRobogaiaPololu/sensors.h
View File

@ -1,34 +0,0 @@
/* Functions for various sensor types */
float microsecondsToCm(long microseconds)
{
// The speed of sound is 340 m/s or 29 microseconds per cm.
// The ping travels out and back, so to find the distance of the
// object we take half of the distance travelled.
return microseconds / 29 / 2;
}
long Ping(int pin) {
long duration, range;
// The PING))) is triggered by a HIGH pulse of 2 or more microseconds.
// Give a short LOW pulse beforehand to ensure a clean HIGH pulse:
pinMode(pin, OUTPUT);
digitalWrite(pin, LOW);
delayMicroseconds(2);
digitalWrite(pin, HIGH);
delayMicroseconds(5);
digitalWrite(pin, LOW);
// The same pin is used to read the signal from the PING))): a HIGH
// pulse whose duration is the time (in microseconds) from the sending
// of the ping to the reception of its echo off of an object.
pinMode(pin, INPUT);
duration = pulseIn(pin, HIGH);
// convert the time into meters
range = microsecondsToCm(duration);
return(range);
}

16
ros_arduino_firmware/src/libraries/MegaRobogaiaPololu/servos.h
View File

@ -1,16 +0,0 @@
/* Define the attachment of any servos here.
The example shows two servos attached on pins 3 and 5.
*/
#define N_SERVOS 2
Servo servos [N_SERVOS];
byte servoPins [N_SERVOS] = {3, 5};

317
ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino
View File

@ -1,317 +0,0 @@
/*********************************************************************
* ROSArduinoBridge
A set of simple serial commands to control a differential drive
robot and receive back sensor and odometry data. Default
configuration assumes use of an Arduino Mega + Pololu motor
controller shield + Robogaia Mega Encoder shield. Edit the
readEncoder() and setMotorSpeed() wrapper functions if using
different motor controller or encoder method.
Created for the Pi Robot Project: http://www.pirobot.org
and the Home Brew Robotics Club (HBRC): http://hbrobotics.org
Authors: Patrick Goebel, James Nugen
Inspired and modeled after the ArbotiX driver by Michael Ferguson
Software License Agreement (BSD License)
Copyright (c) 2012, Patrick Goebel.
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions
are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above
copyright notice, this list of conditions and the following
disclaimer in the documentation and/or other materials provided
with the distribution.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
#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 Pololu MC33926 dual motor driver shield */
//#define POLOLU_MC33926
/* The RoboGaia encoder shield */
#define ROBOGAIA
#endif
//#define USE_SERVOS // Enable use of PWM servos as defined in servos.h
#undef USE_SERVOS // Disable use of PWM servos
/* Serial port baud rate */
#define BAUDRATE 57600
/* Maximum PWM signal */
#define MAX_PWM 255
#if defined(ARDUINO) && ARDUINO >= 100
#include "Arduino.h"
#else
#include "WProgram.h"
#endif
/* Include definition of serial commands */
#include "commands.h"
/* Sensor functions */
#include "sensors.h"
/* Include servo support if required */
#ifdef USE_SERVOS
#include <Servo.h>
#include "servos.h"
#endif
#ifdef USE_BASE
/* Motor driver function definitions */
#include "motor_driver.h"
/* Encoder driver function definitions */
#include "encoder_driver.h"
/* PID parameters and functions */
#include "diff_controller.h"
/* Run the PID loop at 30 times per second */
#define PID_RATE 30 // Hz
/* Convert the rate into an interval */
const int PID_INTERVAL = 1000 / PID_RATE;
/* Track the next time we make a PID calculation */
unsigned long nextPID = PID_INTERVAL;
/* Stop the robot if it hasn't received a movement command
in this number of milliseconds */
#define AUTO_STOP_INTERVAL 2000
long lastMotorCommand = AUTO_STOP_INTERVAL;
#endif
/* Variable initialization */
// A pair of varibles to help parse serial commands (thanks Fergs)
int arg = 0;
int index = 0;
// Variable to hold an input character
char chr;
// Variable to hold the current single-character command
char cmd;
// Character arrays to hold the first and second arguments
char argv1[16];
char argv2[16];
// The arguments converted to integers
long arg1;
long arg2;
/* Clear the current command parameters */
void resetCommand() {
cmd = NULL;
memset(argv1, 0, sizeof(argv1));
memset(argv2, 0, sizeof(argv2));
arg1 = 0;
arg2 = 0;
arg = 0;
index = 0;
}
/* Run a command. Commands are defined in commands.h */
int runCommand() {
int i = 0;
char *p = argv1;
char *str;
int pid_args[4];
arg1 = atoi(argv1);
arg2 = atoi(argv2);
switch(cmd) {
case GET_BAUDRATE:
Serial.println(BAUDRATE);
break;
case ANALOG_READ:
Serial.println(analogRead(arg1));
break;
case DIGITAL_READ:
Serial.println(digitalRead(arg1));
break;
case ANALOG_WRITE:
analogWrite(arg1, arg2);
Serial.println("OK");
break;
case DIGITAL_WRITE:
if (arg2 == 0) digitalWrite(arg1, LOW);
else if (arg2 == 1) digitalWrite(arg1, HIGH);
Serial.println("OK");
break;
case PIN_MODE:
if (arg2 == 0) pinMode(arg1, INPUT);
else if (arg2 == 1) pinMode(arg1, OUTPUT);
Serial.println("OK");
break;
case PING:
Serial.println(Ping(arg1));
break;
#ifdef USE_SERVOS
case SERVO_WRITE:
servos[arg1].write(arg2);
Serial.println("OK");
break;
case SERVO_READ:
Serial.println(servos[arg1].read());
break;
#endif
#ifdef USE_BASE
case READ_ENCODERS:
Serial.print(readEncoder(LEFT));
Serial.print(" ");
Serial.println(readEncoder(RIGHT));
break;
case RESET_ENCODERS:
resetEncoders();
resetPID();
Serial.println("OK");
break;
case MOTOR_SPEEDS:
/* Reset the auto stop timer */
lastMotorCommand = millis();
if (arg1 == 0 && arg2 == 0) {
setMotorSpeeds(0, 0);
moving = 0;
}
else moving = 1;
leftPID.TargetTicksPerFrame = arg1;
rightPID.TargetTicksPerFrame = arg2;
Serial.println("OK");
break;
case UPDATE_PID:
while ((str = strtok_r(p, ":", &p)) != '\0') {
pid_args[i] = atoi(str);
i++;
}
Kp = pid_args[0];
Kd = pid_args[1];
Ki = pid_args[2];
Ko = pid_args[3];
Serial.println("OK");
break;
#endif
default:
Serial.println("Invalid Command");
break;
}
}
/* Setup function--runs once at startup. */
void setup() {
Serial.begin(BAUDRATE);
// Initialize the motor controller if used */
#ifdef USE_BASE
initMotorController();
resetPID();
#endif
/* Attach servos if used */
#ifdef USE_SERVOS
int i;
for (i = 0; i < N_SERVOS; i++) {
servos[i].attach(servoPins[i]);
}
#endif
}
/* Enter the main loop. Read and parse input from the serial port
and run any valid commands. Run a PID calculation at the target
interval and check for auto-stop conditions.
*/
void loop() {
while (Serial.available() > 0) {
// Read the next character
chr = Serial.read();
// Terminate a command with a CR
if (chr == 13) {
if (arg == 1) argv1[index] = NULL;
else if (arg == 2) argv2[index] = NULL;
runCommand();
resetCommand();
}
// Use spaces to delimit parts of the command
else if (chr == ' ') {
// Step through the arguments
if (arg == 0) arg = 1;
else if (arg == 1) {
argv1[index] = NULL;
arg = 2;
index = 0;
}
continue;
}
else {
if (arg == 0) {
// The first arg is the single-letter command
cmd = chr;
}
else if (arg == 1) {
// Subsequent arguments can be more than one character
argv1[index] = chr;
index++;
}
else if (arg == 2) {
argv2[index] = chr;
index++;
}
}
}
// If we are using base control, run a PID calculation at the appropriate intervals
#ifdef USE_BASE
if (millis() > nextPID) {
updatePID();
nextPID += PID_INTERVAL;
}
// Check to see if we have exceeded the auto-stop interval
if ((millis() - lastMotorCommand) > AUTO_STOP_INTERVAL) {;
setMotorSpeeds(0, 0);
moving = 0;
}
#endif
}

26
ros_arduino_firmware/src/libraries/ROSArduinoBridge/commands.h
View File

@ -1,26 +0,0 @@
/* Define single-letter commands that will be sent by the PC over the
serial link.
*/
#ifndef COMMANDS_H
#define COMMANDS_H
#define ANALOG_READ 'a'
#define GET_BAUDRATE 'b'
#define PIN_MODE 'c'
#define DIGITAL_READ 'd'
#define READ_ENCODERS 'e'
#define MOTOR_SPEEDS 'm'
#define PING 'p'
#define RESET_ENCODERS 'r'
#define SERVO_WRITE 's'
#define SERVO_READ 't'
#define UPDATE_PID 'u'
#define DIGITAL_WRITE 'w'
#define ANALOG_WRITE 'x'
#define LEFT 0
#define RIGHT 1
#endif

130
ros_arduino_firmware/src/libraries/ROSArduinoBridge/diff_controller.h
View File

@ -1,130 +0,0 @@
/* Functions and type-defs for PID control.
Taken mostly from Mike Ferguson's ArbotiX code which lives at:
http://vanadium-ros-pkg.googlecode.com/svn/trunk/arbotix/
*/
/* PID setpoint info For a Motor */
typedef struct {
double TargetTicksPerFrame; // target speed in ticks per frame
long Encoder; // encoder count
long PrevEnc; // last encoder count
/*
* Using previous input (PrevInput) instead of PrevError to avoid derivative kick,
* see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-derivative-kick/
*/
int PrevInput; // last input
//int PrevErr; // last error
/*
* Using integrated term (ITerm) instead of integrated error (Ierror),
* to allow tuning changes,
* see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-tuning-changes/
*/
//int Ierror;
int ITerm; //integrated term
long output; // last motor setting
}
SetPointInfo;
SetPointInfo leftPID, rightPID;
/* PID Parameters */
int Kp = 20;
int Kd = 12;
int Ki = 0;
int Ko = 50;
unsigned char moving = 0; // is the base in motion?
/*
* Initialize PID variables to zero to prevent startup spikes
* when turning PID on to start moving
* In particular, assign both Encoder and PrevEnc the current encoder value
* See http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-initialization/
* Note that the assumption here is that PID is only turned on
* when going from stop to moving, that's why we can init everything on zero.
*/
void resetPID(){
leftPID.TargetTicksPerFrame = 0.0;
leftPID.Encoder = readEncoder(0);
leftPID.PrevEnc = leftPID.Encoder;
leftPID.output = 0;
leftPID.PrevInput = 0;
leftPID.ITerm = 0;
rightPID.TargetTicksPerFrame = 0.0;
rightPID.Encoder = readEncoder(1);
rightPID.PrevEnc = rightPID.Encoder;
rightPID.output = 0;
rightPID.PrevInput = 0;
rightPID.ITerm = 0;
}
/* PID routine to compute the next motor commands */
void doPID(SetPointInfo * p) {
long Perror;
long output;
int input;
//Perror = p->TargetTicksPerFrame - (p->Encoder - p->PrevEnc);
input = p->Encoder - p->PrevEnc;
Perror = p->TargetTicksPerFrame - input;
/*
* Avoid derivative kick and allow tuning changes,
* see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-derivative-kick/
* see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-tuning-changes/
*/
//output = (Kp * Perror + Kd * (Perror - p->PrevErr) + Ki * p->Ierror) / Ko;
// p->PrevErr = Perror;
output = (Kp * Perror - Kd * (input - p->PrevInput) + p->ITerm) / Ko;
p->PrevEnc = p->Encoder;
output += p->output;
// Accumulate Integral error *or* Limit output.
// Stop accumulating when output saturates
if (output >= MAX_PWM)
output = MAX_PWM;
else if (output <= -MAX_PWM)
output = -MAX_PWM;
else
/*
* allow turning changes, see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-tuning-changes/
*/
p->ITerm += Ki * Perror;
p->output = output;
p->PrevInput = input;
}
/* Read the encoder values and call the PID routine */
void updatePID() {
/* Read the encoders */
leftPID.Encoder = readEncoder(0);
rightPID.Encoder = readEncoder(1);
/* If we're not moving there is nothing more to do */
if (!moving){
/*
* Reset PIDs once, to prevent startup spikes,
* see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-initialization/
* PrevInput is considered a good proxy to detect
* whether reset has already happened
*/
if (leftPID.PrevInput != 0 || rightPID.PrevInput != 0) resetPID();
return;
}
/* Compute PID update for each motor */
doPID(&rightPID);
doPID(&leftPID);
/* Set the motor speeds accordingly */
setMotorSpeeds(leftPID.output, rightPID.output);
}

8
ros_arduino_firmware/src/libraries/ROSArduinoBridge/encoder_driver.h
View File

@ -1,8 +0,0 @@
/* *************************************************************
Encoder driver function definitions - by James Nugen
************************************************************ */
long readEncoder(int i);
void resetEncoder(int i);
void resetEncoders();

41
ros_arduino_firmware/src/libraries/ROSArduinoBridge/encoder_driver.ino
View File

@ -1,41 +0,0 @@
/* *************************************************************
Encoder definitions
Add a "#if defined" block to this file to include support for
a particular encoder board or library. Then add the appropriate
#define near the top of the main ROSArduinoBridge.ino file.
************************************************************ */
#ifdef USE_BASE
#if defined ROBOGAIA
/* The Robogaia Mega Encoder shield */
#include "MegaEncoderCounter.h"
/* Create the encoder shield object */
MegaEncoderCounter encoders = MegaEncoderCounter(4); // Initializes the Mega Encoder Counter in the 4X Count mode
/* Wrap the encoder reading function */
long readEncoder(int i) {
if (i == LEFT) return encoders.YAxisGetCount();
else return encoders.XAxisGetCount();
}
/* Wrap the encoder reset function */
void resetEncoder(int i) {
if (i == LEFT) return encoders.YAxisReset();
else return encoders.XAxisReset();
}
#else
#error A encoder driver must be selected!
#endif
/* Wrap the encoder reset function */
void resetEncoders() {
resetEncoder(LEFT);
resetEncoder(RIGHT);
}
#endif

7
ros_arduino_firmware/src/libraries/ROSArduinoBridge/motor_driver.h
View File

@ -1,7 +0,0 @@
/***************************************************************
Motor driver function definitions - by James Nugen
*************************************************************/
void initMotorController();
void setMotorSpeed(int i, int spd);
void setMotorSpeeds(int leftSpeed, int rightSpeed);

62
ros_arduino_firmware/src/libraries/ROSArduinoBridge/motor_driver.ino
View File

@ -1,62 +0,0 @@
/***************************************************************
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);
}
#else
#error A motor driver must be selected!
#endif
#endif

34
ros_arduino_firmware/src/libraries/ROSArduinoBridge/sensors.h
View File

@ -1,34 +0,0 @@
/* Functions for various sensor types */
float microsecondsToCm(long microseconds)
{
// The speed of sound is 340 m/s or 29 microseconds per cm.
// The ping travels out and back, so to find the distance of the
// object we take half of the distance travelled.
return microseconds / 29 / 2;
}
long Ping(int pin) {
long duration, range;
// The PING))) is triggered by a HIGH pulse of 2 or more microseconds.
// Give a short LOW pulse beforehand to ensure a clean HIGH pulse:
pinMode(pin, OUTPUT);
digitalWrite(pin, LOW);
delayMicroseconds(2);
digitalWrite(pin, HIGH);
delayMicroseconds(5);
digitalWrite(pin, LOW);
// The same pin is used to read the signal from the PING))): a HIGH
// pulse whose duration is the time (in microseconds) from the sending
// of the ping to the reception of its echo off of an object.
pinMode(pin, INPUT);
duration = pulseIn(pin, HIGH);
// convert the time into meters
range = microsecondsToCm(duration);
return(range);
}

9
ros_arduino_firmware/src/libraries/ROSArduinoBridge/servos.h
View File

@ -1,9 +0,0 @@
/* Define the attachment of any servos here.
The example shows two servos attached on pins 3 and 5.
*/
#define N_SERVOS 2
Servo servos [N_SERVOS];
byte servoPins [N_SERVOS] = {3, 5};

30
ros_arduino_msgs/CMakeLists.txt
View File

@ -1,30 +0,0 @@
cmake_minimum_required(VERSION 2.4.6)
include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
# Set the build type. Options are:
# Coverage : w/ debug symbols, w/o optimization, w/ code-coverage
# Debug : w/ debug symbols, w/o optimization
# Release : w/o debug symbols, w/ optimization
# RelWithDebInfo : w/ debug symbols, w/ optimization
# MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries
#set(ROS_BUILD_TYPE RelWithDebInfo)
rosbuild_init()
#set the default path for built executables to the "bin" directory
set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
#set the default path for built libraries to the "lib" directory
set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
#uncomment if you have defined messages
rosbuild_genmsg()
#uncomment if you have defined services
rosbuild_gensrv()
#common commands for building c++ executables and libraries
#rosbuild_add_library(${PROJECT_NAME} src/example.cpp)
#target_link_libraries(${PROJECT_NAME} another_library)
#rosbuild_add_boost_directories()
#rosbuild_link_boost(${PROJECT_NAME} thread)
#rosbuild_add_executable(example examples/example.cpp)
#target_link_libraries(example ${PROJECT_NAME})

1
ros_arduino_msgs/Makefile
View File

@ -1 +0,0 @@
include $(shell rospack find mk)/cmake.mk

26
ros_arduino_msgs/mainpage.dox
View File

@ -1,26 +0,0 @@
/**
\mainpage
\htmlinclude manifest.html
\b ros_arduino_msgs is ...
<!--
Provide an overview of your package.
-->
\section codeapi Code API
<!--
Provide links to specific auto-generated API documentation within your
package that is of particular interest to a reader. Doxygen will
document pretty much every part of your code, so do your best here to
point the reader to the actual API.
If your codebase is fairly large or has different sets of APIs, you
should use the doxygen 'group' tag to keep these APIs together. For
example, the roscpp documentation has 'libros' group.
-->
*/

13
ros_arduino_msgs/manifest.xml
View File

@ -1,13 +0,0 @@
<package>
<description brief="ros_arduino_msgs">
ROS Arduino Messages
</description>
<author>Patrick Goebel</author>
<license>BSD</license>
<review status="unreviewed" notes=""/>
<url>http://ros.org/wiki/ros_arduino_msgs</url>
</package>

3
ros_arduino_msgs/msg/Analog.msg
View File

@ -1,3 +0,0 @@
# Reading from a single analog IO pin.
Header header
uint16 value

3
ros_arduino_msgs/msg/AnalogFloat.msg
View File

@ -1,3 +0,0 @@
# Float message from a single analog IO pin.
Header header
float64 value

4
ros_arduino_msgs/msg/Digital.msg
View File

@ -1,4 +0,0 @@
# Reading on a digital pin
Header header
uint8 value

4
ros_arduino_msgs/msg/SensorState.msg
View File

@ -1,4 +0,0 @@
Header header
string[] name
float32[] value

0
ros_arduino_msgs/src/ros_arduino_msgs/__init__.py
View File

3
ros_arduino_msgs/srv/DigitalSetDirection.srv
View File

@ -1,3 +0,0 @@
uint8 pin
bool direction
---

3
ros_arduino_msgs/srv/DigitalWrite.srv
View File

@ -1,3 +0,0 @@
uint8 pin
bool value
---

3
ros_arduino_msgs/srv/ServoRead.srv
View File

@ -1,3 +0,0 @@
uint8 id
---
int16 value

3
ros_arduino_msgs/srv/ServoWrite.srv
View File

@ -1,3 +0,0 @@
uint8 id
int16 value
---

30
ros_arduino_python/CMakeLists.txt
View File

@ -1,30 +0,0 @@
cmake_minimum_required(VERSION 2.4.6)
include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
# Set the build type. Options are:
# Coverage : w/ debug symbols, w/o optimization, w/ code-coverage
# Debug : w/ debug symbols, w/o optimization
# Release : w/o debug symbols, w/ optimization
# RelWithDebInfo : w/ debug symbols, w/ optimization
# MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries
#set(ROS_BUILD_TYPE RelWithDebInfo)
rosbuild_init()
#set the default path for built executables to the "bin" directory
set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
#set the default path for built libraries to the "lib" directory
set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
#uncomment if you have defined messages
#rosbuild_genmsg()
#uncomment if you have defined services
#rosbuild_gensrv()
#common commands for building c++ executables and libraries
#rosbuild_add_library(${PROJECT_NAME} src/example.cpp)
#target_link_libraries(${PROJECT_NAME} another_library)
#rosbuild_add_boost_directories()
#rosbuild_link_boost(${PROJECT_NAME} thread)
#rosbuild_add_executable(example examples/example.cpp)
#target_link_libraries(example ${PROJECT_NAME})

1
ros_arduino_python/Makefile
View File

@ -1 +0,0 @@
include $(shell rospack find mk)/cmake.mk

48
ros_arduino_python/config/arduino_params.yaml
View File

@ -1,48 +0,0 @@
# 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: False
base_controller_rate: 10
# === Robot drivetrain parameters
#wheel_diameter: 0.146
#wheel_track: 0.2969
#encoder_resolution: 8384 # from Pololu for 131:1 motors
#gear_reduction: 1.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: 0, type: PololuMotorCurrent, rate: 5},
#motor_current_right: {pin: 1, 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}
}

5
ros_arduino_python/launch/arduino.launch
View File

@ -1,5 +0,0 @@
<launch>
<node name="arduino" pkg="ros_arduino_python" type="arduino_node.py" output="screen">
<rosparam file="$(find ros_arduino_python)/config/my_arduino_params.yaml" command="load" />
</node>
</launch>

26
ros_arduino_python/mainpage.dox
View File

@ -1,26 +0,0 @@
/**
\mainpage
\htmlinclude manifest.html
\b ros_arduino_python is ...
<!--
Provide an overview of your package.
-->
\section codeapi Code API
<!--
Provide links to specific auto-generated API documentation within your
package that is of particular interest to a reader. Doxygen will
document pretty much every part of your code, so do your best here to
point the reader to the actual API.
If your codebase is fairly large or has different sets of APIs, you
should use the doxygen 'group' tag to keep these APIs together. For
example, the roscpp documentation has 'libros' group.
-->
*/

24
ros_arduino_python/manifest.xml
View File

@ -1,24 +0,0 @@
<package>
<description brief="ros_arduino_python">
ROS Arduino Python
</description>
<author>Patrick Goebel</author>
<license>BSD</license>
<review status="unreviewed" notes=""/>
<url>http://ros.org/wiki/ros_arduino_python</url>
<depend package="roscpp"/>
<depend package="rospy"/>
<depend package="std_msgs"/>
<depend package="sensor_msgs"/>
<depend package="geometry_msgs"/>
<depend package="nav_msgs"/>
<depend package="tf"/>
<depend package="ros_arduino_msgs"/>
<rosdep name="python-serial"/>
</package>

188
ros_arduino_python/nodes/arduino_node.py
View File

@ -1,188 +0,0 @@
#!/usr/bin/env python
"""
A ROS Node for the Arduino microcontroller
Created for the Pi Robot Project: http://www.pirobot.org
Copyright (c) 2012 Patrick Goebel. All rights reserved.
This program is free software; you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation; either version 2 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details at:
http://www.gnu.org/licenses/gpl.html
"""
import roslib; roslib.load_manifest('ros_arduino_python')
import rospy
from arduino_driver import Arduino
from arduino_sensors import *
from ros_arduino_msgs.srv import *
from base_controller import BaseController
from geometry_msgs.msg import Twist
import os, time
import thread
class ArduinoROS():
def __init__(self):
rospy.init_node('Arduino', log_level=rospy.DEBUG)
# Cleanup when termniating the node
rospy.on_shutdown(self.shutdown)
self.port = rospy.get_param("~port", "/dev/ttyACM0")
self.baud = int(rospy.get_param("~baud", 57600))
self.timeout = rospy.get_param("~timeout", 0.5)
# Overall loop rate: should be faster than fastest sensor rate
self.rate = int(rospy.get_param("~rate", 50))
r = rospy.Rate(self.rate)
# Rate at which summary SensorState message is published. Individual sensors publish
# at their own rates.
self.sensorstate_rate = int(rospy.get_param("~sensorstate_rate", 10))
self.use_base_controller = rospy.get_param("~use_base_controller", False)
# Set up the time for publishing the next SensorState message
now = rospy.Time.now()
self.t_delta_sensors = rospy.Duration(1.0 / self.sensorstate_rate)
self.t_next_sensors = now + self.t_delta_sensors
# Initialize a Twist message
self.cmd_vel = Twist()
# A cmd_vel publisher so we can stop the robot when shutting down
self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist)
# The SensorState publisher periodically publishes the values of all sensors on
# a single topic.
self.sensorStatePub = rospy.Publisher('~sensor_state', SensorState)
# A service to position a PWM servo
rospy.Service('~servo_write', ServoWrite, self.ServoWriteHandler)
# A service to read the position of a PWM servo
rospy.Service('~servo_read', ServoRead, self.ServoReadHandler)
# A service to turn set the direction of a digital pin (0 = input, 1 = output)
rospy.Service('~digital_set_direction', DigitalSetDirection, self.DigitalSetDirectionHandler)
# A service to turn a digital sensor on or off
rospy.Service('~digital_write', DigitalWrite, self.DigitalWriteHandler)
# Initialize the controlller
self.controller = Arduino(self.port, self.baud, self.timeout)
# Make the connection
self.controller.connect()
rospy.loginfo("Connected to Arduino on port " + self.port + " at " + str(self.baud) + " baud")
# Reserve a thread lock
mutex = thread.allocate_lock()
# Initialize any sensors
self.mySensors = list()
sensor_params = rospy.get_param("~sensors", dict({}))
for name, params in sensor_params.iteritems():
# Set the direction to input if not specified
try:
params['direction']
except:
params['direction'] = 'input'
if params['type'] == "Ping":
sensor = Ping(self.controller, name, params['pin'], params['rate'])
elif params['type'] == "GP2D12":
sensor = GP2D12(self.controller, name, params['pin'], params['rate'])
elif params['type'] == 'Digital':
sensor = DigitalSensor(self.controller, name, params['pin'], params['rate'], direction=params['direction'])
elif params['type'] == 'Analog':
sensor = AnalogSensor(self.controller, name, params['pin'], params['rate'], direction=params['direction'])
elif params['type'] == 'PololuMotorCurrent':
sensor = PololuMotorCurrent(self.controller, name, params['pin'], params['rate'])
elif params['type'] == 'PhidgetsVoltage':
sensor = PhidgetsVoltage(self.controller, name, params['pin'], params['rate'])
elif params['type'] == 'PhidgetsCurrent':
sensor = PhidgetsCurrent(self.controller, name, params['pin'], params['rate'])
# if params['type'] == "MaxEZ1":
# self.sensors[len(self.sensors)]['trigger_pin'] = params['trigger_pin']
# self.sensors[len(self.sensors)]['output_pin'] = params['output_pin']
self.mySensors.append(sensor)
rospy.loginfo(name + " " + str(params))
# Initialize the base controller if used
if self.use_base_controller:
self.myBaseController = BaseController(self.controller)
# Start polling the sensors and base controller
while not rospy.is_shutdown():
for sensor in self.mySensors:
mutex.acquire()
sensor.poll()
mutex.release()
if self.use_base_controller:
mutex.acquire()
self.myBaseController.poll()
mutex.release()
# Publish all sensor values on a single topic for convenience
now = rospy.Time.now()
if now > self.t_next_sensors:
msg = SensorState()
msg.header.frame_id = 'base_link'
msg.header.stamp = now
for i in range(len(self.mySensors)):
msg.name.append(self.mySensors[i].name)
msg.value.append(self.mySensors[i].value)
try:
self.sensorStatePub.publish(msg)
except:
pass
self.t_next_sensors = now + self.t_delta_sensors
r.sleep()
# Service callback functions
def ServoWriteHandler(self, req):
self.controller.servo_write(req.id, req.value)
return ServoWriteResponse()
def ServoReadHandler(self, req):
self.controller.servo_read(req.id)
return ServoReadResponse()
def DigitalSetDirectionHandler(self, req):
self.controller.pin_mode(req.pin, req.direction)
return DigitalSetDirectionResponse()
def DigitalWriteHandler(self, req):
self.controller.digital_write(req.pin, req.value)
return DigitalWriteResponse()
def shutdown(self):
# Stop the robot
try:
rospy.loginfo("Stopping the robot...")
self.cmd_vel_pub.Publish(Twist())
rospy.sleep(2)
except:
pass
rospy.loginfo("Shutting down Arduino Node...")
if __name__ == '__main__':
myArduino = ArduinoROS()

0
ros_arduino_python/src/__init__.py
View File

374
ros_arduino_python/src/arduino_driver.py
View File

@ -1,374 +0,0 @@
#!/usr/bin/env python
"""
A Python driver for the Arduino microcontroller running the
ROSArduinoBridge firmware.
Created for the Pi Robot Project: http://www.pirobot.org
Copyright (c) 2012 Patrick Goebel. All rights reserved.
This program is free software; you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation; either version 2 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details at:
http://www.gnu.org/licenses/gpl.html
"""
import thread
from math import pi as PI, degrees, radians
import os
import time
import sys, traceback
from serial.serialutil import SerialException
from serial import Serial
SERVO_MAX = 180
SERVO_MIN = 0
class Arduino:
''' Configuration Parameters
'''
N_ANALOG_PORTS = 6
N_DIGITAL_PORTS = 12
def __init__(self, port="/dev/ttyUSB0", baudrate=57600, timeout=0.5):
self.PID_RATE = 30 # Do not change this! It is a fixed property of the Arduino PID controller.
self.PID_INTERVAL = 1000 / 30
self.port = port
self.baudrate = baudrate
self.timeout = timeout
self.encoder_count = 0
self.writeTimeout = timeout
self.interCharTimeout = timeout / 30.
# Keep things thread safe
self.mutex = thread.allocate_lock()
# An array to cache analog sensor readings
self.analog_sensor_cache = [None] * self.N_ANALOG_PORTS
# An array to cache digital sensor readings
self.digital_sensor_cache = [None] * self.N_DIGITAL_PORTS
def connect(self):
try:
print "Connecting to Arduino on port", self.port, "..."
self.port = Serial(port=self.port, baudrate=self.baudrate, timeout=self.timeout, writeTimeout=self.writeTimeout)
# The next line is necessary to give the firmware time to wake up.
time.sleep(1)
test = self.get_baud()
if test != self.baudrate:
time.sleep(1)
test = self.get_baud()
if test != self.baudrate:
raise SerialException
print "Connected at", self.baudrate
print "Arduino is ready."
except SerialException:
print "Serial Exception:"
print sys.exc_info()
print "Traceback follows:"
traceback.print_exc(file=sys.stdout)
print "Cannot connect to Arduino!"
os._exit(1)
def open(self):
''' Open the serial port.
'''
self.port.open()
def close(self):
''' Close the serial port.
'''
self.port.close()
def send(self, cmd):
''' This command should not be used on its own: it is called by the execute commands
below in a thread safe manner.
'''
self.port.write(cmd + '\r')
def recv(self, timeout=0.5):
timeout = min(timeout, self.timeout)
''' This command should not be used on its own: it is called by the execute commands
below in a thread safe manner. Note: we use read() instead of readline() since
readline() tends to return garbage characters from the Arduino
'''
c = ''
value = ''
attempts = 0
while c != '\r':
c = self.port.read(1)
value += c
attempts += 1
if attempts * self.interCharTimeout > timeout:
return None
value = value.strip('\r')
return value
def recv_ack(self):
''' This command should not be used on its own: it is called by the execute commands
below in a thread safe manner.
'''
ack = self.recv(self.timeout)
return ack == 'OK'
def recv_int(self):
''' This command should not be used on its own: it is called by the execute commands
below in a thread safe manner.
'''
value = self.recv(self.timeout)
try:
return int(value)
except:
return None
def recv_array(self):
''' This command should not be used on its own: it is called by the execute commands
below in a thread safe manner.
'''
try:
values = self.recv(self.timeout * self.N_ANALOG_PORTS).split()
return map(int, values)
except:
return []
def execute(self, cmd):
''' Thread safe execution of "cmd" on the Arduino returning a single integer value.
'''
self.mutex.acquire()
try:
self.port.flushInput()
except:
pass
ntries = 1
attempts = 0
try:
self.port.write(cmd + '\r')
value = self.recv(self.timeout)
while attempts < ntries and (value == '' or value == 'Invalid Command' or value == None):
try:
self.port.flushInput()
self.port.write(cmd + '\r')
value = self.recv(self.timeout)
except:
print "Exception executing command: " + cmd
attempts += 1
except:
self.mutex.release()
print "Exception executing command: " + cmd
value = None
self.mutex.release()
return int(value)
def execute_array(self, cmd):
''' Thread safe execution of "cmd" on the Arduino returning an array.
'''
self.mutex.acquire()
try:
self.port.flushInput()
except:
pass
ntries = 1
attempts = 0
try:
self.port.write(cmd + '\r')
values = self.recv_array()
while attempts < ntries and (values == '' or values == 'Invalid Command' or values == [] or values == None):
try:
self.port.flushInput()
self.port.write(cmd + '\r')
values = self.recv_array()
except:
print("Exception executing command: " + cmd)
attempts += 1
except:
self.mutex.release()
print "Exception executing command: " + cmd
raise SerialException
return []
try:
values = map(int, values)
except:
values = []
self.mutex.release()
return values
def execute_ack(self, cmd):
''' Thread safe execution of "cmd" on the Arduino returning True if response is ACK.
'''
self.mutex.acquire()
try:
self.port.flushInput()
except:
pass
ntries = 1
attempts = 0
try:
self.port.write(cmd + '\r')
ack = self.recv(self.timeout)
while attempts < ntries and (ack == '' or ack == 'Invalid Command' or ack == None):
try:
self.port.flushInput()
self.port.write(cmd + '\r')
ack = self.recv(self.timeout)
except:
print "Exception executing command: " + cmd
attempts += 1
except:
self.mutex.release()
print "execute_ack exception when executing", cmd
print sys.exc_info()
return 0
self.mutex.release()
return ack == 'OK'
def update_pid(self, Kp, Kd, Ki, Ko):
''' Set the PID parameters on the Arduino
'''
print "Updating PID parameters"
cmd = 'u ' + str(Kp) + ':' + str(Kd) + ':' + str(Ki) + ':' + str(Ko)
self.execute_ack(cmd)
def get_baud(self):
''' Get the current baud rate on the serial port.
'''
return int(self.execute('b'));
def get_encoder_counts(self):
values = self.execute_array('e')
if len(values) != 2:
print "Encoder count was not 2"
raise SerialException
return None
else:
return values
def reset_encoders(self):
''' Reset the encoder counts to 0
'''
return self.execute_ack('r')
def drive(self, right, left):
''' Speeds are given in encoder ticks per PID interval
'''
return self.execute_ack('m %d %d' %(right, left))
def drive_m_per_s(self, right, left):
''' Set the motor speeds in meters per second.
'''
left_revs_per_second = float(left) / (self.wheel_diameter * PI)
right_revs_per_second = float(right) / (self.wheel_diameter * PI)
left_ticks_per_loop = int(left_revs_per_second * self.encoder_resolution * self.PID_INTERVAL * self.gear_reduction)
right_ticks_per_loop = int(right_revs_per_second * self.encoder_resolution * self.PID_INTERVAL * self.gear_reduction)
self.drive(right_ticks_per_loop , left_ticks_per_loop )
def stop(self):
''' Stop both motors.
'''
self.drive(0, 0)
def analog_read(self, pin):
return self.execute('a %d' %pin)
def analog_write(self, pin, value):
return self.execute_ack('x %d %d' %(pin, value))
def digital_read(self, pin):
return self.execute('d %d' %pin)
def digital_write(self, pin, value):
return self.execute_ack('w %d %d' %(pin, value))
def pin_mode(self, pin, mode):
return self.execute_ack('c %d %d' %(pin, mode))
def servo_write(self, id, pos):
''' Usage: servo_write(id, pos)
Position is given in radians and converted to degrees before sending
'''
return self.execute_ack('s %d %d' %(id, min(SERVO_MAX, max(SERVO_MIN, degrees(pos)))))
def servo_read(self, id):
''' Usage: servo_read(id)
The returned position is converted from degrees to radians
'''
return radians(self.execute('t %d' %id))
def ping(self, pin):
''' The srf05/Ping command queries an SRF05/Ping sonar sensor
connected to the General Purpose I/O line pinId for a distance,
and returns the range in cm. Sonar distance resolution is integer based.
'''
return self.execute('p %d' %pin);
# def get_maxez1(self, triggerPin, outputPin):
# ''' The maxez1 command queries a Maxbotix MaxSonar-EZ1 sonar
# sensor connected to the General Purpose I/O lines, triggerPin, and
# outputPin, for a distance, and returns it in Centimeters. NOTE: MAKE
# SURE there's nothing directly in front of the MaxSonar-EZ1 upon
# power up, otherwise it wont range correctly for object less than 6
# inches away! The sensor reading defaults to use English units
# (inches). The sensor distance resolution is integer based. Also, the
# maxsonar trigger pin is RX, and the echo pin is PW.
# '''
# return self.execute('z %d %d' %(triggerPin, outputPin))
""" Basic test for connectivity """
if __name__ == "__main__":
if os.name == "posix":
portName = "/dev/ttyACM0"
else:
portName = "COM43" # Windows style COM port.
baudRate = 57600
myArduino = Arduino(port=portName, baudrate=baudRate, timeout=0.5)
myArduino.connect()
print "Sleeping for 1 second..."
time.sleep(1)
print "Reading on analog port 0", myArduino.analog_read(0)
print "Reading on digital port 0", myArduino.digital_read(0)
print "Blinking the LED 3 times"
for i in range(3):
myArduino.digital_write(13, 1)
time.sleep(1.0)
#print "Current encoder counts", myArduino.encoders()
print "Connection test successful.",
myArduino.stop()
myArduino.close()
print "Shutting down Arduino."

256
ros_arduino_python/src/arduino_sensors.py
View File

@ -1,256 +0,0 @@
#!/usr/bin/env python
"""
Sensor class for the arudino_python package
Created for the Pi Robot Project: http://www.pirobot.org
Copyright (c) 2012 Patrick Goebel. All rights reserved.
This program is free software; you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation; either version 2 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details at:
http://www.gnu.org/licenses/gpl.html
"""
import roslib; roslib.load_manifest('ros_arduino_python')
import rospy
from sensor_msgs.msg import Range
from ros_arduino_msgs.msg import *
LOW = 0
HIGH = 1
INPUT = 0
OUTPUT = 1
class MessageType:
ANALOG = 0
DIGITAL = 1
RANGE = 2
FLOAT = 3
INT = 4
BOOL = 5
class Sensor(object):
def __init__(self, controller, name, pin, rate, frame_id="/base_link", direction="input", **kwargs):
self.controller = controller
self.name = name
self.pin = pin
self.rate = rate
self.direction = direction
self.frame_id = frame_id
self.value = None
self.t_delta = rospy.Duration(1.0 / self.rate)
self.t_next = rospy.Time.now() + self.t_delta
def poll(self):
now = rospy.Time.now()
if now > self.t_next:
if self.direction == "input":
try:
self.value = self.read_value()
except:
return
else:
try:
self.ack = self.write_value()
except:
return
# For range sensors, assign the value to the range message field
if self.message_type == MessageType.RANGE:
self.msg.range = self.value
else:
self.msg.value = self.value
# Add a timestamp and publish the message
self.msg.header.stamp = rospy.Time.now()
self.pub.publish(self.msg)
self.t_next = now + self.t_delta
class AnalogSensor(Sensor):
def __init__(self, *args, **kwargs):
super(AnalogSensor, self).__init__(*args, **kwargs)
self.message_type = MessageType.ANALOG
self.msg = Analog()
self.msg.header.frame_id = self.frame_id
self.pub = rospy.Publisher("~sensor/" + self.name, Analog)
def read_value(self):
return self.controller.analog_read(self.pin)
def write_value(self, value):
return self.controller.analog_write(self.pin, value)
class AnalogFloatSensor(Sensor):
def __init__(self, *args, **kwargs):
super(AnalogFloatSensor, self).__init__(*args, **kwargs)
self.message_type = MessageType.ANALOG
self.msg = AnalogFloat()
self.msg.header.frame_id = self.frame_id
self.pub = rospy.Publisher("~sensor/" + self.name, AnalogFloat)
class DigitalSensor(Sensor):
def __init__(self, *args, **kwargs):
super(DigitalSensor, self).__init__(*args, **kwargs)
self.message_type = MessageType.BOOL
self.msg = Digital()
self.msg.header.frame_id = self.frame_id
self.pub = rospy.Publisher("~sensor/" + self.name, Digital)
if self.direction == "output":
self.controller.pin_mode(self.pin, OUTPUT)
else:
self.controller.pin_mode(self.pin, INPUT)
self.value = LOW
def read_value(self):
return self.controller.digital_read(self.pin)
def write_value(self):
# Alternate HIGH/LOW when writing at a fixed rate
self.value = not self.value
return self.controller.digital_write(self.pin, self.value)
class RangeSensor(Sensor):
def __init__(self, *args, **kwargs):
super(RangeSensor, self).__init__(*args, **kwargs)
self.message_type = MessageType.RANGE
self.msg = Range()
self.msg.header.frame_id = self.frame_id
self.pub = rospy.Publisher("~sensor/" + self.name, Range)
def read_value(self):
self.msg.header.stamp = rospy.Time.now()
class SonarSensor(RangeSensor):
def __init__(self, *args, **kwargs):
super(SonarSensor, self).__init__(*args, **kwargs)
self.msg.radiation_type = Range.ULTRASOUND
class IRSensor(RangeSensor):
def __init__(self, *args, **kwargs):
super(IRSensor, self).__init__(*args, **kwargs)
self.msg.radiation_type = Range.INFRARED
class Ping(SonarSensor):
def __init__(self,*args, **kwargs):
super(Ping, self).__init__(*args, **kwargs)
self.msg.field_of_view = 0.785398163
self.msg.min_range = 0.02
self.msg.max_range = 3.0
def read_value(self):
# The Arduino Ping code returns the distance in centimeters
cm = self.controller.ping(self.pin)
# Convert it to meters for ROS
distance = cm / 100.0
return distance
class GP2D12(IRSensor):
def __init__(self, *args, **kwargs):
super(GP2D12, self).__init__(*args, **kwargs)
self.msg.field_of_view = 0.001
self.msg.min_range = 0.10
self.msg.max_range = 0.80
def read_value(self):
value = self.controller.analog_read(self.pin)
if value <= 3.0:
return self.msg.max_range
try:
distance = (6787.0 / (float(value) - 3.0)) - 4.0
except:
return self.msg.max_range
# Convert to meters
distance /= 100.0
# If we get a spurious reading, set it to the max_range
if distance > self.msg.max_range: distance = self.msg.max_range
if distance < self.msg.min_range: distance = self.msg.max_range
return distance
class PololuMotorCurrent(AnalogFloatSensor):
def __init__(self, *args, **kwargs):
super(PololuMotorCurrent, self).__init__(*args, **kwargs)
def read_value(self):
# From the Pololu source code
milliamps = self.controller.analog_read(self.pin) * 34
return milliamps / 1000.0
class PhidgetsVoltage(AnalogFloatSensor):
def __init__(self, *args, **kwargs):
super(PhidgetsVoltage, self).__init__(*args, **kwargs)
def read_value(self):
# From the Phidgets documentation
voltage = 0.06 * (self.controller.analog_read(self.pin) - 500.)
return voltage
class PhidgetsCurrent(AnalogFloatSensor):
def __init__(self, *args, **kwargs):
super(PhidgetsCurrent, self).__init__(*args, **kwargs)
def read_value(self):
# From the Phidgets documentation for the 20 amp DC sensor
current = 0.05 * (self.controller.analog_read(self.pin) - 500.)
return current
class MaxEZ1Sensor(SonarSensor):
def __init__(self, *args, **kwargs):
super(MaxEZ1Sensor, self).__init__(*args, **kwargs)
self.trigger_pin = kwargs['trigger_pin']
self.output_pin = kwargs['output_pin']
self.msg.field_of_view = 0.785398163
self.msg.min_range = 0.02
self.msg.max_range = 3.0
def read_value(self):
return self.controller.get_MaxEZ1(self.trigger_pin, self.output_pin)
if __name__ == '__main__':
myController = Controller()
mySensor = SonarSensor(myController, "My Sonar", type=Type.PING, pin=0, rate=10)

246
ros_arduino_python/src/base_controller.py
View File

@ -1,246 +0,0 @@
#!/usr/bin/env python
"""
A base controller class for the Arduino microcontroller
Borrowed heavily from Mike Feguson's ArbotiX base_controller.py code.
Created for the Pi Robot Project: http://www.pirobot.org
Copyright (c) 2010 Patrick Goebel. All rights reserved.
This program is free software; you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation; either version 2 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details at:
http://www.gnu.org/licenses
"""
import roslib; roslib.load_manifest('ros_arduino_python')
import rospy
import os
from math import sin, cos, pi
from geometry_msgs.msg import Quaternion, Twist, Pose
from nav_msgs.msg import Odometry
from tf.broadcaster import TransformBroadcaster
""" Class to receive Twist commands and publish Odometry data """
class BaseController:
def __init__(self, arduino):
self.arduino = arduino
self.rate = float(rospy.get_param("~base_controller_rate", 10))
self.timeout = rospy.get_param('~base_controller_timeout', 1.0)
self.stopped = False
pid_params = dict()
pid_params['wheel_diameter'] = rospy.get_param("~wheel_diameter", "")
pid_params['wheel_track'] = rospy.get_param("~wheel_track", "")
pid_params['encoder_resolution'] = rospy.get_param("~encoder_resolution", "")
pid_params['gear_reduction'] = rospy.get_param("~gear_reduction", 1.0)
pid_params['Kp'] = rospy.get_param("~Kp", 20)
pid_params['Kd'] = rospy.get_param("~Kd", 12)
pid_params['Ki'] = rospy.get_param("~Ki", 0)
pid_params['Ko'] = rospy.get_param("~Ko", 50)
self.accel_limit = rospy.get_param('~accel_limit', 0.1)
self.motors_reversed = rospy.get_param("~motors_reversed", False)
# Set up PID parameters and check for missing values
self.setup_pid(pid_params)
# How many encoder ticks are there per meter?
self.ticks_per_meter = self.encoder_resolution * self.gear_reduction / (self.wheel_diameter * pi)
# What is the maximum acceleration we will tolerate when changing wheel speeds?
self.max_accel = self.accel_limit * self.ticks_per_meter / self.rate
# Track how often we get a bad encoder count (if any)
self.bad_encoder_count = 0
now = rospy.Time.now()
self.then = now # time for determining dx/dy
self.t_delta = rospy.Duration(1.0 / self.rate)
self.t_next = now + self.t_delta
# internal data
self.enc_left = None # encoder readings
self.enc_right = None
self.x = 0 # position in xy plane
self.y = 0
self.th = 0 # rotation in radians
self.v_left = 0
self.v_right = 0
self.v_des_left = 0 # cmd_vel setpoint
self.v_des_right = 0
self.last_cmd_vel = now
# subscriptions
rospy.Subscriber("cmd_vel", Twist, self.cmdVelCallback)
# Clear any old odometry info
self.arduino.reset_encoders()
# Set up the odometry broadcaster
self.odomPub = rospy.Publisher('odom', Odometry)
self.odomBroadcaster = TransformBroadcaster()
rospy.loginfo("Started base controller for a base of " + str(self.wheel_track) + "m wide with " + str(self.encoder_resolution) + " ticks per rev")
rospy.loginfo("Publishing odometry data at: " + str(self.rate) + " Hz")
def setup_pid(self, pid_params):
# Check to see if any PID parameters are missing
missing_params = False
for param in pid_params:
if pid_params[param] == "":
print("*** PID Parameter " + param + " is missing. ***")
missing_params = True
if missing_params:
os._exit(1)
self.wheel_diameter = pid_params['wheel_diameter']
self.wheel_track = pid_params['wheel_track']
self.encoder_resolution = pid_params['encoder_resolution']
self.gear_reduction = pid_params['gear_reduction']
self.Kp = pid_params['Kp']
self.Kd = pid_params['Kd']
self.Ki = pid_params['Ki']
self.Ko = pid_params['Ko']
self.arduino.update_pid(self.Kp, self.Kd, self.Ki, self.Ko)
def poll(self):
now = rospy.Time.now()
if now > self.t_next:
# Read the encoders
try:
left_enc, right_enc = self.arduino.get_encoder_counts()
except:
self.bad_encoder_count += 1
rospy.logerr("Encoder exception count: " + str(self.bad_encoder_count))
return
dt = now - self.then
self.then = now
dt = dt.to_sec()
# calculate odometry
if self.enc_left == None:
dright = 0
dleft = 0
else:
dright = (right_enc - self.enc_right) / self.ticks_per_meter
dleft = (left_enc - self.enc_left) / self.ticks_per_meter
self.enc_right = right_enc
self.enc_left = left_enc
dxy_ave = (dright + dleft) / 2.0
dth = (dright - dleft) / self.wheel_track
vxy = dxy_ave / dt
vth = dth / dt
if (dxy_ave != 0):
dx = cos(dth) * dxy_ave
dy = -sin(dth) * dxy_ave
self.x += (cos(self.th) * dx - sin(self.th) * dy)
self.y += (sin(self.th) * dx + cos(self.th) * dy)
if (dth != 0):
self.th += dth
quaternion = Quaternion()
quaternion.x = 0.0
quaternion.y = 0.0
quaternion.z = sin(self.th / 2.0)
quaternion.w = cos(self.th / 2.0)
# Create the odometry transform frame broadcaster.
self.odomBroadcaster.sendTransform(
(self.x, self.y, 0),
(quaternion.x, quaternion.y, quaternion.z, quaternion.w),
rospy.Time.now(),
"base_link",
"odom"
)
odom = Odometry()
odom.header.frame_id = "odom"
odom.child_frame_id = "base_link"
odom.header.stamp = now
odom.pose.pose.position.x = self.x
odom.pose.pose.position.y = self.y
odom.pose.pose.position.z = 0
odom.pose.pose.orientation = quaternion
odom.twist.twist.linear.x = vxy
odom.twist.twist.linear.y = 0
odom.twist.twist.angular.z = vth
self.odomPub.publish(odom)
if now > (self.last_cmd_vel + rospy.Duration(self.timeout)):
self.v_des_left = 0
self.v_des_right = 0
if self.v_left < self.v_des_left:
self.v_left += self.max_accel
if self.v_left > self.v_des_left:
self.v_left = self.v_des_left
else:
self.v_left -= self.max_accel
if self.v_left < self.v_des_left:
self.v_left = self.v_des_left
if self.v_right < self.v_des_right:
self.v_right += self.max_accel
if self.v_right > self.v_des_right:
self.v_right = self.v_des_right
else:
self.v_right -= self.max_accel
if self.v_right < self.v_des_right:
self.v_right = self.v_des_right
# Set motor speeds in encoder ticks per PID loop
if not self.stopped:
self.arduino.drive(self.v_left, self.v_right)
self.t_next = now + self.t_delta
def stop(self):
self.stopped = True
self.arduino.drive(0, 0)
def cmdVelCallback(self, req):
# Handle velocity-based movement requests
self.last_cmd_vel = rospy.Time.now()
x = req.linear.x # m/s
th = req.angular.z # rad/s
if x == 0:
# Turn in place
right = th * self.wheel_track * self.gear_reduction / 2.0
left = -right
elif th == 0:
# Pure forward/backward motion
left = right = x
else:
# Rotation about a point in space
left = x - th * self.wheel_track * self.gear_reduction / 2.0
right = x + th * self.wheel_track * self.gear_reduction / 2.0
self.v_des_left = int(left * self.ticks_per_meter / self.arduino.PID_RATE)
self.v_des_right = int(right * self.ticks_per_meter / self.arduino.PID_RATE)

0
ros_arduino_python/src/ros_arduino_python/__init__.py
View File

12
stack.xml
View File

@ -1,12 +0,0 @@
<stack>
<description brief="ros_arduino_bridge">ROS Arduino Bridge</description>
<author>Maintained by Patrick Goebel</author>
<license>BSD</license>
<review status="unreviewed" notes=""/>
<url>http://ros.org/wiki/ros_arduino_bridge</url>
<depend stack="ros" />
<depend stack="ros_comm" /> <!-- std_srvs, rospy, roscpp -->
<depend stack="common_msgs" /> <!-- nav_msgs, geometry_msgs, sensor_msgs, diagnostic_msgs -->
<depend stack="geometry" /> <!-- tf -->
</stack>