indentation error fix

Adding l298 motor driver indigo

.gitignore

@@ -0,0 +1,106 @@
+
+# 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
+*~

CMakeLists.txt

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

Makefile

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

README.md

@@ -1,3 +1,11 @@
+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.
 
 Features of the stack include:
@@ -9,7 +17,6 @@ Features of the stack include:
 * Can control digital outputs (e.g. turn a switch or LED on and off)
 
 * Support for PWM servos
-
 * Configurable base controller if using the required hardware
 
 The stack includes a base controller for a differential drive
@@ -19,9 +26,11 @@ 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).
 
 * Robogaia Mega Encoder shield
-(http://www.robogaia.com/two-axis-encoder-counter-mega-shield-version-2.html).
+(http://www.robogaia.com/two-axis-encoder-counter-mega-shield-version-2.html) or on-board wheel encoder counters.
 
-**NOTE:** The Robogaia Mega Encoder shield can only be used with an Arduino Mega.
+**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.
+
+* L298 motor driver
 
 * The library can be easily extended to include support for other motor controllers and encoder hardware or libraries.
 
@@ -46,8 +55,11 @@ or
 
     $ 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.
 
-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.
+**Hardware:**
+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:
 
@@ -61,6 +73,8 @@ The Robogaia Mega Encoder library can be found at:
 
 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
 sketchbook/libraries directory.
 
@@ -104,10 +118,10 @@ and you should see a list of groups you belong to including dialout.
 Installation of the ros\_arduino\_bridge Stack
 ----------------------------------------------
 
-    $ cd ~/ros_workspace
+    $ cd ~/catkin_workspace/src
     $ git clone https://github.com/hbrobotics/ros_arduino_bridge.git
-    $ cd ros_arduino_bridge
+    $ cd ~/catkin_workspace
-    $ rosmake
+    $ catkin_make
 
 The provided Arduino library is called ROSArduinoBridge and is
 located in the ros\_arduino\_firmware package.  This sketch is
@@ -142,21 +156,18 @@ still want to try the code, see the notes at the end of the file.
 
 Choose one of the supported motor controllers by uncommenting its #define statement and commenting out any others.  By default, the Pololu VNH5019 driver is chosen.
 
-Choose a supported encoder library by by uncommenting its #define statement and commenting out any others.  At the moment, only the Robogaia Mega Encoder shield is supported and it is chosen by default.
+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.
 
-If you want to control PWM servos attached to your controller, change
+If you want to control PWM servos attached to your controller, look for the line:
-the two lines that look like this:
-
-<pre>
-//#define USE_SERVOS
-#undef USE_SERVOS
-</pre>
-
-to this:
 
 <pre>
 #define USE_SERVOS
-//#undef USE_SERVOS
+</pre>
+
+and make sure it is not commented out like this:
+
+<pre>
+//#define USE_SERVOS
 </pre>
 
 You must then edit the include file servos.h and change the N_SERVOS
@@ -426,7 +437,7 @@ where pin is the pin number and value is 0 for LOW and 1 for HIGH.
 
     $ rosservice call /arduino/servo_write id pos
 
-where id is the index of the servo as defined in the Arduino sketch (servos.h) and pos is the position in degrees (0 - 180).
+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).
 
 **servo\_read** - read the position of a servo
 
@@ -434,6 +445,43 @@ 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)
 
+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
 -----
 If you do not have the hardware required to run the base controller,
@@ -441,21 +489,19 @@ follow the instructions below so that you can still use your
 Arduino-compatible controller to read sensors and control PWM servos.
 
 First, you need to edit the ROSArduinoBridge sketch. At the top of
-the file, change the two lines that look like this:
+the file comment out the line:
 
 <pre>
 #define USE_BASE
-//#undef USE_BASE
 </pre>
 
-to this:
+so that it looks like this:
 
 <pre>
 //#define USE_BASE
-#undef USE_BASE
 </pre>
 
-**NOTE:** You also need to comment out the line that looks like this in the file encoder_driver.ino:
+**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:
 
     #include "MegaEncoderCounter.h"
 

