mirror of
https://github.com/YikeStone/ros_arduino_bridge.git
synced 2025-08-04 03:34:08 +05:30
Compare commits
No commits in common. "indigo-devel" and "0.1.1" have entirely different histories.
indigo-dev
...
0.1.1
106
.gitignore
vendored
106
.gitignore
vendored
@ -1,106 +0,0 @@
|
|||||||
|
|
||||||
# Created by https://www.gitignore.io/api/python
|
|
||||||
|
|
||||||
### Python ###
|
|
||||||
# Byte-compiled / optimized / DLL files
|
|
||||||
__pycache__/
|
|
||||||
*.py[cod]
|
|
||||||
*$py.class
|
|
||||||
|
|
||||||
# C extensions
|
|
||||||
*.so
|
|
||||||
|
|
||||||
# Distribution / packaging
|
|
||||||
.Python
|
|
||||||
env/
|
|
||||||
build/
|
|
||||||
develop-eggs/
|
|
||||||
dist/
|
|
||||||
downloads/
|
|
||||||
eggs/
|
|
||||||
.eggs/
|
|
||||||
lib/
|
|
||||||
lib64/
|
|
||||||
parts/
|
|
||||||
sdist/
|
|
||||||
var/
|
|
||||||
wheels/
|
|
||||||
*.egg-info/
|
|
||||||
.installed.cfg
|
|
||||||
*.egg
|
|
||||||
|
|
||||||
# PyInstaller
|
|
||||||
# Usually these files are written by a python script from a template
|
|
||||||
# before PyInstaller builds the exe, so as to inject date/other infos into it.
|
|
||||||
*.manifest
|
|
||||||
*.spec
|
|
||||||
|
|
||||||
# Installer logs
|
|
||||||
pip-log.txt
|
|
||||||
pip-delete-this-directory.txt
|
|
||||||
|
|
||||||
# Unit test / coverage reports
|
|
||||||
htmlcov/
|
|
||||||
.tox/
|
|
||||||
.coverage
|
|
||||||
.coverage.*
|
|
||||||
.cache
|
|
||||||
nosetests.xml
|
|
||||||
coverage.xml
|
|
||||||
*,cover
|
|
||||||
.hypothesis/
|
|
||||||
|
|
||||||
# Translations
|
|
||||||
*.mo
|
|
||||||
*.pot
|
|
||||||
|
|
||||||
# Django stuff:
|
|
||||||
*.log
|
|
||||||
local_settings.py
|
|
||||||
|
|
||||||
# Flask stuff:
|
|
||||||
instance/
|
|
||||||
.webassets-cache
|
|
||||||
|
|
||||||
# Scrapy stuff:
|
|
||||||
.scrapy
|
|
||||||
|
|
||||||
# Sphinx documentation
|
|
||||||
docs/_build/
|
|
||||||
|
|
||||||
# PyBuilder
|
|
||||||
target/
|
|
||||||
|
|
||||||
# Jupyter Notebook
|
|
||||||
.ipynb_checkpoints
|
|
||||||
|
|
||||||
# pyenv
|
|
||||||
.python-version
|
|
||||||
|
|
||||||
# celery beat schedule file
|
|
||||||
celerybeat-schedule
|
|
||||||
|
|
||||||
# SageMath parsed files
|
|
||||||
*.sage.py
|
|
||||||
|
|
||||||
# dotenv
|
|
||||||
.env
|
|
||||||
|
|
||||||
# virtualenv
|
|
||||||
.venv
|
|
||||||
venv/
|
|
||||||
ENV/
|
|
||||||
|
|
||||||
# Spyder project settings
|
|
||||||
.spyderproject
|
|
||||||
.spyproject
|
|
||||||
|
|
||||||
# Rope project settings
|
|
||||||
.ropeproject
|
|
||||||
|
|
||||||
# mkdocs documentation
|
|
||||||
/site
|
|
||||||
|
|
||||||
# End of https://www.gitignore.io/api/python
|
|
||||||
# temporary files
|
|
||||||
*~
|
|
||||||
17
CMakeLists.txt
Normal file
17
CMakeLists.txt
Normal file
@ -0,0 +1,17 @@
|
|||||||
|
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)
|
||||||
94
README.md
94
README.md
@ -1,11 +1,3 @@
|
|||||||
This Fork is modified for L298N motor driver and quad encoders, with one major bug fix and other minor bug fixes.
|
|
||||||
This supports separate PID control system for two (left, right) motors.
|
|
||||||
Removed support for all other extra stuff.
|
|
||||||
|
|
||||||
Overview
|
|
||||||
--------
|
|
||||||
This branch (indigo-devel) is intended for ROS Indigo and above, and uses the Catkin buildsystem. It may also be compatible with ROS Hydro.
|
|
||||||
|
|
||||||
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 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.
|
||||||
|
|
||||||
Features of the stack include:
|
Features of the stack include:
|
||||||
@ -17,6 +9,7 @@ Features of the stack include:
|
|||||||
* Can control digital outputs (e.g. turn a switch or LED on and off)
|
* Can control digital outputs (e.g. turn a switch or LED on and off)
|
||||||
|
|
||||||
* Support for PWM servos
|
* Support for PWM servos
|
||||||
|
|
||||||
* Configurable base controller if using the required hardware
|
* Configurable base controller if using the required hardware
|
||||||
|
|
||||||
The stack includes a base controller for a differential drive
|
The stack includes a base controller for a differential drive
|
||||||
@ -26,11 +19,9 @@ the PC. The base controller requires the use of a motor controller and encoders
|
|||||||
* 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).
|
* 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
|
* Robogaia Mega Encoder shield
|
||||||
(http://www.robogaia.com/two-axis-encoder-counter-mega-shield-version-2.html) or on-board wheel encoder counters.
|
(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 on-board wheel encoder counters are currently only supported by Arduino Uno.
|
**NOTE:** The Robogaia Mega Encoder shield can only be used with an Arduino Mega.
|
||||||
|
|
||||||
* L298 motor driver
|
|
||||||
|
|
||||||
* The library can be easily extended to include support for other motor controllers and encoder hardware or libraries.
|
* The library can be easily extended to include support for other motor controllers and encoder hardware or libraries.
|
||||||
|
|
||||||
@ -55,11 +46,8 @@ or
|
|||||||
|
|
||||||
$ sudo easy_install -U pyserial
|
$ sudo easy_install -U pyserial
|
||||||
|
|
||||||
**Arduino IDE 1.6.6 or Higher:**
|
|
||||||
Note that the preprocessing of conditional #include statements is broken in earlier versions of the Arduino IDE. To ensure that the ROS Arduino Bridge firmware compiles correctly, be sure to install version 1.6.6 or higher of the Arduino IDE. You can download the IDE from https://www.arduino.cc/en/Main/Software.
|
|
||||||
|
|
||||||
**Hardware:**
|
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.
|
||||||
The firmware 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:
|
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:
|
||||||
|
|
||||||
@ -73,8 +61,6 @@ The Robogaia Mega Encoder library can be found at:
|
|||||||
|
|
||||||
http://www.robogaia.com/uploads/6/8/0/9/6809982/__megaencodercounter-1.3.tar.gz
|
http://www.robogaia.com/uploads/6/8/0/9/6809982/__megaencodercounter-1.3.tar.gz
|
||||||
|
|
||||||
L298 Motor Driver doesn't require any libraries
|
|
||||||
|
|
||||||
These libraries should be installed in your standard Arduino
|
These libraries should be installed in your standard Arduino
|
||||||
sketchbook/libraries directory.
|
sketchbook/libraries directory.
|
||||||
|
|
||||||
@ -118,10 +104,10 @@ and you should see a list of groups you belong to including dialout.
|
|||||||
Installation of the ros\_arduino\_bridge Stack
|
Installation of the ros\_arduino\_bridge Stack
|
||||||
----------------------------------------------
|
----------------------------------------------
|
||||||
|
|
||||||
$ cd ~/catkin_workspace/src
|
$ cd ~/ros_workspace
|
||||||
$ git clone https://github.com/hbrobotics/ros_arduino_bridge.git
|
$ git clone https://github.com/hbrobotics/ros_arduino_bridge.git
|
||||||
$ cd ~/catkin_workspace
|
$ cd ros_arduino_bridge
|
||||||
$ catkin_make
|
$ rosmake
|
||||||
|
|
||||||
The provided Arduino library is called ROSArduinoBridge and is
|
The provided Arduino library is called ROSArduinoBridge and is
|
||||||
located in the ros\_arduino\_firmware package. This sketch is
|
located in the ros\_arduino\_firmware package. This sketch is
|
||||||
@ -156,18 +142,21 @@ 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 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. the Robogaia Mega Encoder shield is chosen by default.
|
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, look for the line:
|
If you want to control PWM servos attached to your controller, change
|
||||||
|
the two lines that look like this:
|
||||||
<pre>
|
|
||||||
#define USE_SERVOS
|
|
||||||
</pre>
|
|
||||||
|
|
||||||
and make sure it is not commented out like this:
|
|
||||||
|
|
||||||
<pre>
|
<pre>
|
||||||
//#define USE_SERVOS
|
//#define USE_SERVOS
|
||||||
|
#undef USE_SERVOS
|
||||||
|
</pre>
|
||||||
|
|
||||||
|
to this:
|
||||||
|
|
||||||
|
<pre>
|
||||||
|
#define USE_SERVOS
|
||||||
|
//#undef USE_SERVOS
|
||||||
</pre>
|
</pre>
|
||||||
|
|
||||||
You must then edit the include file servos.h and change the N_SERVOS
|
You must then edit the include file servos.h and change the N_SERVOS
|
||||||
@ -437,7 +426,7 @@ where pin is the pin number and value is 0 for LOW and 1 for HIGH.
|
|||||||
|
|
||||||
$ rosservice call /arduino/servo_write id pos
|
$ 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 radians (0 - 3.14).
|
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
|
**servo\_read** - read the position of a servo
|
||||||
|
|
||||||
@ -445,43 +434,6 @@ where id is the index of the servo as defined in the Arduino sketch (servos.h) a
|
|||||||
|
|
||||||
where id is the index of the servo as defined in the Arduino sketch (servos.h)
|
where id is the index of the servo as defined in the Arduino sketch (servos.h)
|
||||||
|
|
||||||
Using the on-board wheel encoder counters (Arduino Uno only)
|
|
||||||
------------------------------------------------------------
|
|
||||||
The firmware supports on-board wheel encoder counters for Arduino Uno.
|
|
||||||
This allows connecting wheel encoders directly to the Arduino board, without the need for any additional wheel encoder counter equipment (such as a RoboGaia encoder shield).
|
|
||||||
|
|
||||||
For speed, the code is directly addressing specific Atmega328p ports and interrupts, making this implementation Atmega328p (Arduino Uno) dependent. (It should be easy to adapt for other boards/AVR chips though.)
|
|
||||||
|
|
||||||
To use the on-board wheel encoder counters, connect your wheel encoders to Arduino Uno as follows:
|
|
||||||
|
|
||||||
Left wheel encoder A output -- Arduino UNO pin 2
|
|
||||||
Left wheel encoder B output -- Arduino UNO pin 3
|
|
||||||
|
|
||||||
Right wheel encoder A output -- Arduino UNO pin A4
|
|
||||||
Right wheel encoder B output -- Arduino UNO pin A5
|
|
||||||
|
|
||||||
Make the following changes in the ROSArduinoBridge sketch to disable the RoboGaia encoder shield, and enable the on-board one:
|
|
||||||
|
|
||||||
/* The RoboGaia encoder shield */
|
|
||||||
//#define ROBOGAIA
|
|
||||||
/* Encoders directly attached to Arduino board */
|
|
||||||
#define ARDUINO_ENC_COUNTER
|
|
||||||
|
|
||||||
Compile the changes and upload to your controller.
|
|
||||||
|
|
||||||
Using L298 Motor driver
|
|
||||||
-----------------------
|
|
||||||
the wiring between the L298 motor driver and arduino board is defined in motor_driver.h in the firmware as follow:
|
|
||||||
|
|
||||||
#define RIGHT_MOTOR_BACKWARD 5
|
|
||||||
#define LEFT_MOTOR_BACKWARD 6
|
|
||||||
#define RIGHT_MOTOR_FORWARD 9
|
|
||||||
#define LEFT_MOTOR_FORWARD 10
|
|
||||||
#define RIGHT_MOTOR_ENABLE 12
|
|
||||||
#define LEFT_MOTOR_ENABLE 13
|
|
||||||
|
|
||||||
wire them this way or change them if you want, and make sure that the L298 motor driver is defined then compile and upload the firmware.
|
|
||||||
|
|
||||||
NOTES
|
NOTES
|
||||||
-----
|
-----
|
||||||
If you do not have the hardware required to run the base controller,
|
If you do not have the hardware required to run the base controller,
|
||||||
@ -489,19 +441,21 @@ follow the instructions below so that you can still use your
|
|||||||
Arduino-compatible controller to read sensors and control PWM servos.
|
Arduino-compatible controller to read sensors and control PWM servos.
|
||||||
|
|
||||||
First, you need to edit the ROSArduinoBridge sketch. At the top of
|
First, you need to edit the ROSArduinoBridge sketch. At the top of
|
||||||
the file comment out the line:
|
the file, change the two lines that look like this:
|
||||||
|
|
||||||
<pre>
|
<pre>
|
||||||
#define USE_BASE
|
#define USE_BASE
|
||||||
|
//#undef USE_BASE
|
||||||
</pre>
|
</pre>
|
||||||
|
|
||||||
so that it looks like this:
|
to this:
|
||||||
|
|
||||||
<pre>
|
<pre>
|
||||||
//#define USE_BASE
|
//#define USE_BASE
|
||||||
|
#undef USE_BASE
|
||||||
</pre>
|
</pre>
|
||||||
|
|
||||||
**NOTE:** If you are using a version of the Arduino IDE previous to 1.6.6, you also need to comment out the line that looks like this in the file encoder_driver.ino:
|
**NOTE:** You also need to comment out the line that looks like this in the file encoder_driver.ino:
|
||||||
|
|
||||||
#include "MegaEncoderCounter.h"
|
#include "MegaEncoderCounter.h"
|
||||||
|
|
||||||
|
|||||||
@ -1,4 +0,0 @@
|
|||||||
cmake_minimum_required(VERSION 2.8.3)
|
|
||||||
project(ros_arduino_bridge)
|
|
||||||
find_package(catkin REQUIRED)
|
|
||||||
catkin_metapackage()
|
|
||||||
@ -1,21 +0,0 @@
|
|||||||
<package>
|
|
||||||
<name>ros_arduino_bridge</name>
|
|
||||||
<version>0.2.0</version>
|
|
||||||
<description>
|
|
||||||
Metapackage for ros_arduino_bridge.
|
|
||||||
</description>
|
|
||||||
<author>Patrick Goebel</author>
|
|
||||||
<maintainer email="patrick@pirobot.org">Patrick Goebel</maintainer>
|
|
||||||
<license>BSD</license>
|
|
||||||
<url>http://ros.org/wiki/ros_arduino_bridge</url>
|
|
||||||
|
|
||||||
<buildtool_depend>catkin</buildtool_depend>
|
|
||||||
|
|
||||||
<run_depend>ros_arduino_firmware</run_depend>
|
|
||||||
<run_depend>ros_arduino_msgs</run_depend>
|
|
||||||
<run_depend>ros_arduino_python</run_depend>
|
|
||||||
|
|
||||||
<export>
|
|
||||||
<metapackage/>
|
|
||||||
</export>
|
|
||||||
</package>
|
|
||||||
@ -1,9 +1,30 @@
|
|||||||
cmake_minimum_required(VERSION 2.8.3)
|
cmake_minimum_required(VERSION 2.4.6)
|
||||||
project(ros_arduino_firmware)
|
include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
|
||||||
|
|
||||||
find_package(catkin REQUIRED)
|
# Set the build type. Options are:
|
||||||
catkin_package(DEPENDS)
|
# 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)
|
||||||
|
|
||||||
install(DIRECTORY src
|
rosbuild_init()
|
||||||
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
|
|
||||||
)
|
#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
Normal file
1
ros_arduino_firmware/Makefile
Normal file
@ -0,0 +1 @@
|
|||||||
|
include $(shell rospack find mk)/cmake.mk
|
||||||
26
ros_arduino_firmware/mainpage.dox
Normal file
26
ros_arduino_firmware/mainpage.dox
Normal file
@ -0,0 +1,26 @@
|
|||||||
|
/**
|
||||||
|
\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
Normal file
14
ros_arduino_firmware/manifest.xml
Normal file
@ -0,0 +1,14 @@
|
|||||||
|
<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>
|
||||||
|
|
||||||
|
|
||||||
@ -1,13 +0,0 @@
|
|||||||
<package>
|
|
||||||
<name>ros_arduino_firmware</name>
|
|
||||||
<version>0.2.0</version>
|
|
||||||
<description>
|
|
||||||
ROS Arduino Firmware.
|
|
||||||
</description>
|
|
||||||
<author>Patrick Goebel</author>
|
|
||||||
<maintainer email="patrick@pirobot.org">Patrick Goebel</maintainer>
|
|
||||||
<license>BSD</license>
|
|
||||||
<url>http://ros.org/wiki/ros_arduino_firmware</url>
|
|
||||||
|
|
||||||
<buildtool_depend>catkin</buildtool_depend>
|
|
||||||
</package>
|
|
||||||
@ -0,0 +1,341 @@
|
|||||||
|
/*********************************************************************
|
||||||
|
* 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
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
@ -0,0 +1,22 @@
|
|||||||
|
/* 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
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
@ -0,0 +1,71 @@
|
|||||||
|
/* 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);
|
||||||
|
}
|
||||||
|
|
||||||
@ -0,0 +1,34 @@
|
|||||||
|
/* 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);
|
||||||
|
}
|
||||||
|
|
||||||
@ -0,0 +1,16 @@
|
|||||||
|
/* 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};
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
@ -1,4 +1,72 @@
|
|||||||
|
/*********************************************************************
|
||||||
|
* 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
|
#define BAUDRATE 57600
|
||||||
|
|
||||||
|
/* Maximum PWM signal */
|
||||||
#define MAX_PWM 255
|
#define MAX_PWM 255
|
||||||
|
|
||||||
#if defined(ARDUINO) && ARDUINO >= 100
|
#if defined(ARDUINO) && ARDUINO >= 100
|
||||||
@ -6,24 +74,65 @@
|
|||||||
#else
|
#else
|
||||||
#include "WProgram.h"
|
#include "WProgram.h"
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
/* Include definition of serial commands */
|
||||||
#include "commands.h"
|
#include "commands.h"
|
||||||
#include "motor_driver.h"
|
|
||||||
#include "encoder_driver.h"
|
/* Sensor functions */
|
||||||
#include "diff_controller.h"
|
#include "sensors.h"
|
||||||
#define PID_RATE 30 // Hz
|
|
||||||
const int PID_INTERVAL = 1000 / PID_RATE;
|
/* Include servo support if required */
|
||||||
unsigned long nextPID = PID_INTERVAL;
|
#ifdef USE_SERVOS
|
||||||
#define AUTO_STOP_INTERVAL 2000
|
#include <Servo.h>
|
||||||
long lastMotorCommand = AUTO_STOP_INTERVAL;
|
#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 arg = 0;
|
||||||
int index = 0;
|
int index = 0;
|
||||||
|
|
||||||
|
// Variable to hold an input character
|
||||||
char chr;
|
char chr;
|
||||||
|
|
||||||
|
// Variable to hold the current single-character command
|
||||||
char cmd;
|
char cmd;
|
||||||
|
|
||||||
|
// Character arrays to hold the first and second arguments
|
||||||
char argv1[16];
|
char argv1[16];
|
||||||
char argv2[16];
|
char argv2[16];
|
||||||
|
|
||||||
|
// The arguments converted to integers
|
||||||
long arg1;
|
long arg1;
|
||||||
long arg2;
|
long arg2;
|
||||||
|
|
||||||
|
/* Clear the current command parameters */
|
||||||
void resetCommand() {
|
void resetCommand() {
|
||||||
cmd = NULL;
|
cmd = NULL;
|
||||||
memset(argv1, 0, sizeof(argv1));
|
memset(argv1, 0, sizeof(argv1));
|
||||||
@ -34,6 +143,7 @@ void resetCommand() {
|
|||||||
index = 0;
|
index = 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/* Run a command. Commands are defined in commands.h */
|
||||||
int runCommand() {
|
int runCommand() {
|
||||||
int i = 0;
|
int i = 0;
|
||||||
char *p = argv1;
|
char *p = argv1;
|
||||||
@ -46,7 +156,40 @@ int runCommand() {
|
|||||||
case GET_BAUDRATE:
|
case GET_BAUDRATE:
|
||||||
Serial.println(BAUDRATE);
|
Serial.println(BAUDRATE);
|
||||||
break;
|
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:
|
case READ_ENCODERS:
|
||||||
Serial.print(readEncoder(LEFT));
|
Serial.print(readEncoder(LEFT));
|
||||||
Serial.print(" ");
|
Serial.print(" ");
|
||||||
@ -62,49 +205,25 @@ int runCommand() {
|
|||||||
lastMotorCommand = millis();
|
lastMotorCommand = millis();
|
||||||
if (arg1 == 0 && arg2 == 0) {
|
if (arg1 == 0 && arg2 == 0) {
|
||||||
setMotorSpeeds(0, 0);
|
setMotorSpeeds(0, 0);
|
||||||
resetPID();
|
|
||||||
moving = 0;
|
moving = 0;
|
||||||
}
|
}
|
||||||
else moving = 1;
|
else moving = 1;
|
||||||
leftPID.TargetTicksPerFrame = arg1;
|
leftPID.TargetTicksPerFrame = arg1;
|
||||||
rightPID.TargetTicksPerFrame = arg2;
|
rightPID.TargetTicksPerFrame = arg2;
|
||||||
|
|
||||||
Serial.println("OK");
|
Serial.println("OK");
|
||||||
break;
|
break;
|
||||||
|
case UPDATE_PID:
|
||||||
case UPDATE_PID_L:
|
|
||||||
while ((str = strtok_r(p, ":", &p)) != '\0') {
|
while ((str = strtok_r(p, ":", &p)) != '\0') {
|
||||||
pid_args[i] = atoi(str);
|
pid_args[i] = atoi(str);
|
||||||
i++;
|
i++;
|
||||||
}
|
}
|
||||||
leftPID.Kp = pid_args[0];
|
Kp = pid_args[0];
|
||||||
leftPID.Kd = pid_args[1];
|
Kd = pid_args[1];
|
||||||
leftPID.Ki = pid_args[2];
|
Ki = pid_args[2];
|
||||||
leftPID.Ko = pid_args[3];
|
Ko = pid_args[3];
|
||||||
Serial.println("OK");
|
|
||||||
break;
|
|
||||||
case UPDATE_PID_R:
|
|
||||||
while ((str = strtok_r(p, ":", &p)) != '\0') {
|
|
||||||
pid_args[i] = atoi(str);
|
|
||||||
i++;
|
|
||||||
}
|
|
||||||
rightPID.Kp = pid_args[0];
|
|
||||||
rightPID.Kd = pid_args[1];
|
|
||||||
rightPID.Ki = pid_args[2];
|
|
||||||
rightPID.Ko = pid_args[3];
|
|
||||||
Serial.println("OK");
|
|
||||||
break;
|
|
||||||
case DISP_PID_P:
|
|
||||||
Serial.println(leftPID.Kp);
|
|
||||||
Serial.println(leftPID.Kd);
|
|
||||||
Serial.println(leftPID.Ki);
|
|
||||||
Serial.println(leftPID.Ko);
|
|
||||||
Serial.println(rightPID.Kp);
|
|
||||||
Serial.println(rightPID.Kd);
|
|
||||||
Serial.println(rightPID.Ki);
|
|
||||||
Serial.println(rightPID.Ko);
|
|
||||||
Serial.println("OK");
|
Serial.println("OK");
|
||||||
break;
|
break;
|
||||||
|
#endif
|
||||||
default:
|
default:
|
||||||
Serial.println("Invalid Command");
|
Serial.println("Invalid Command");
|
||||||
break;
|
break;
|
||||||
@ -115,34 +234,26 @@ int runCommand() {
|
|||||||
void setup() {
|
void setup() {
|
||||||
Serial.begin(BAUDRATE);
|
Serial.begin(BAUDRATE);
|
||||||
|
|
||||||
DDRD &= ~(1<<LEFT_ENC_PIN_A);
|
// Initialize the motor controller if used */
|
||||||
DDRD &= ~(1<<LEFT_ENC_PIN_B);
|
#ifdef USE_BASE
|
||||||
DDRC &= ~(1<<RIGHT_ENC_PIN_A);
|
|
||||||
DDRC &= ~(1<<RIGHT_ENC_PIN_B);
|
|
||||||
|
|
||||||
//enable pull up resistors
|
|
||||||
PORTD |= (1<<LEFT_ENC_PIN_A);
|
|
||||||
PORTD |= (1<<LEFT_ENC_PIN_B);
|
|
||||||
PORTC |= (1<<RIGHT_ENC_PIN_A);
|
|
||||||
PORTC |= (1<<RIGHT_ENC_PIN_B);
|
|
||||||
|
|
||||||
// tell pin change mask to listen to left encoder pins
|
|
||||||
PCMSK2 |= (1 << LEFT_ENC_PIN_A)|(1 << LEFT_ENC_PIN_B);
|
|
||||||
// tell pin change mask to listen to right encoder pins
|
|
||||||
PCMSK1 |= (1 << RIGHT_ENC_PIN_A)|(1 << RIGHT_ENC_PIN_B);
|
|
||||||
|
|
||||||
// enable PCINT1 and PCINT2 interrupt in the general interrupt mask
|
|
||||||
PCICR |= (1 << PCIE1) | (1 << PCIE2);
|
|
||||||
|
|
||||||
initMotorController();
|
initMotorController();
|
||||||
resetPID();
|
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() {
|
void loop() {
|
||||||
|
|
||||||
|
|
||||||
while (Serial.available() > 0) {
|
while (Serial.available() > 0) {
|
||||||
|
|
||||||
// Read the next character
|
// Read the next character
|
||||||
@ -183,6 +294,8 @@ void loop() {
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// If we are using base control, run a PID calculation at the appropriate intervals
|
||||||
|
#ifdef USE_BASE
|
||||||
if (millis() > nextPID) {
|
if (millis() > nextPID) {
|
||||||
updatePID();
|
updatePID();
|
||||||
nextPID += PID_INTERVAL;
|
nextPID += PID_INTERVAL;
|
||||||
@ -194,5 +307,11 @@ void loop() {
|
|||||||
moving = 0;
|
moving = 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
#endif
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|||||||
@ -5,14 +5,19 @@
|
|||||||
#ifndef COMMANDS_H
|
#ifndef COMMANDS_H
|
||||||
#define COMMANDS_H
|
#define COMMANDS_H
|
||||||
|
|
||||||
|
#define ANALOG_READ 'a'
|
||||||
#define GET_BAUDRATE 'b'
|
#define GET_BAUDRATE 'b'
|
||||||
|
#define PIN_MODE 'c'
|
||||||
|
#define DIGITAL_READ 'd'
|
||||||
#define READ_ENCODERS 'e'
|
#define READ_ENCODERS 'e'
|
||||||
#define MOTOR_SPEEDS 'm'
|
#define MOTOR_SPEEDS 'm'
|
||||||
|
#define PING 'p'
|
||||||
#define RESET_ENCODERS 'r'
|
#define RESET_ENCODERS 'r'
|
||||||
#define UPDATE_PID_L 'L'
|
#define SERVO_WRITE 's'
|
||||||
#define UPDATE_PID_R 'R'
|
#define SERVO_READ 't'
|
||||||
#define DISP_PID_P 'z'
|
#define UPDATE_PID 'u'
|
||||||
|
#define DIGITAL_WRITE 'w'
|
||||||
|
#define ANALOG_WRITE 'x'
|
||||||
#define LEFT 0
|
#define LEFT 0
|
||||||
#define RIGHT 1
|
#define RIGHT 1
|
||||||
|
|
||||||
|
|||||||
@ -1,16 +1,30 @@
|
|||||||
|
/* 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 {
|
typedef struct {
|
||||||
double TargetTicksPerFrame; // target speed in ticks per frame
|
double TargetTicksPerFrame; // target speed in ticks per frame
|
||||||
long Encoder; // encoder count
|
long Encoder; // encoder count
|
||||||
long PrevEnc; // last 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 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
|
int ITerm; //integrated term
|
||||||
int Kp = 20;
|
|
||||||
int Kd = 12;
|
|
||||||
int Ki = 0;
|
|
||||||
int Ko = 50;
|
|
||||||
|
|
||||||
long output; // last motor setting
|
long output; // last motor setting
|
||||||
}
|
}
|
||||||
@ -18,25 +32,39 @@ SetPointInfo;
|
|||||||
|
|
||||||
SetPointInfo leftPID, rightPID;
|
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?
|
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(){
|
void resetPID(){
|
||||||
leftPID.TargetTicksPerFrame = 0.0;
|
leftPID.TargetTicksPerFrame = 0.0;
|
||||||
leftPID.Encoder = readEncoder(LEFT);
|
leftPID.Encoder = readEncoder(0);
|
||||||
leftPID.PrevEnc = leftPID.Encoder;
|
leftPID.PrevEnc = leftPID.Encoder;
|
||||||
leftPID.output = 0;
|
leftPID.output = 0;
|
||||||
leftPID.PrevInput = 0;
|
leftPID.PrevInput = 0;
|
||||||
leftPID.ITerm = 0;
|
leftPID.ITerm = 0;
|
||||||
|
|
||||||
rightPID.TargetTicksPerFrame = 0.0;
|
rightPID.TargetTicksPerFrame = 0.0;
|
||||||
rightPID.Encoder = readEncoder(RIGHT);
|
rightPID.Encoder = readEncoder(1);
|
||||||
rightPID.PrevEnc = rightPID.Encoder;
|
rightPID.PrevEnc = rightPID.Encoder;
|
||||||
rightPID.output = 0;
|
rightPID.output = 0;
|
||||||
rightPID.PrevInput = 0;
|
rightPID.PrevInput = 0;
|
||||||
rightPID.ITerm = 0;
|
rightPID.ITerm = 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/* PID routine to compute the next motor commands */
|
||||||
void doPID(SetPointInfo * p) {
|
void doPID(SetPointInfo * p) {
|
||||||
long Perror;
|
long Perror;
|
||||||
long output;
|
long output;
|
||||||
@ -46,18 +74,29 @@ void doPID(SetPointInfo * p) {
|
|||||||
input = p->Encoder - p->PrevEnc;
|
input = p->Encoder - p->PrevEnc;
|
||||||
Perror = p->TargetTicksPerFrame - input;
|
Perror = p->TargetTicksPerFrame - input;
|
||||||
|
|
||||||
output = ((p->Kp) * Perror + (p->Kd) * (input - p->PrevInput) + p->ITerm) / (p->Ko);
|
|
||||||
|
/*
|
||||||
|
* 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;
|
p->PrevEnc = p->Encoder;
|
||||||
|
|
||||||
output += p->output;
|
output += p->output;
|
||||||
|
// Accumulate Integral error *or* Limit output.
|
||||||
|
// Stop accumulating when output saturates
|
||||||
if (output >= MAX_PWM)
|
if (output >= MAX_PWM)
|
||||||
output = MAX_PWM;
|
output = MAX_PWM;
|
||||||
else if (output <= -MAX_PWM)
|
else if (output <= -MAX_PWM)
|
||||||
output = -MAX_PWM;
|
output = -MAX_PWM;
|
||||||
else
|
else
|
||||||
|
/*
|
||||||
p->ITerm += (p->Ki) * Perror;
|
* 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->output = output;
|
||||||
p->PrevInput = input;
|
p->PrevInput = input;
|
||||||
@ -66,17 +105,26 @@ void doPID(SetPointInfo * p) {
|
|||||||
/* Read the encoder values and call the PID routine */
|
/* Read the encoder values and call the PID routine */
|
||||||
void updatePID() {
|
void updatePID() {
|
||||||
/* Read the encoders */
|
/* Read the encoders */
|
||||||
leftPID.Encoder = readEncoder(LEFT);
|
leftPID.Encoder = readEncoder(0);
|
||||||
rightPID.Encoder = readEncoder(RIGHT);
|
rightPID.Encoder = readEncoder(1);
|
||||||
|
|
||||||
|
/* If we're not moving there is nothing more to do */
|
||||||
if (!moving){
|
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();
|
if (leftPID.PrevInput != 0 || rightPID.PrevInput != 0) resetPID();
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
/* Compute PID update for each motor */
|
/* Compute PID update for each motor */
|
||||||
doPID(&leftPID);
|
|
||||||
doPID(&rightPID);
|
doPID(&rightPID);
|
||||||
|
doPID(&leftPID);
|
||||||
|
|
||||||
|
/* Set the motor speeds accordingly */
|
||||||
setMotorSpeeds(leftPID.output, rightPID.output);
|
setMotorSpeeds(leftPID.output, rightPID.output);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@ -1,8 +1,6 @@
|
|||||||
#define LEFT_ENC_PIN_A PD2 //pin 2
|
/* *************************************************************
|
||||||
#define LEFT_ENC_PIN_B PD3 //pin 3
|
Encoder driver function definitions - by James Nugen
|
||||||
|
************************************************************ */
|
||||||
#define RIGHT_ENC_PIN_A PC4 //pin A4
|
|
||||||
#define RIGHT_ENC_PIN_B PC5 //pin A5
|
|
||||||
|
|
||||||
long readEncoder(int i);
|
long readEncoder(int i);
|
||||||
void resetEncoder(int i);
|
void resetEncoder(int i);
|
||||||
|
|||||||
@ -1,43 +1,41 @@
|
|||||||
volatile long left_enc_pos = 0L;
|
/* *************************************************************
|
||||||
volatile long right_enc_pos = 0L;
|
Encoder definitions
|
||||||
static const int8_t ENC_STATES [] = {0,1,-1,0,-1,0,0,1,1,0,0,-1,0,-1,1,0}; //encoder lookup table
|
|
||||||
|
|
||||||
ISR (PCINT2_vect){
|
Add a "#if defined" block to this file to include support for
|
||||||
static uint8_t enc_last=0;
|
a particular encoder board or library. Then add the appropriate
|
||||||
|
#define near the top of the main ROSArduinoBridge.ino file.
|
||||||
|
|
||||||
enc_last <<=2; //shift previous state two places
|
************************************************************ */
|
||||||
enc_last |= (PIND & (3 << 2)) >> 2; //read the current state into lowest 2 bits
|
|
||||||
|
|
||||||
left_enc_pos += ENC_STATES[(enc_last & 0x0f)];
|
#ifdef USE_BASE
|
||||||
}
|
|
||||||
|
|
||||||
ISR (PCINT1_vect){
|
#if defined ROBOGAIA
|
||||||
static uint8_t enc_last=0;
|
/* The Robogaia Mega Encoder shield */
|
||||||
|
#include "MegaEncoderCounter.h"
|
||||||
|
|
||||||
enc_last <<=2; //shift previous state two places
|
/* Create the encoder shield object */
|
||||||
enc_last |= (PINC & (3 << 4)) >> 4; //read the current state into lowest 2 bits
|
MegaEncoderCounter encoders = MegaEncoderCounter(4); // Initializes the Mega Encoder Counter in the 4X Count mode
|
||||||
|
|
||||||
right_enc_pos += ENC_STATES[(enc_last & 0x0f)];
|
|
||||||
}
|
|
||||||
|
|
||||||
|
/* Wrap the encoder reading function */
|
||||||
long readEncoder(int i) {
|
long readEncoder(int i) {
|
||||||
if (i == LEFT) return left_enc_pos;
|
if (i == LEFT) return encoders.YAxisGetCount();
|
||||||
else return right_enc_pos;
|
else return encoders.XAxisGetCount();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/* Wrap the encoder reset function */
|
||||||
void resetEncoder(int i) {
|
void resetEncoder(int i) {
|
||||||
if (i == LEFT){
|
if (i == LEFT) return encoders.YAxisReset();
|
||||||
left_enc_pos=0L;
|
else return encoders.XAxisReset();
|
||||||
return;
|
|
||||||
} else {
|
|
||||||
right_enc_pos=0L;
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
#else
|
||||||
|
#error A encoder driver must be selected!
|
||||||
|
#endif
|
||||||
|
|
||||||
|
/* Wrap the encoder reset function */
|
||||||
void resetEncoders() {
|
void resetEncoders() {
|
||||||
resetEncoder(LEFT);
|
resetEncoder(LEFT);
|
||||||
resetEncoder(RIGHT);
|
resetEncoder(RIGHT);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
||||||
|
|||||||
@ -1,9 +1,6 @@
|
|||||||
#define RIGHT_MOTOR_BACKWARD 5
|
/***************************************************************
|
||||||
#define LEFT_MOTOR_BACKWARD 6
|
Motor driver function definitions - by James Nugen
|
||||||
#define RIGHT_MOTOR_FORWARD 9
|
*************************************************************/
|
||||||
#define LEFT_MOTOR_FORWARD 10
|
|
||||||
#define RIGHT_MOTOR_ENABLE 12
|
|
||||||
#define LEFT_MOTOR_ENABLE 13
|
|
||||||
|
|
||||||
void initMotorController();
|
void initMotorController();
|
||||||
void setMotorSpeed(int i, int spd);
|
void setMotorSpeed(int i, int spd);
|
||||||
|
|||||||
@ -1,32 +1,62 @@
|
|||||||
|
/***************************************************************
|
||||||
|
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() {
|
void initMotorController() {
|
||||||
digitalWrite(RIGHT_MOTOR_ENABLE, HIGH);
|
drive.init();
|
||||||
digitalWrite(LEFT_MOTOR_ENABLE, HIGH);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/* Wrap the drive motor set speed function */
|
||||||
void setMotorSpeed(int i, int spd) {
|
void setMotorSpeed(int i, int spd) {
|
||||||
unsigned char reverse = 0;
|
if (i == LEFT) drive.setM1Speed(spd);
|
||||||
|
else drive.setM2Speed(spd);
|
||||||
if (spd < 0)
|
|
||||||
{
|
|
||||||
spd = -spd;
|
|
||||||
reverse = 1;
|
|
||||||
}
|
|
||||||
if (spd > 255)
|
|
||||||
spd = 255;
|
|
||||||
|
|
||||||
if (i == LEFT) {
|
|
||||||
if(spd == 0) { digitalWrite(LEFT_MOTOR_FORWARD, 1); digitalWrite(LEFT_MOTOR_BACKWARD, 1); }
|
|
||||||
else if (reverse == 0) { analogWrite(LEFT_MOTOR_FORWARD, spd); analogWrite(LEFT_MOTOR_BACKWARD, 0); }
|
|
||||||
else if (reverse == 1) { analogWrite(LEFT_MOTOR_BACKWARD, spd); analogWrite(LEFT_MOTOR_FORWARD, 0); }
|
|
||||||
}
|
|
||||||
else if (i == RIGHT){
|
|
||||||
if(spd == 0) { digitalWrite(RIGHT_MOTOR_FORWARD, 1); digitalWrite(RIGHT_MOTOR_BACKWARD, 1); }
|
|
||||||
else if (reverse == 0) { analogWrite(RIGHT_MOTOR_FORWARD, spd); analogWrite(RIGHT_MOTOR_BACKWARD, 0); }
|
|
||||||
else if (reverse == 1) { analogWrite(RIGHT_MOTOR_BACKWARD, spd); analogWrite(RIGHT_MOTOR_FORWARD, 0); }
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// A convenience function for setting both motor speeds
|
||||||
void setMotorSpeeds(int leftSpeed, int rightSpeed) {
|
void setMotorSpeeds(int leftSpeed, int rightSpeed) {
|
||||||
setMotorSpeed(LEFT, leftSpeed);
|
setMotorSpeed(LEFT, leftSpeed);
|
||||||
setMotorSpeed(RIGHT, rightSpeed);
|
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
|
||||||
|
|||||||
@ -0,0 +1,34 @@
|
|||||||
|
/* 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);
|
||||||
|
}
|
||||||
|
|
||||||
@ -0,0 +1,9 @@
|
|||||||
|
/* 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};
|
||||||
|
|
||||||
@ -1,29 +1,30 @@
|
|||||||
cmake_minimum_required(VERSION 2.8.3)
|
cmake_minimum_required(VERSION 2.4.6)
|
||||||
project(ros_arduino_msgs)
|
include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
|
||||||
|
|
||||||
find_package(catkin REQUIRED COMPONENTS std_msgs message_generation)
|
# 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)
|
||||||
|
|
||||||
add_message_files(FILES
|
rosbuild_init()
|
||||||
AnalogFloat.msg
|
|
||||||
Analog.msg
|
|
||||||
ArduinoConstants.msg
|
|
||||||
Digital.msg
|
|
||||||
SensorState.msg
|
|
||||||
)
|
|
||||||
|
|
||||||
add_service_files(FILES
|
#set the default path for built executables to the "bin" directory
|
||||||
DigitalSetDirection.srv
|
set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
|
||||||
DigitalWrite.srv
|
#set the default path for built libraries to the "lib" directory
|
||||||
DigitalRead.srv
|
set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
|
||||||
ServoRead.srv
|
|
||||||
ServoWrite.srv
|
|
||||||
AnalogWrite.srv
|
|
||||||
AnalogRead.srv
|
|
||||||
)
|
|
||||||
|
|
||||||
generate_messages(
|
#uncomment if you have defined messages
|
||||||
DEPENDENCIES
|
rosbuild_genmsg()
|
||||||
std_msgs
|
#uncomment if you have defined services
|
||||||
)
|
rosbuild_gensrv()
|
||||||
|
|
||||||
catkin_package(CATKIN_DEPENDS message_runtime std_msgs)
|
#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
Normal file
1
ros_arduino_msgs/Makefile
Normal file
@ -0,0 +1 @@
|
|||||||
|
include $(shell rospack find mk)/cmake.mk
|
||||||
26
ros_arduino_msgs/mainpage.dox
Normal file
26
ros_arduino_msgs/mainpage.dox
Normal file
@ -0,0 +1,26 @@
|
|||||||
|
/**
|
||||||
|
\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
Normal file
13
ros_arduino_msgs/manifest.xml
Normal file
@ -0,0 +1,13 @@
|
|||||||
|
<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>
|
||||||
|
|
||||||
|
|
||||||
@ -1,5 +0,0 @@
|
|||||||
# Arduino-style constants
|
|
||||||
uint8 LOW=0
|
|
||||||
uint8 HIGH=1
|
|
||||||
uint8 INPUT=0
|
|
||||||
uint8 OUTPUT=1
|
|
||||||
@ -1,20 +0,0 @@
|
|||||||
<package>
|
|
||||||
<name>ros_arduino_msgs</name>
|
|
||||||
<version>0.2.0</version>
|
|
||||||
<description>
|
|
||||||
ROS Arduino Messages.
|
|
||||||
</description>
|
|
||||||
<author>Patrick Goebel</author>
|
|
||||||
<maintainer email="patrick@pirobot.org">Patrick Goebel</maintainer>
|
|
||||||
<license>BSD</license>
|
|
||||||
<url>http://ros.org/wiki/ros_arduino_msgs</url>
|
|
||||||
|
|
||||||
<buildtool_depend>catkin</buildtool_depend>
|
|
||||||
|
|
||||||
<build_depend>message_generation</build_depend>
|
|
||||||
<build_depend>std_msgs</build_depend>
|
|
||||||
|
|
||||||
<run_depend>message_runtime</run_depend>
|
|
||||||
<run_depend>std_msgs</run_depend>
|
|
||||||
|
|
||||||
</package>
|
|
||||||
0
ros_arduino_msgs/src/ros_arduino_msgs/__init__.py
Normal file
0
ros_arduino_msgs/src/ros_arduino_msgs/__init__.py
Normal file
@ -1,3 +0,0 @@
|
|||||||
uint8 pin
|
|
||||||
---
|
|
||||||
uint16 value
|
|
||||||
@ -1,3 +0,0 @@
|
|||||||
uint8 pin
|
|
||||||
uint16 value
|
|
||||||
---
|
|
||||||
@ -1,3 +0,0 @@
|
|||||||
uint8 pin
|
|
||||||
---
|
|
||||||
bool value
|
|
||||||
@ -1,3 +1,3 @@
|
|||||||
uint8 id
|
uint8 id
|
||||||
---
|
---
|
||||||
float32 value
|
int16 value
|
||||||
|
|||||||
@ -1,3 +1,3 @@
|
|||||||
uint8 id
|
uint8 id
|
||||||
float32 value
|
int16 value
|
||||||
---
|
---
|
||||||
|
|||||||
@ -1,18 +1,30 @@
|
|||||||
cmake_minimum_required(VERSION 2.8.3)
|
cmake_minimum_required(VERSION 2.4.6)
|
||||||
project(ros_arduino_python)
|
include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
|
||||||
|
|
||||||
find_package(catkin REQUIRED)
|
# Set the build type. Options are:
|
||||||
catkin_package(DEPENDS)
|
# Coverage : w/ debug symbols, w/o optimization, w/ code-coverage
|
||||||
catkin_python_setup()
|
# 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)
|
||||||
|
|
||||||
install(DIRECTORY config
|
rosbuild_init()
|
||||||
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
|
|
||||||
)
|
|
||||||
|
|
||||||
install(DIRECTORY launch
|
#set the default path for built executables to the "bin" directory
|
||||||
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
|
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)
|
||||||
|
|
||||||
install(DIRECTORY nodes
|
#uncomment if you have defined messages
|
||||||
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
|
#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
Normal file
1
ros_arduino_python/Makefile
Normal file
@ -0,0 +1 @@
|
|||||||
|
include $(shell rospack find mk)/cmake.mk
|
||||||
@ -13,9 +13,6 @@ sensorstate_rate: 10
|
|||||||
use_base_controller: False
|
use_base_controller: False
|
||||||
base_controller_rate: 10
|
base_controller_rate: 10
|
||||||
|
|
||||||
# For a robot that uses base_footprint, change base_frame to base_footprint
|
|
||||||
base_frame: base_link
|
|
||||||
|
|
||||||
# === Robot drivetrain parameters
|
# === Robot drivetrain parameters
|
||||||
#wheel_diameter: 0.146
|
#wheel_diameter: 0.146
|
||||||
#wheel_track: 0.2969
|
#wheel_track: 0.2969
|
||||||
@ -24,15 +21,10 @@ base_frame: base_link
|
|||||||
#motors_reversed: True
|
#motors_reversed: True
|
||||||
|
|
||||||
# === PID parameters
|
# === PID parameters
|
||||||
#lKp: 10
|
#Kp: 10
|
||||||
#lKd: 12
|
#Kd: 12
|
||||||
#lKi: 0
|
#Ki: 0
|
||||||
#lKo: 50
|
#Ko: 50
|
||||||
#rKp: 10
|
|
||||||
#rKd: 12
|
|
||||||
#rKi: 0
|
|
||||||
#rKo: 50
|
|
||||||
|
|
||||||
#accel_limit: 1.0
|
#accel_limit: 1.0
|
||||||
|
|
||||||
# === Sensor definitions. Examples only - edit for your robot.
|
# === Sensor definitions. Examples only - edit for your robot.
|
||||||
|
|||||||
26
ros_arduino_python/mainpage.dox
Normal file
26
ros_arduino_python/mainpage.dox
Normal file
@ -0,0 +1,26 @@
|
|||||||
|
/**
|
||||||
|
\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
Normal file
24
ros_arduino_python/manifest.xml
Normal file
@ -0,0 +1,24 @@
|
|||||||
|
<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>
|
||||||
|
|
||||||
|
|
||||||
@ -19,22 +19,19 @@
|
|||||||
http://www.gnu.org/licenses/gpl.html
|
http://www.gnu.org/licenses/gpl.html
|
||||||
"""
|
"""
|
||||||
|
|
||||||
|
import roslib; roslib.load_manifest('ros_arduino_python')
|
||||||
import rospy
|
import rospy
|
||||||
from ros_arduino_python.arduino_driver import Arduino
|
from arduino_driver import Arduino
|
||||||
from ros_arduino_python.arduino_sensors import *
|
from arduino_sensors import *
|
||||||
from ros_arduino_msgs.srv import *
|
from ros_arduino_msgs.srv import *
|
||||||
from ros_arduino_python.base_controller import BaseController
|
from base_controller import BaseController
|
||||||
from geometry_msgs.msg import Twist
|
from geometry_msgs.msg import Twist
|
||||||
import os, time
|
import os, time
|
||||||
import thread
|
import thread
|
||||||
from serial.serialutil import SerialException
|
|
||||||
|
|
||||||
class ArduinoROS():
|
class ArduinoROS():
|
||||||
def __init__(self):
|
def __init__(self):
|
||||||
rospy.init_node('arduino', log_level=rospy.INFO)
|
rospy.init_node('Arduino', log_level=rospy.DEBUG)
|
||||||
|
|
||||||
# Get the actual node name in case it is set in the launch file
|
|
||||||
self.name = rospy.get_name()
|
|
||||||
|
|
||||||
# Cleanup when termniating the node
|
# Cleanup when termniating the node
|
||||||
rospy.on_shutdown(self.shutdown)
|
rospy.on_shutdown(self.shutdown)
|
||||||
@ -42,8 +39,7 @@ class ArduinoROS():
|
|||||||
self.port = rospy.get_param("~port", "/dev/ttyACM0")
|
self.port = rospy.get_param("~port", "/dev/ttyACM0")
|
||||||
self.baud = int(rospy.get_param("~baud", 57600))
|
self.baud = int(rospy.get_param("~baud", 57600))
|
||||||
self.timeout = rospy.get_param("~timeout", 0.5)
|
self.timeout = rospy.get_param("~timeout", 0.5)
|
||||||
self.base_frame = rospy.get_param("~base_frame", 'base_link')
|
|
||||||
self.motors_reversed = rospy.get_param("~motors_reversed", False)
|
|
||||||
# Overall loop rate: should be faster than fastest sensor rate
|
# Overall loop rate: should be faster than fastest sensor rate
|
||||||
self.rate = int(rospy.get_param("~rate", 50))
|
self.rate = int(rospy.get_param("~rate", 50))
|
||||||
r = rospy.Rate(self.rate)
|
r = rospy.Rate(self.rate)
|
||||||
@ -63,11 +59,11 @@ class ArduinoROS():
|
|||||||
self.cmd_vel = Twist()
|
self.cmd_vel = Twist()
|
||||||
|
|
||||||
# A cmd_vel publisher so we can stop the robot when shutting down
|
# A cmd_vel publisher so we can stop the robot when shutting down
|
||||||
self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=5)
|
self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist)
|
||||||
|
|
||||||
# The SensorState publisher periodically publishes the values of all sensors on
|
# The SensorState publisher periodically publishes the values of all sensors on
|
||||||
# a single topic.
|
# a single topic.
|
||||||
self.sensorStatePub = rospy.Publisher('~sensor_state', SensorState, queue_size=5)
|
self.sensorStatePub = rospy.Publisher('~sensor_state', SensorState)
|
||||||
|
|
||||||
# A service to position a PWM servo
|
# A service to position a PWM servo
|
||||||
rospy.Service('~servo_write', ServoWrite, self.ServoWriteHandler)
|
rospy.Service('~servo_write', ServoWrite, self.ServoWriteHandler)
|
||||||
@ -81,17 +77,8 @@ class ArduinoROS():
|
|||||||
# A service to turn a digital sensor on or off
|
# A service to turn a digital sensor on or off
|
||||||
rospy.Service('~digital_write', DigitalWrite, self.DigitalWriteHandler)
|
rospy.Service('~digital_write', DigitalWrite, self.DigitalWriteHandler)
|
||||||
|
|
||||||
# A service to read the value of a digital sensor
|
|
||||||
rospy.Service('~digital_read', DigitalRead, self.DigitalReadHandler)
|
|
||||||
|
|
||||||
# A service to set pwm values for the pins
|
|
||||||
rospy.Service('~analog_write', AnalogWrite, self.AnalogWriteHandler)
|
|
||||||
|
|
||||||
# A service to read the value of an analog sensor
|
|
||||||
rospy.Service('~analog_read', AnalogRead, self.AnalogReadHandler)
|
|
||||||
|
|
||||||
# Initialize the controlller
|
# Initialize the controlller
|
||||||
self.controller = Arduino(self.port, self.baud, self.timeout, self.motors_reversed)
|
self.controller = Arduino(self.port, self.baud, self.timeout)
|
||||||
|
|
||||||
# Make the connection
|
# Make the connection
|
||||||
self.controller.connect()
|
self.controller.connect()
|
||||||
@ -114,32 +101,30 @@ class ArduinoROS():
|
|||||||
params['direction'] = 'input'
|
params['direction'] = 'input'
|
||||||
|
|
||||||
if params['type'] == "Ping":
|
if params['type'] == "Ping":
|
||||||
sensor = Ping(self.controller, name, params['pin'], params['rate'], self.base_frame)
|
sensor = Ping(self.controller, name, params['pin'], params['rate'])
|
||||||
elif params['type'] == "GP2D12":
|
elif params['type'] == "GP2D12":
|
||||||
sensor = GP2D12(self.controller, name, params['pin'], params['rate'], self.base_frame)
|
sensor = GP2D12(self.controller, name, params['pin'], params['rate'])
|
||||||
elif params['type'] == 'Digital':
|
elif params['type'] == 'Digital':
|
||||||
sensor = DigitalSensor(self.controller, name, params['pin'], params['rate'], self.base_frame, direction=params['direction'])
|
sensor = DigitalSensor(self.controller, name, params['pin'], params['rate'], direction=params['direction'])
|
||||||
elif params['type'] == 'Analog':
|
elif params['type'] == 'Analog':
|
||||||
sensor = AnalogSensor(self.controller, name, params['pin'], params['rate'], self.base_frame, direction=params['direction'])
|
sensor = AnalogSensor(self.controller, name, params['pin'], params['rate'], direction=params['direction'])
|
||||||
elif params['type'] == 'PololuMotorCurrent':
|
elif params['type'] == 'PololuMotorCurrent':
|
||||||
sensor = PololuMotorCurrent(self.controller, name, params['pin'], params['rate'], self.base_frame)
|
sensor = PololuMotorCurrent(self.controller, name, params['pin'], params['rate'])
|
||||||
elif params['type'] == 'PhidgetsVoltage':
|
elif params['type'] == 'PhidgetsVoltage':
|
||||||
sensor = PhidgetsVoltage(self.controller, name, params['pin'], params['rate'], self.base_frame)
|
sensor = PhidgetsVoltage(self.controller, name, params['pin'], params['rate'])
|
||||||
elif params['type'] == 'PhidgetsCurrent':
|
elif params['type'] == 'PhidgetsCurrent':
|
||||||
sensor = PhidgetsCurrent(self.controller, name, params['pin'], params['rate'], self.base_frame)
|
sensor = PhidgetsCurrent(self.controller, name, params['pin'], params['rate'])
|
||||||
|
|
||||||
# if params['type'] == "MaxEZ1":
|
# if params['type'] == "MaxEZ1":
|
||||||
# self.sensors[len(self.sensors)]['trigger_pin'] = params['trigger_pin']
|
# self.sensors[len(self.sensors)]['trigger_pin'] = params['trigger_pin']
|
||||||
# self.sensors[len(self.sensors)]['output_pin'] = params['output_pin']
|
# self.sensors[len(self.sensors)]['output_pin'] = params['output_pin']
|
||||||
try:
|
|
||||||
self.mySensors.append(sensor)
|
self.mySensors.append(sensor)
|
||||||
rospy.loginfo(name + " " + str(params) + " published on topic " + rospy.get_name() + "/sensor/" + name)
|
rospy.loginfo(name + " " + str(params))
|
||||||
except:
|
|
||||||
rospy.logerr("Sensor type " + str(params['type']) + " not recognized.")
|
|
||||||
|
|
||||||
# Initialize the base controller if used
|
# Initialize the base controller if used
|
||||||
if self.use_base_controller:
|
if self.use_base_controller:
|
||||||
self.myBaseController = BaseController(self.controller, self.base_frame, self.name + "_base_controller")
|
self.myBaseController = BaseController(self.controller)
|
||||||
|
|
||||||
# Start polling the sensors and base controller
|
# Start polling the sensors and base controller
|
||||||
while not rospy.is_shutdown():
|
while not rospy.is_shutdown():
|
||||||
@ -158,7 +143,7 @@ class ArduinoROS():
|
|||||||
|
|
||||||
if now > self.t_next_sensors:
|
if now > self.t_next_sensors:
|
||||||
msg = SensorState()
|
msg = SensorState()
|
||||||
msg.header.frame_id = self.base_frame
|
msg.header.frame_id = 'base_link'
|
||||||
msg.header.stamp = now
|
msg.header.stamp = now
|
||||||
for i in range(len(self.mySensors)):
|
for i in range(len(self.mySensors)):
|
||||||
msg.name.append(self.mySensors[i].name)
|
msg.name.append(self.mySensors[i].name)
|
||||||
@ -178,8 +163,8 @@ class ArduinoROS():
|
|||||||
return ServoWriteResponse()
|
return ServoWriteResponse()
|
||||||
|
|
||||||
def ServoReadHandler(self, req):
|
def ServoReadHandler(self, req):
|
||||||
pos = self.controller.servo_read(req.id)
|
self.controller.servo_read(req.id)
|
||||||
return ServoReadResponse(pos)
|
return ServoReadResponse()
|
||||||
|
|
||||||
def DigitalSetDirectionHandler(self, req):
|
def DigitalSetDirectionHandler(self, req):
|
||||||
self.controller.pin_mode(req.pin, req.direction)
|
self.controller.pin_mode(req.pin, req.direction)
|
||||||
@ -189,21 +174,7 @@ class ArduinoROS():
|
|||||||
self.controller.digital_write(req.pin, req.value)
|
self.controller.digital_write(req.pin, req.value)
|
||||||
return DigitalWriteResponse()
|
return DigitalWriteResponse()
|
||||||
|
|
||||||
def DigitalReadHandler(self, req):
|
|
||||||
value = self.controller.digital_read(req.pin)
|
|
||||||
return DigitalReadResponse(value)
|
|
||||||
|
|
||||||
def AnalogWriteHandler(self, req):
|
|
||||||
self.controller.analog_write(req.pin, req.value)
|
|
||||||
return AnalogWriteResponse()
|
|
||||||
|
|
||||||
def AnalogReadHandler(self, req):
|
|
||||||
value = self.controller.analog_read(req.pin)
|
|
||||||
return AnalogReadResponse(value)
|
|
||||||
|
|
||||||
def shutdown(self):
|
def shutdown(self):
|
||||||
rospy.loginfo("Shutting down Arduino Node...")
|
|
||||||
|
|
||||||
# Stop the robot
|
# Stop the robot
|
||||||
try:
|
try:
|
||||||
rospy.loginfo("Stopping the robot...")
|
rospy.loginfo("Stopping the robot...")
|
||||||
@ -211,19 +182,7 @@ class ArduinoROS():
|
|||||||
rospy.sleep(2)
|
rospy.sleep(2)
|
||||||
except:
|
except:
|
||||||
pass
|
pass
|
||||||
|
rospy.loginfo("Shutting down Arduino Node...")
|
||||||
# Close the serial port
|
|
||||||
try:
|
|
||||||
self.controller.close()
|
|
||||||
except:
|
|
||||||
pass
|
|
||||||
finally:
|
|
||||||
rospy.loginfo("Serial port closed.")
|
|
||||||
os._exit(0)
|
|
||||||
|
|
||||||
if __name__ == '__main__':
|
if __name__ == '__main__':
|
||||||
try:
|
myArduino = ArduinoROS()
|
||||||
myArduino = ArduinoROS()
|
|
||||||
except SerialException:
|
|
||||||
rospy.logerr("Serial exception trying to open port.")
|
|
||||||
os._exit(0)
|
|
||||||
|
|||||||
@ -1,22 +0,0 @@
|
|||||||
<package>
|
|
||||||
<name>ros_arduino_python</name>
|
|
||||||
<version>0.2.0</version>
|
|
||||||
<description>
|
|
||||||
ROS Arduino Python.
|
|
||||||
</description>
|
|
||||||
<author>Patrick Goebel</author>
|
|
||||||
<maintainer email="patrick@pirobot.org">Patrick Goebel</maintainer>
|
|
||||||
<license>BSD</license>
|
|
||||||
<url>http://ros.org/wiki/ros_arduino_python</url>
|
|
||||||
|
|
||||||
<buildtool_depend>catkin</buildtool_depend>
|
|
||||||
|
|
||||||
<run_depend>rospy</run_depend>
|
|
||||||
<run_depend>std_msgs</run_depend>
|
|
||||||
<run_depend>sensor_msgs</run_depend>
|
|
||||||
<run_depend>geometry_msgs</run_depend>
|
|
||||||
<run_depend>nav_msgs</run_depend>
|
|
||||||
<run_depend>tf</run_depend>
|
|
||||||
<run_depend>ros_arduino_msgs</run_depend>
|
|
||||||
<run_depend>python-serial</run_depend>
|
|
||||||
</package>
|
|
||||||
@ -1,11 +0,0 @@
|
|||||||
#!/usr/bin/env python
|
|
||||||
|
|
||||||
from distutils.core import setup
|
|
||||||
from catkin_pkg.python_setup import generate_distutils_setup
|
|
||||||
|
|
||||||
d = generate_distutils_setup(
|
|
||||||
packages=['ros_arduino_python'],
|
|
||||||
package_dir={'': 'src'},
|
|
||||||
)
|
|
||||||
|
|
||||||
setup(**d)
|
|
||||||
0
ros_arduino_python/src/__init__.py
Normal file
0
ros_arduino_python/src/__init__.py
Normal file
@ -38,7 +38,7 @@ class Arduino:
|
|||||||
N_ANALOG_PORTS = 6
|
N_ANALOG_PORTS = 6
|
||||||
N_DIGITAL_PORTS = 12
|
N_DIGITAL_PORTS = 12
|
||||||
|
|
||||||
def __init__(self, port="/dev/ttyUSB0", baudrate=57600, timeout=0.5, motors_reversed=False):
|
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_RATE = 30 # Do not change this! It is a fixed property of the Arduino PID controller.
|
||||||
self.PID_INTERVAL = 1000 / 30
|
self.PID_INTERVAL = 1000 / 30
|
||||||
@ -49,7 +49,7 @@ class Arduino:
|
|||||||
self.encoder_count = 0
|
self.encoder_count = 0
|
||||||
self.writeTimeout = timeout
|
self.writeTimeout = timeout
|
||||||
self.interCharTimeout = timeout / 30.
|
self.interCharTimeout = timeout / 30.
|
||||||
self.motors_reversed = motors_reversed
|
|
||||||
# Keep things thread safe
|
# Keep things thread safe
|
||||||
self.mutex = thread.allocate_lock()
|
self.mutex = thread.allocate_lock()
|
||||||
|
|
||||||
@ -248,22 +248,17 @@ class Arduino:
|
|||||||
self.mutex.release()
|
self.mutex.release()
|
||||||
return ack == 'OK'
|
return ack == 'OK'
|
||||||
|
|
||||||
def update_pid(self, lKp, lKd, lKi, lKo, rKp, rKd, rKi, rKo):
|
def update_pid(self, Kp, Kd, Ki, Ko):
|
||||||
''' Set the PID parameters on the Arduino
|
''' Set the PID parameters on the Arduino
|
||||||
'''
|
'''
|
||||||
print "Updating PID parameters"
|
print "Updating PID parameters"
|
||||||
cmd = 'L ' + str(lKp) + ':' + str(lKd) + ':' + str(lKi) + ':' + str(lKo)
|
cmd = 'u ' + str(Kp) + ':' + str(Kd) + ':' + str(Ki) + ':' + str(Ko)
|
||||||
self.execute_ack(cmd)
|
|
||||||
cmd = 'R ' + str(rKp) + ':' + str(rKd) + ':' + str(rKi) + ':' + str(rKo)
|
|
||||||
self.execute_ack(cmd)
|
self.execute_ack(cmd)
|
||||||
|
|
||||||
def get_baud(self):
|
def get_baud(self):
|
||||||
''' Get the current baud rate on the serial port.
|
''' Get the current baud rate on the serial port.
|
||||||
'''
|
'''
|
||||||
try:
|
return int(self.execute('b'));
|
||||||
return int(self.execute('b'));
|
|
||||||
except:
|
|
||||||
return None
|
|
||||||
|
|
||||||
def get_encoder_counts(self):
|
def get_encoder_counts(self):
|
||||||
values = self.execute_array('e')
|
values = self.execute_array('e')
|
||||||
@ -272,8 +267,6 @@ class Arduino:
|
|||||||
raise SerialException
|
raise SerialException
|
||||||
return None
|
return None
|
||||||
else:
|
else:
|
||||||
if self.motors_reversed:
|
|
||||||
values[0], values[1] = -values[0], -values[1]
|
|
||||||
return values
|
return values
|
||||||
|
|
||||||
def reset_encoders(self):
|
def reset_encoders(self):
|
||||||
@ -284,8 +277,6 @@ class Arduino:
|
|||||||
def drive(self, right, left):
|
def drive(self, right, left):
|
||||||
''' Speeds are given in encoder ticks per PID interval
|
''' Speeds are given in encoder ticks per PID interval
|
||||||
'''
|
'''
|
||||||
if self.motors_reversed:
|
|
||||||
left, right = -left, -right
|
|
||||||
return self.execute_ack('m %d %d' %(right, left))
|
return self.execute_ack('m %d %d' %(right, left))
|
||||||
|
|
||||||
def drive_m_per_s(self, right, left):
|
def drive_m_per_s(self, right, left):
|
||||||
@ -380,3 +371,4 @@ if __name__ == "__main__":
|
|||||||
myArduino.close()
|
myArduino.close()
|
||||||
|
|
||||||
print "Shutting down Arduino."
|
print "Shutting down Arduino."
|
||||||
|
|
||||||
@ -39,7 +39,7 @@ class MessageType:
|
|||||||
BOOL = 5
|
BOOL = 5
|
||||||
|
|
||||||
class Sensor(object):
|
class Sensor(object):
|
||||||
def __init__(self, controller, name, pin, rate, frame_id, direction="input", **kwargs):
|
def __init__(self, controller, name, pin, rate, frame_id="/base_link", direction="input", **kwargs):
|
||||||
self.controller = controller
|
self.controller = controller
|
||||||
self.name = name
|
self.name = name
|
||||||
self.pin = pin
|
self.pin = pin
|
||||||
@ -87,14 +87,7 @@ class AnalogSensor(Sensor):
|
|||||||
self.msg = Analog()
|
self.msg = Analog()
|
||||||
self.msg.header.frame_id = self.frame_id
|
self.msg.header.frame_id = self.frame_id
|
||||||
|
|
||||||
self.pub = rospy.Publisher("~sensor/" + self.name, Analog, queue_size=5)
|
self.pub = rospy.Publisher("~sensor/" + self.name, Analog)
|
||||||
|
|
||||||
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):
|
def read_value(self):
|
||||||
return self.controller.analog_read(self.pin)
|
return self.controller.analog_read(self.pin)
|
||||||
@ -111,20 +104,7 @@ class AnalogFloatSensor(Sensor):
|
|||||||
self.msg = AnalogFloat()
|
self.msg = AnalogFloat()
|
||||||
self.msg.header.frame_id = self.frame_id
|
self.msg.header.frame_id = self.frame_id
|
||||||
|
|
||||||
self.pub = rospy.Publisher("~sensor/" + self.name, AnalogFloat, queue_size=5)
|
self.pub = rospy.Publisher("~sensor/" + self.name, AnalogFloat)
|
||||||
|
|
||||||
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.analog_read(self.pin)
|
|
||||||
|
|
||||||
def write_value(self, value):
|
|
||||||
return self.controller.analog_write(self.pin, value)
|
|
||||||
|
|
||||||
|
|
||||||
class DigitalSensor(Sensor):
|
class DigitalSensor(Sensor):
|
||||||
@ -136,7 +116,7 @@ class DigitalSensor(Sensor):
|
|||||||
self.msg = Digital()
|
self.msg = Digital()
|
||||||
self.msg.header.frame_id = self.frame_id
|
self.msg.header.frame_id = self.frame_id
|
||||||
|
|
||||||
self.pub = rospy.Publisher("~sensor/" + self.name, Digital, queue_size=5)
|
self.pub = rospy.Publisher("~sensor/" + self.name, Digital)
|
||||||
|
|
||||||
if self.direction == "output":
|
if self.direction == "output":
|
||||||
self.controller.pin_mode(self.pin, OUTPUT)
|
self.controller.pin_mode(self.pin, OUTPUT)
|
||||||
@ -163,7 +143,7 @@ class RangeSensor(Sensor):
|
|||||||
self.msg = Range()
|
self.msg = Range()
|
||||||
self.msg.header.frame_id = self.frame_id
|
self.msg.header.frame_id = self.frame_id
|
||||||
|
|
||||||
self.pub = rospy.Publisher("~sensor/" + self.name, Range, queue_size=5)
|
self.pub = rospy.Publisher("~sensor/" + self.name, Range)
|
||||||
|
|
||||||
def read_value(self):
|
def read_value(self):
|
||||||
self.msg.header.stamp = rospy.Time.now()
|
self.msg.header.stamp = rospy.Time.now()
|
||||||
@ -31,12 +31,10 @@ from tf.broadcaster import TransformBroadcaster
|
|||||||
|
|
||||||
""" Class to receive Twist commands and publish Odometry data """
|
""" Class to receive Twist commands and publish Odometry data """
|
||||||
class BaseController:
|
class BaseController:
|
||||||
def __init__(self, arduino, base_frame, name="base_controllers"):
|
def __init__(self, arduino):
|
||||||
self.arduino = arduino
|
self.arduino = arduino
|
||||||
self.name = name
|
|
||||||
self.base_frame = base_frame
|
|
||||||
self.rate = float(rospy.get_param("~base_controller_rate", 10))
|
self.rate = float(rospy.get_param("~base_controller_rate", 10))
|
||||||
self.timeout = rospy.get_param("~base_controller_timeout", 1.0)
|
self.timeout = rospy.get_param('~base_controller_timeout', 1.0)
|
||||||
self.stopped = False
|
self.stopped = False
|
||||||
|
|
||||||
pid_params = dict()
|
pid_params = dict()
|
||||||
@ -44,15 +42,10 @@ class BaseController:
|
|||||||
pid_params['wheel_track'] = rospy.get_param("~wheel_track", "")
|
pid_params['wheel_track'] = rospy.get_param("~wheel_track", "")
|
||||||
pid_params['encoder_resolution'] = rospy.get_param("~encoder_resolution", "")
|
pid_params['encoder_resolution'] = rospy.get_param("~encoder_resolution", "")
|
||||||
pid_params['gear_reduction'] = rospy.get_param("~gear_reduction", 1.0)
|
pid_params['gear_reduction'] = rospy.get_param("~gear_reduction", 1.0)
|
||||||
pid_params['lKp'] = rospy.get_param("~lKp", 20)
|
pid_params['Kp'] = rospy.get_param("~Kp", 20)
|
||||||
pid_params['lKd'] = rospy.get_param("~lKd", 12)
|
pid_params['Kd'] = rospy.get_param("~Kd", 12)
|
||||||
pid_params['lKi'] = rospy.get_param("~lKi", 0)
|
pid_params['Ki'] = rospy.get_param("~Ki", 0)
|
||||||
pid_params['lKo'] = rospy.get_param("~lKo", 50)
|
pid_params['Ko'] = rospy.get_param("~Ko", 50)
|
||||||
pid_params['rKp'] = rospy.get_param("~rKp", 20)
|
|
||||||
pid_params['rKd'] = rospy.get_param("~rKd", 12)
|
|
||||||
pid_params['rKi'] = rospy.get_param("~rKi", 0)
|
|
||||||
pid_params['rKo'] = rospy.get_param("~rKo", 50)
|
|
||||||
|
|
||||||
|
|
||||||
self.accel_limit = rospy.get_param('~accel_limit', 0.1)
|
self.accel_limit = rospy.get_param('~accel_limit', 0.1)
|
||||||
self.motors_reversed = rospy.get_param("~motors_reversed", False)
|
self.motors_reversed = rospy.get_param("~motors_reversed", False)
|
||||||
@ -74,7 +67,7 @@ class BaseController:
|
|||||||
self.t_delta = rospy.Duration(1.0 / self.rate)
|
self.t_delta = rospy.Duration(1.0 / self.rate)
|
||||||
self.t_next = now + self.t_delta
|
self.t_next = now + self.t_delta
|
||||||
|
|
||||||
# Internal data
|
# internal data
|
||||||
self.enc_left = None # encoder readings
|
self.enc_left = None # encoder readings
|
||||||
self.enc_right = None
|
self.enc_right = None
|
||||||
self.x = 0 # position in xy plane
|
self.x = 0 # position in xy plane
|
||||||
@ -86,18 +79,18 @@ class BaseController:
|
|||||||
self.v_des_right = 0
|
self.v_des_right = 0
|
||||||
self.last_cmd_vel = now
|
self.last_cmd_vel = now
|
||||||
|
|
||||||
# Subscriptions
|
# subscriptions
|
||||||
rospy.Subscriber("cmd_vel", Twist, self.cmdVelCallback)
|
rospy.Subscriber("cmd_vel", Twist, self.cmdVelCallback)
|
||||||
|
|
||||||
# Clear any old odometry info
|
# Clear any old odometry info
|
||||||
self.arduino.reset_encoders()
|
self.arduino.reset_encoders()
|
||||||
|
|
||||||
# Set up the odometry broadcaster
|
# Set up the odometry broadcaster
|
||||||
self.odomPub = rospy.Publisher('odom', Odometry, queue_size=5)
|
self.odomPub = rospy.Publisher('odom', Odometry)
|
||||||
self.odomBroadcaster = TransformBroadcaster()
|
self.odomBroadcaster = TransformBroadcaster()
|
||||||
self.cameraBroadcaster = 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("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 using " + str(self.base_frame) + " as base frame")
|
rospy.loginfo("Publishing odometry data at: " + str(self.rate) + " Hz")
|
||||||
|
|
||||||
def setup_pid(self, pid_params):
|
def setup_pid(self, pid_params):
|
||||||
# Check to see if any PID parameters are missing
|
# Check to see if any PID parameters are missing
|
||||||
@ -115,17 +108,12 @@ class BaseController:
|
|||||||
self.encoder_resolution = pid_params['encoder_resolution']
|
self.encoder_resolution = pid_params['encoder_resolution']
|
||||||
self.gear_reduction = pid_params['gear_reduction']
|
self.gear_reduction = pid_params['gear_reduction']
|
||||||
|
|
||||||
self.lKp = pid_params['lKp']
|
self.Kp = pid_params['Kp']
|
||||||
self.lKd = pid_params['lKd']
|
self.Kd = pid_params['Kd']
|
||||||
self.lKi = pid_params['lKi']
|
self.Ki = pid_params['Ki']
|
||||||
self.lKo = pid_params['lKo']
|
self.Ko = pid_params['Ko']
|
||||||
|
|
||||||
self.rKp = pid_params['rKp']
|
self.arduino.update_pid(self.Kp, self.Kd, self.Ki, self.Ko)
|
||||||
self.rKd = pid_params['rKd']
|
|
||||||
self.rKi = pid_params['rKi']
|
|
||||||
self.rKo = pid_params['rKo']
|
|
||||||
|
|
||||||
self.arduino.update_pid(self.lKp, self.lKd, self.lKi, self.lKo,self.rKp, self.rKd, self.rKi, self.rKo)
|
|
||||||
|
|
||||||
def poll(self):
|
def poll(self):
|
||||||
now = rospy.Time.now()
|
now = rospy.Time.now()
|
||||||
@ -142,7 +130,7 @@ class BaseController:
|
|||||||
self.then = now
|
self.then = now
|
||||||
dt = dt.to_sec()
|
dt = dt.to_sec()
|
||||||
|
|
||||||
# Calculate odometry
|
# calculate odometry
|
||||||
if self.enc_left == None:
|
if self.enc_left == None:
|
||||||
dright = 0
|
dright = 0
|
||||||
dleft = 0
|
dleft = 0
|
||||||
@ -178,17 +166,13 @@ class BaseController:
|
|||||||
(self.x, self.y, 0),
|
(self.x, self.y, 0),
|
||||||
(quaternion.x, quaternion.y, quaternion.z, quaternion.w),
|
(quaternion.x, quaternion.y, quaternion.z, quaternion.w),
|
||||||
rospy.Time.now(),
|
rospy.Time.now(),
|
||||||
self.base_frame,
|
"base_link",
|
||||||
"odom"
|
"odom"
|
||||||
)
|
)
|
||||||
self.odomBroadcaster.sendTransform((self.x - 0.15, self.y - 0.055, 0.225),
|
|
||||||
(quaternion.x, quaternion.y, quaternion.z, quaternion.w),
|
|
||||||
rospy.Time.now(),
|
|
||||||
"depth_camera_frame",
|
|
||||||
self.base_frame)
|
|
||||||
odom = Odometry()
|
odom = Odometry()
|
||||||
odom.header.frame_id = "odom"
|
odom.header.frame_id = "odom"
|
||||||
odom.child_frame_id = self.base_frame
|
odom.child_frame_id = "base_link"
|
||||||
odom.header.stamp = now
|
odom.header.stamp = now
|
||||||
odom.pose.pose.position.x = self.x
|
odom.pose.pose.position.x = self.x
|
||||||
odom.pose.pose.position.y = self.y
|
odom.pose.pose.position.y = self.y
|
||||||
12
stack.xml
Normal file
12
stack.xml
Normal file
@ -0,0 +1,12 @@
|
|||||||
|
<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>
|
||||||
Loading…
x
Reference in New Issue
Block a user