Added preliminary support for Adafruit 9-DOF IMU to firmware but not yet to ROS nodes

ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino

@@ -45,7 +45,9 @@
  *  POSSIBILITY OF SUCH DAMAGE.
  *********************************************************************/
 
-//#define USE_BASE      // Enable/disable the base controller code
+#define USE_BASE      // Enable/disable the base controller code
+
+//#define USE_IMU       // Enable/disable use of an IMU
 
 /* Define the motor controller and encoder library you are using */
 #ifdef USE_BASE
@@ -55,17 +57,26 @@
   /* The Pololu MC33926 dual motor driver shield */
   //#define POLOLU_MC33926
 
+  /* The Adafruit motor shield V2 */
+  #define ADAFRUIT_MOTOR_SHIELD_V2
+
   /* The Ardunino Motor Shield R3 */
   //#define ARDUINO_MOTOR_SHIELD_R3
   
+  /* The brake uses digital pins 8 and 9 and is not compatible with the Robogaia 3-axis
+  *  endcoder shield.  Cut the brake jumpers on the R3 motor shield if you want to use
+  *  it with the 3-axis encoder shield.
+  */
+  //#define USE_ARDUINO_MOTOR_SHIELD_R3_BRAKE
+  
   /* For testing only */
-   //#define NO_MOTOR_CONTROLLER
+  // #define NO_MOTOR_CONTROLLER
   
   /* The RoboGaia encoder shield */
   //#define ROBOGAIA
   
   /* The RoboGaia 3-axis encoder shield */
-   //#define ROBOGAIA_3_AXIS
+  #define ROBOGAIA_3_AXIS
   
   /* Encoders directly attached to Arduino board */
   //#define ARDUINO_ENC_COUNTER
@@ -129,6 +140,13 @@
   long lastMotorCommand = AUTO_STOP_INTERVAL;
 #endif
 
+#ifdef USE_IMU
+  #include "imu.h"
+
+  #define ADAFRUIT_9DOF
+
+#endif
+
 /* Variable initialization */
 #define BAUDRATE     57600
 
@@ -169,9 +187,11 @@ int runCommand() {
   int pid_args[4];
   arg1 = atoi(argv1);
   arg2 = atoi(argv2);
+  String output;
 
   switch(cmd) {
   case GET_BAUDRATE:
+    Serial.print(" ");
     Serial.println(BAUDRATE);
     break;
   case ANALOG_READ:
@@ -197,6 +217,34 @@ int runCommand() {
   case PING:
     Serial.println(Ping(arg1));
     break;
+#ifdef USE_IMU
+  case READ_IMU:
+    imu_data = readIMU();
+    Serial.print(imu_data.ax);
+    Serial.print(" ");
+    Serial.print(imu_data.ay);
+    Serial.print(" ");
+    Serial.print(imu_data.az);
+    Serial.print(" ");
+    Serial.print(imu_data.gx);
+    Serial.print(" ");
+    Serial.print(imu_data.gy);
+    Serial.print(" ");
+    Serial.print(imu_data.gz);
+    Serial.print(" ");
+    Serial.print(imu_data.mx);
+    Serial.print(" ");
+    Serial.print(imu_data.my);
+    Serial.print(" ");
+    Serial.print(imu_data.mz);
+    Serial.print(" ");
+    Serial.print(imu_data.roll);
+    Serial.print(" ");
+    Serial.print(imu_data.pitch);
+    Serial.print(" ");
+    Serial.println(imu_data.uh);
+    break;
+#endif
 #ifdef USE_SERVOS
   case SERVO_WRITE:
     servos[arg1].setTargetPosition(arg2);
@@ -302,6 +350,10 @@ void setup() {
     resetPID();
   #endif
 
+  #ifdef USE_IMU
+    initIMU();
+  #endif
+
   /* Attach servos if used */
   #ifdef USE_SERVOS
     int i;

ros_arduino_firmware/src/libraries/ROSArduinoBridge/commands.h

@@ -10,6 +10,7 @@
 #define PIN_MODE       'c'
 #define DIGITAL_READ   'd'
 #define READ_ENCODERS  'e'
+#define READ_IMU       'i'
 #define CONFIG_SERVO   'j'
 #define MOTOR_SPEEDS   'm'
 #define PING           'p'

ros_arduino_firmware/src/libraries/ROSArduinoBridge/imu.h

@@ -0,0 +1,26 @@
+/* Provide a unified interface to IMU devices */
+
+#ifndef IMU_H
+#define IMU_H
+
+typedef struct imuData_s
+    {
+      float ax = -999;
+      float ay = -999;
+      float az = -999;
+      float gx = -999;
+      float gy = -999;
+      float gz = -999;
+      float mx = -999;
+      float my = -999;
+      float mz = -999;
+      float roll = -999;
+      float pitch = -999;
+      float uh = -999;
+    } imuData;
+
+    void initIMU();
+    imuData readIMU();
+    imuData imu_data;
+
+#endif

ros_arduino_firmware/src/libraries/ROSArduinoBridge/imu.ino

@@ -0,0 +1,68 @@
+#ifdef USE_IMU
+
+  #ifdef ADAFRUIT_9DOF
+  
+    #include <Wire.h>
+    #include <Adafruit_Sensor.h>
+    #include <Adafruit_LSM303_U.h>
+    #include <Adafruit_L3GD20_U.h>
+    #include <Adafruit_9DOF.h>
+    
+    /* Assign a unique ID to the sensors */
+    Adafruit_9DOF                 dof   = Adafruit_9DOF();
+    Adafruit_LSM303_Accel_Unified accel = Adafruit_LSM303_Accel_Unified(30301);
+    Adafruit_LSM303_Mag_Unified   mag   = Adafruit_LSM303_Mag_Unified(30302);
+    
+    /* Update this with the correct SLP for accurate altitude measurements */
+    float seaLevelPressure = SENSORS_PRESSURE_SEALEVELHPA;
+    
+    void initIMU()
+    {
+      if(!accel.begin())
+      {
+        /* There was a problem detecting the LSM303 ... check your connections */
+        Serial.println(F("Ooops, no LSM303 detected ... Check your wiring!"));
+        while(1);
+      }
+      if(!mag.begin())
+      {
+        /* There was a problem detecting the LSM303 ... check your connections */
+        Serial.println("Ooops, no LSM303 detected ... Check your wiring!");
+        while(1);
+      }
+    }
+  
+    imuData readIMU() {
+      imuData_s data;
+    
+      sensors_event_t accel_event;
+      sensors_event_t mag_event;
+      sensors_vec_t   orientation;
+      
+      /* Calculate pitch and roll from the raw accelerometer data */
+      accel.getEvent(&accel_event);
+      data.ax = accel_event.acceleration.x;
+      data.ay = accel_event.acceleration.y;
+      data.az = accel_event.acceleration.z;
+
+      if (dof.accelGetOrientation(&accel_event, &orientation))
+      {
+         /* 'orientation' should have valid .roll and .pitch fields */
+        data.roll = orientation.roll;
+        data.pitch = orientation.pitch;
+      }
+      
+      /* Calculate the heading using the magnetometer */
+      mag.getEvent(&mag_event);
+      if (dof.magGetOrientation(SENSOR_AXIS_Z, &mag_event, &orientation))
+      {
+        /* 'orientation' should have valid .heading data now */
+        data.mz = orientation.heading;
+      }
+      
+      return data;
+    }
+  
+  #endif
+
+#endif