ros_arduino_bridge/CMakeLists.txt

@@ -0,0 +1,4 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(ros_arduino_bridge)
+find_package(catkin REQUIRED)
+catkin_metapackage()

ros_arduino_bridge/package.xml

@@ -0,0 +1,21 @@
+<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>

ros_arduino_firmware/CMakeLists.txt

@@ -1,30 +1,9 @@
-cmake_minimum_required(VERSION 2.4.6)
+cmake_minimum_required(VERSION 2.8.3)
-include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
+project(ros_arduino_firmware)
 
-# Set the build type.  Options are:
+find_package(catkin REQUIRED)
-#  Coverage       : w/ debug symbols, w/o optimization, w/ code-coverage
+catkin_package(DEPENDS)
-#  Debug          : w/ debug symbols, w/o optimization
-#  Release        : w/o debug symbols, w/ optimization
-#  RelWithDebInfo : w/ debug symbols, w/ optimization
-#  MinSizeRel     : w/o debug symbols, w/ optimization, stripped binaries
-#set(ROS_BUILD_TYPE RelWithDebInfo)
 
-rosbuild_init()
+install(DIRECTORY src
-
+  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})

ros_arduino_firmware/Makefile

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

ros_arduino_firmware/mainpage.dox

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

ros_arduino_firmware/manifest.xml

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

ros_arduino_firmware/package.xml

@@ -0,0 +1,13 @@
+<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>

ros_arduino_firmware/src/libraries/MegaRobogaiaPololu/MegaRobogaiaPololu.ino

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

ros_arduino_firmware/src/libraries/MegaRobogaiaPololu/commands.h

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

ros_arduino_firmware/src/libraries/MegaRobogaiaPololu/diff_controller.h

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

ros_arduino_firmware/src/libraries/MegaRobogaiaPololu/sensors.h

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

ros_arduino_firmware/src/libraries/MegaRobogaiaPololu/servos.h

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

ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino

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

ros_arduino_firmware/src/libraries/ROSArduinoBridge/commands.h

@@ -5,19 +5,14 @@
 #ifndef COMMANDS_H
 #define COMMANDS_H
 
-#define ANALOG_READ    'a'
 #define GET_BAUDRATE   'b'
-#define PIN_MODE       'c'
-#define DIGITAL_READ   'd'
 #define READ_ENCODERS  'e'
 #define MOTOR_SPEEDS   'm'
-#define PING           'p'
 #define RESET_ENCODERS 'r'
-#define SERVO_WRITE    's'
+#define UPDATE_PID_L   'L'
-#define SERVO_READ     't'
+#define UPDATE_PID_R   'R'
-#define UPDATE_PID     'u'
+#define DISP_PID_P     'z'
-#define DIGITAL_WRITE  'w'
+
-#define ANALOG_WRITE   'x'
 #define LEFT            0
 #define RIGHT           1
 

ros_arduino_firmware/src/libraries/ROSArduinoBridge/diff_controller.h

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

ros_arduino_firmware/src/libraries/ROSArduinoBridge/encoder_driver.h

@@ -1,6 +1,8 @@
-/* *************************************************************
+  #define LEFT_ENC_PIN_A PD2  //pin 2
-   Encoder driver function definitions - by James Nugen
+  #define LEFT_ENC_PIN_B PD3  //pin 3
-   ************************************************************ */
+  
+  #define RIGHT_ENC_PIN_A PC4  //pin A4
+  #define RIGHT_ENC_PIN_B PC5   //pin A5
    
 long readEncoder(int i);
 void resetEncoder(int i);

ros_arduino_firmware/src/libraries/ROSArduinoBridge/encoder_driver.ino

