Added ROSArduinoBridge sketch which breaks out the motor controller and encoder functions

ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino

@@ -0,0 +1,325 @@
+/*********************************************************************
+ *  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 the motor controller and encoder library you are using */
+#ifdef USE_BASE
+   /* The Pololu VNH5019 dual motor driver shield */
+   #define POLOLU_VNH5019
+
+   /* The RoboGaia encoder shield */
+   #define ROBOGAIA
+#endif
+
+//#define USE_SERVOS  // Enable use of PWM servos as defined in servos.h
+#undef USE_SERVOS     // Disable use of PWM servos
+
+/* Serial port baud rate */
+#define BAUDRATE     57600
+
+/* Maximum PWM signal */
+#define MAX_PWM        255
+
+#if defined(ARDUINO) && ARDUINO >= 100
+#include "Arduino.h"
+#else
+#include "WProgram.h"
+#endif
+
+/* Include definition of serial commands */
+#include "commands.h"
+
+/* Sensor functions */
+#include "sensors.h"
+
+/* Include servo support if required */
+#ifdef USE_SERVOS
+   #include <Servo.h>
+   #include "servos.h"
+#endif
+
+#ifdef USE_BASE
+  #ifdef POLOLU_VNH5019
+    /* Include the Pololu library */
+    #include "DualVNH5019MotorShield.h"
+  #endif
+  
+  /* Include the motor driver functions */
+  #include "motor_driver.h"
+  
+  #ifdef ROBOGAIA
+    /* The Robogaia Mega Encoder shield */
+    #include "MegaEncoderCounter.h"
+  #endif
+
+  /* Include the encoder functions */
+  #include "encoders.h"
+  
+  /* Wrap the encoder reset function */
+  void resetEncoders() {
+    resetEncoder(LEFT);
+    resetEncoder(RIGHT);
+  }
+  
+  /* PID parameters and functions */
+  #include "diff_controller.h"
+
+  /* Run the PID loop at 30 times per second */
+  #define PID_RATE           30     // Hz
+
+  /* Convert the rate into an interval */
+  const int PID_INTERVAL = 1000 / PID_RATE;
+  
+  /* Track the next time we make a PID calculation */
+  unsigned long nextPID = PID_INTERVAL;
+
+  /* Stop the robot if it hasn't received a movement command
+   in this number of milliseconds */
+  #define AUTO_STOP_INTERVAL 2000
+  long lastMotorCommand = AUTO_STOP_INTERVAL;
+#endif
+
+/* Variable initialization */
+
+// A pair of varibles to help parse serial commands (thanks Fergs)
+int arg = 0;
+int index = 0;
+
+// Variable to hold an input character
+char chr;
+
+// Variable to hold the current single-character command
+char cmd;
+
+// Character arrays to hold the first and second arguments
+char argv1[16];
+char argv2[16];
+
+// The arguments converted to integers
+long arg1;
+long arg2;
+
+/* Clear the current command parameters */
+void resetCommand() {
+  cmd = NULL;
+  memset(argv1, 0, sizeof(argv1));
+  memset(argv2, 0, sizeof(argv2));
+  arg1 = 0;
+  arg2 = 0;
+  arg = 0;
+  index = 0;
+}
+
+/* Run a command.  Commands are defined in commands.h */
+int runCommand() {
+  int i = 0;
+  char *p = argv1;
+  char *str;
+  int pid_args[4];
+  arg1 = atoi(argv1);
+  arg2 = atoi(argv2);
+  
+  switch(cmd) {
+  case GET_BAUDRATE:
+    Serial.println(BAUDRATE);
+    break;
+  case ANALOG_READ:
+    Serial.println(analogRead(arg1));
+    break;
+  case DIGITAL_READ:
+    Serial.println(digitalRead(arg1));
+    break;
+  case ANALOG_WRITE:
+    analogWrite(arg1, arg2);
+    Serial.println("OK"); 
+    break;
+  case DIGITAL_WRITE:
+    if (arg2 == 0) digitalWrite(arg1, LOW);
+    else if (arg2 == 1) digitalWrite(arg1, HIGH);
+    Serial.println("OK"); 
+    break;
+  case PIN_MODE:
+    if (arg2 == 0) pinMode(arg1, INPUT);
+    else if (arg2 == 1) pinMode(arg1, OUTPUT);
+    Serial.println("OK");
+    break;
+  case PING:
+    Serial.println(Ping(arg1));
+    break;
+#ifdef USE_SERVOS
+  case SERVO_WRITE:
+    servos[arg1].write(arg2);
+    Serial.println("OK");
+    break;
+  case SERVO_READ:
+    Serial.println(servos[arg1].read());
+    break;
+#endif
+    
+#ifdef USE_BASE
+  case READ_ENCODERS:
+    Serial.print(readEncoder(LEFT));
+    Serial.print(" ");
+    Serial.println(readEncoder(RIGHT));
+    break;
+   case RESET_ENCODERS:
+    resetEncoders();
+    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/ROSArduinoBridge/commands.h

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

ros_arduino_firmware/src/libraries/ROSArduinoBridge/diff_controller.h

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

ros_arduino_firmware/src/libraries/ROSArduinoBridge/encoders.h

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

ros_arduino_firmware/src/libraries/ROSArduinoBridge/motor_driver.h

@@ -0,0 +1,32 @@
+/***************************************************************
+   Motor driver definitions
+   
+   Add a #ifdef 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 POLOLU_VNH5019
+  /* Create the motor driver object */
+  DualVNH5019MotorShield drive;
+  
+  /* Wrap the motor driver initialization */
+  void initMotorController() {
+    drive.init();
+  }
+
+  /* Wrap the drive motor set speed function */
+  void setMotorSpeed(int i, int spd) {
+    if (i == LEFT) drive.setM1Speed(spd);
+    else drive.setM2Speed(spd);
+  }
+
+  // A convenience function for setting both motor speeds
+  void setMotorSpeeds(int leftSpeed, int rightSpeed) {
+    setMotorSpeed(LEFT, leftSpeed);
+    setMotorSpeed(RIGHT, rightSpeed);
+  }
+#endif
+
+

ros_arduino_firmware/src/libraries/ROSArduinoBridge/sensors.h

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

ros_arduino_firmware/src/libraries/ROSArduinoBridge/servos.h

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