@@ -1,41 +1,43 @@
-/* *************************************************************
+  volatile long left_enc_pos = 0L;
-   Encoder definitions
+  volatile long right_enc_pos = 0L;
-   
+  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
-   Add a "#if defined" block to this file to include support for
+    
-   a particular encoder board or library. Then add the appropriate
+  ISR (PCINT2_vect){
-   #define near the top of the main ROSArduinoBridge.ino file.
+  	static uint8_t enc_last=0;
-   
+        
-   ************************************************************ */
+	enc_last <<=2; //shift previous state two places
-   
+	enc_last |= (PIND & (3 << 2)) >> 2; //read the current state into lowest 2 bits
-#ifdef USE_BASE
+  
-
+  	left_enc_pos += ENC_STATES[(enc_last & 0x0f)];
-#if defined ROBOGAIA
+  }
-  /* The Robogaia Mega Encoder shield */
+  
-  #include "MegaEncoderCounter.h"
+  ISR (PCINT1_vect){
-
+        static uint8_t enc_last=0;
-  /* Create the encoder shield object */
+          	
-  MegaEncoderCounter encoders = MegaEncoderCounter(4); // Initializes the Mega Encoder Counter in the 4X Count mode
+	enc_last <<=2; //shift previous state two places
+	enc_last |= (PINC & (3 << 4)) >> 4; //read the current state into lowest 2 bits
+  
+  	right_enc_pos += ENC_STATES[(enc_last & 0x0f)];
+  }
   
-  /* Wrap the encoder reading function */
   long readEncoder(int i) {
-    if (i == LEFT) return encoders.YAxisGetCount();
+    if (i == LEFT) return left_enc_pos;
-    else return encoders.XAxisGetCount();
+    else return right_enc_pos;
   }
 
-  /* Wrap the encoder reset function */
   void resetEncoder(int i) {
-    if (i == LEFT) return encoders.YAxisReset();
+    if (i == LEFT){
-    else return encoders.XAxisReset();
+      left_enc_pos=0L;
+      return;
+    } else { 
+      right_enc_pos=0L;
+      return;
+    }
   }
-#else
-  #error A encoder driver must be selected!
-#endif
 
-/* Wrap the encoder reset function */
 void resetEncoders() {
   resetEncoder(LEFT);
   resetEncoder(RIGHT);
 }
 
-#endif
 

ros_arduino_firmware/src/libraries/ROSArduinoBridge/motor_driver.h

@@ -1,6 +1,9 @@
-/***************************************************************
+  #define RIGHT_MOTOR_BACKWARD 5
-   Motor driver function definitions - by James Nugen
+  #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
 
 void initMotorController();
 void setMotorSpeed(int i, int spd);

ros_arduino_firmware/src/libraries/ROSArduinoBridge/motor_driver.ino

@@ -1,62 +1,32 @@
-/***************************************************************
-   Motor driver definitions
-   
-   Add a "#elif defined" block to this file to include support
-   for a particular motor driver.  Then add the appropriate
-   #define near the top of the main ROSArduinoBridge.ino file.
-   
-   *************************************************************/
-
-#ifdef USE_BASE
-   
-#if defined POLOLU_VNH5019
-  /* Include the Pololu library */
-  #include "DualVNH5019MotorShield.h"
-
-  /* Create the motor driver object */
-  DualVNH5019MotorShield drive;
-  
-  /* Wrap the motor driver initialization */
   void initMotorController() {
-    drive.init();
+    digitalWrite(RIGHT_MOTOR_ENABLE, HIGH);
+    digitalWrite(LEFT_MOTOR_ENABLE, HIGH);
   }
-
+  
-  /* Wrap the drive motor set speed function */
   void setMotorSpeed(int i, int spd) {
-    if (i == LEFT) drive.setM1Speed(spd);
+    unsigned char reverse = 0;
-    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) {
     setMotorSpeed(LEFT, leftSpeed);
     setMotorSpeed(RIGHT, rightSpeed);
   }
-#elif defined POLOLU_MC33926
-  /* Include the Pololu library */
-  #include "DualMC33926MotorShield.h"
-
-  /* Create the motor driver object */
-  DualMC33926MotorShield drive;
-  
-  /* Wrap the motor driver initialization */
-  void initMotorController() {
-    drive.init();
-  }
-
-  /* Wrap the drive motor set speed function */
-  void setMotorSpeed(int i, int spd) {
-    if (i == LEFT) drive.setM1Speed(spd);
-    else drive.setM2Speed(spd);
-  }
-
-  // A convenience function for setting both motor speeds
-  void setMotorSpeeds(int leftSpeed, int rightSpeed) {
-    setMotorSpeed(LEFT, leftSpeed);
-    setMotorSpeed(RIGHT, rightSpeed);
-  }
-#else
-  #error A motor driver must be selected!
-#endif
-
-#endif

ros_arduino_firmware/src/libraries/ROSArduinoBridge/sensors.h

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

ros_arduino_firmware/src/libraries/ROSArduinoBridge/servos.h

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

ros_arduino_msgs/CMakeLists.txt

@@ -1,30 +1,29 @@
-cmake_minimum_required(VERSION 2.4.6)
+cmake_minimum_required(VERSION 2.8.3)
-include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
+project(ros_arduino_msgs)
 
-# Set the build type.  Options are:
+find_package(catkin REQUIRED COMPONENTS std_msgs message_generation)
-#  Coverage       : w/ debug symbols, w/o optimization, w/ code-coverage
-#  Debug          : w/ debug symbols, w/o optimization
-#  Release        : w/o debug symbols, w/ optimization
-#  RelWithDebInfo : w/ debug symbols, w/ optimization
-#  MinSizeRel     : w/o debug symbols, w/ optimization, stripped binaries
-#set(ROS_BUILD_TYPE RelWithDebInfo)
 
-rosbuild_init()
+add_message_files(FILES
+                  AnalogFloat.msg
+                  Analog.msg
+                  ArduinoConstants.msg
+                  Digital.msg
+                  SensorState.msg
+                 )
 
-#set the default path for built executables to the "bin" directory
+add_service_files(FILES
-set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
+                  DigitalSetDirection.srv
-#set the default path for built libraries to the "lib" directory
+                  DigitalWrite.srv
-set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
+                  DigitalRead.srv
+                  ServoRead.srv
+                  ServoWrite.srv
+                  AnalogWrite.srv
+                  AnalogRead.srv
+                 )
 
-#uncomment if you have defined messages
+generate_messages(   
-rosbuild_genmsg()
+	DEPENDENCIES  
-#uncomment if you have defined services
+	std_msgs  
-rosbuild_gensrv()
+)  
 
-#common commands for building c++ executables and libraries
+catkin_package(CATKIN_DEPENDS message_runtime std_msgs)
-#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})

ros_arduino_msgs/Makefile

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

ros_arduino_msgs/mainpage.dox

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

ros_arduino_msgs/manifest.xml

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

ros_arduino_msgs/msg/ArduinoConstants.msg

@@ -0,0 +1,5 @@
+# Arduino-style constants
+uint8 LOW=0
+uint8 HIGH=1
+uint8 INPUT=0
+uint8 OUTPUT=1

ros_arduino_msgs/package.xml

@@ -0,0 +1,20 @@
+<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>

ros_arduino_msgs/srv/AnalogRead.srv

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

ros_arduino_msgs/srv/AnalogWrite.srv

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

ros_arduino_msgs/srv/DigitalRead.srv

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

ros_arduino_msgs/srv/ServoRead.srv

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

ros_arduino_msgs/srv/ServoWrite.srv

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

ros_arduino_python/CMakeLists.txt

@@ -1,30 +1,18 @@
-cmake_minimum_required(VERSION 2.4.6)
+cmake_minimum_required(VERSION 2.8.3)
-include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
+project(ros_arduino_python)
 
-# Set the build type.  Options are:
+find_package(catkin REQUIRED)
-#  Coverage       : w/ debug symbols, w/o optimization, w/ code-coverage
+catkin_package(DEPENDS)
-#  Debug          : w/ debug symbols, w/o optimization
+catkin_python_setup()
-#  Release        : w/o debug symbols, w/ optimization
-#  RelWithDebInfo : w/ debug symbols, w/ optimization
-#  MinSizeRel     : w/o debug symbols, w/ optimization, stripped binaries
-#set(ROS_BUILD_TYPE RelWithDebInfo)
 
-rosbuild_init()
+install(DIRECTORY config
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
 
-#set the default path for built executables to the "bin" directory
+install(DIRECTORY launch
-set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
-#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
+install(DIRECTORY nodes
-#rosbuild_genmsg()
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
-#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})

ros_arduino_python/Makefile

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

ros_arduino_python/config/arduino_params.yaml

@@ -13,6 +13,9 @@ sensorstate_rate: 10
 use_base_controller: False
 base_controller_rate: 10
 
+# For a robot that uses base_footprint, change base_frame to base_footprint
+base_frame: base_link
+
 # === Robot drivetrain parameters
 #wheel_diameter: 0.146
 #wheel_track: 0.2969
@@ -21,10 +24,15 @@ base_controller_rate: 10
 #motors_reversed: True
 
 # === PID parameters
-#Kp: 10
+#lKp: 10
-#Kd: 12
+#lKd: 12
-#Ki: 0
+#lKi: 0
-#Ko: 50
+#lKo: 50
+#rKp: 10
+#rKd: 12
+#rKi: 0
+#rKo: 50
+
 #accel_limit: 1.0
 
 # === Sensor definitions.  Examples only - edit for your robot.

ros_arduino_python/mainpage.dox

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

ros_arduino_python/manifest.xml

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

ros_arduino_python/nodes/arduino_node.py

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

ros_arduino_python/package.xml

@@ -0,0 +1,22 @@
+<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>

ros_arduino_python/setup.py

@@ -0,0 +1,11 @@
+#!/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)

ros_arduino_python/src/ros_arduino_python/arduino_driver.py

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

ros_arduino_python/src/ros_arduino_python/arduino_sensors.py

@@ -39,7 +39,7 @@ class MessageType:
     BOOL = 5
     
 class Sensor(object):
-    def __init__(self, controller, name, pin, rate, frame_id="/base_link", direction="input", **kwargs):
+    def __init__(self, controller, name, pin, rate, frame_id, direction="input", **kwargs):
         self.controller = controller
         self.name = name
         self.pin = pin
@@ -87,7 +87,14 @@ class AnalogSensor(Sensor):
         self.msg = Analog()
         self.msg.header.frame_id = self.frame_id
         
-        self.pub = rospy.Publisher("~sensor/" + self.name, Analog)
+        self.pub = rospy.Publisher("~sensor/" + self.name, Analog, queue_size=5)
+        
+        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)
@@ -104,7 +111,20 @@ class AnalogFloatSensor(Sensor):
         self.msg = AnalogFloat()
         self.msg.header.frame_id = self.frame_id
         
-        self.pub = rospy.Publisher("~sensor/" + self.name, AnalogFloat)
+        self.pub = rospy.Publisher("~sensor/" + self.name, AnalogFloat, queue_size=5)
+        
+        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):
@@ -116,7 +136,7 @@ class DigitalSensor(Sensor):
         self.msg = Digital()
         self.msg.header.frame_id = self.frame_id
         
-        self.pub = rospy.Publisher("~sensor/" + self.name, Digital)
+        self.pub = rospy.Publisher("~sensor/" + self.name, Digital, queue_size=5)
         
         if self.direction == "output":
             self.controller.pin_mode(self.pin, OUTPUT)
@@ -143,7 +163,7 @@ class RangeSensor(Sensor):
         self.msg = Range()
         self.msg.header.frame_id = self.frame_id
         
-        self.pub = rospy.Publisher("~sensor/" + self.name, Range)
+        self.pub = rospy.Publisher("~sensor/" + self.name, Range, queue_size=5)
         
     def read_value(self):
         self.msg.header.stamp = rospy.Time.now()

ros_arduino_python/src/ros_arduino_python/base_controller.py

@@ -31,10 +31,12 @@ from tf.broadcaster import TransformBroadcaster
  
 """ Class to receive Twist commands and publish Odometry data """
 class BaseController:
-    def __init__(self, arduino):
+    def __init__(self, arduino, base_frame, name="base_controllers"):
         self.arduino = arduino
+        self.name = name
+        self.base_frame = base_frame
         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
                  
         pid_params = dict()
@@ -42,11 +44,16 @@ class BaseController:
         pid_params['wheel_track'] = rospy.get_param("~wheel_track", "")
         pid_params['encoder_resolution'] = rospy.get_param("~encoder_resolution", "") 
         pid_params['gear_reduction'] = rospy.get_param("~gear_reduction", 1.0)
-        pid_params['Kp'] = rospy.get_param("~Kp", 20)
+        pid_params['lKp'] = rospy.get_param("~lKp", 20)
-        pid_params['Kd'] = rospy.get_param("~Kd", 12)
+        pid_params['lKd'] = rospy.get_param("~lKd", 12)
-        pid_params['Ki'] = rospy.get_param("~Ki", 0)
+        pid_params['lKi'] = rospy.get_param("~lKi", 0)
-        pid_params['Ko'] = rospy.get_param("~Ko", 50)
+        pid_params['lKo'] = rospy.get_param("~lKo", 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.motors_reversed = rospy.get_param("~motors_reversed", False)
         
@@ -67,7 +74,7 @@ class BaseController:
         self.t_delta = rospy.Duration(1.0 / self.rate)
         self.t_next = now + self.t_delta
 
-        # internal data        
+        # Internal data        
         self.enc_left = None            # encoder readings
         self.enc_right = None
         self.x = 0                      # position in xy plane
@@ -79,18 +86,18 @@ class BaseController:
         self.v_des_right = 0
         self.last_cmd_vel = now
 
-        # subscriptions
+        # Subscriptions
         rospy.Subscriber("cmd_vel", Twist, self.cmdVelCallback)
         
         # Clear any old odometry info
         self.arduino.reset_encoders()
         
         # Set up the odometry broadcaster
-        self.odomPub = rospy.Publisher('odom', Odometry)
+        self.odomPub = rospy.Publisher('odom', Odometry, queue_size=5)
         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("Publishing odometry data at: " + str(self.rate) + " Hz")
+        rospy.loginfo("Publishing odometry data at: " + str(self.rate) + " Hz using " + str(self.base_frame) + " as base frame")
         
     def setup_pid(self, pid_params):
         # Check to see if any PID parameters are missing
@@ -108,12 +115,17 @@ class BaseController:
         self.encoder_resolution = pid_params['encoder_resolution']
         self.gear_reduction = pid_params['gear_reduction']
         
-        self.Kp = pid_params['Kp']
+        self.lKp = pid_params['lKp']
-        self.Kd = pid_params['Kd']
+        self.lKd = pid_params['lKd']
-        self.Ki = pid_params['Ki']
+        self.lKi = pid_params['lKi']
-        self.Ko = pid_params['Ko']
+        self.lKo = pid_params['lKo']
         
-        self.arduino.update_pid(self.Kp, self.Kd, self.Ki, self.Ko)
+        self.rKp = pid_params['rKp']
+        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):
         now = rospy.Time.now()
@@ -130,7 +142,7 @@ class BaseController:
             self.then = now
             dt = dt.to_sec()
             
-            # calculate odometry
+            # Calculate odometry
             if self.enc_left == None:
                 dright = 0
                 dleft = 0
@@ -166,13 +178,17 @@ class BaseController:
                 (self.x, self.y, 0), 
                 (quaternion.x, quaternion.y, quaternion.z, quaternion.w),
                 rospy.Time.now(),
-                "base_link",
+                self.base_frame,
                 "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.header.frame_id = "odom"
-            odom.child_frame_id = "base_link"
+            odom.child_frame_id = self.base_frame
             odom.header.stamp = now
             odom.pose.pose.position.x = self.x
             odom.pose.pose.position.y = self.y

stack.xml

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