From 6adb1bde269aafde3489e47ba55e3a0d5ff5526b Mon Sep 17 00:00:00 2001 From: Patrick Goebel Date: Mon, 4 Apr 2016 08:45:51 -0700 Subject: [PATCH] Added preliminary support for Adafruit 9-DOF IMU to firmware but not yet to ROS nodes --- .../ROSArduinoBridge/ROSArduinoBridge.ino | 88 +++++++++++++++---- .../src/libraries/ROSArduinoBridge/commands.h | 1 + .../src/libraries/ROSArduinoBridge/imu.h | 26 ++++++ .../src/libraries/ROSArduinoBridge/imu.ino | 68 ++++++++++++++ 4 files changed, 165 insertions(+), 18 deletions(-) create mode 100644 ros_arduino_firmware/src/libraries/ROSArduinoBridge/imu.h create mode 100644 ros_arduino_firmware/src/libraries/ROSArduinoBridge/imu.ino diff --git a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino index dbe1db4..046cf1e 100644 --- a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino +++ b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino @@ -45,30 +45,41 @@ * 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 - /* The Pololu VNH5019 dual motor driver shield */ - //#define POLOLU_VNH5019 + /* The Pololu VNH5019 dual motor driver shield */ + //#define POLOLU_VNH5019 - /* The Pololu MC33926 dual motor driver shield */ - //#define POLOLU_MC33926 + /* The Pololu MC33926 dual motor driver shield */ + //#define POLOLU_MC33926 - /* The Ardunino Motor Shield R3 */ - //#define ARDUINO_MOTOR_SHIELD_R3 + /* The Adafruit motor shield V2 */ + #define ADAFRUIT_MOTOR_SHIELD_V2 - /* For testing only */ - //#define NO_MOTOR_CONTROLLER - - /* The RoboGaia encoder shield */ - //#define ROBOGAIA - - /* The RoboGaia 3-axis encoder shield */ - //#define ROBOGAIA_3_AXIS - - /* Encoders directly attached to Arduino board */ - //#define ARDUINO_ENC_COUNTER + /* 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 + + /* The RoboGaia encoder shield */ + //#define ROBOGAIA + + /* The RoboGaia 3-axis encoder shield */ + #define ROBOGAIA_3_AXIS + + /* Encoders directly attached to Arduino board */ + //#define ARDUINO_ENC_COUNTER #endif //#define USE_SERVOS // Enable/disable use of old PWM servo support as defined in servos.h @@ -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; diff --git a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/commands.h b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/commands.h index 402f11c..026c388 100644 --- a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/commands.h +++ b/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' diff --git a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/imu.h b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/imu.h new file mode 100644 index 0000000..64c5f9f --- /dev/null +++ b/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 diff --git a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/imu.ino b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/imu.ino new file mode 100644 index 0000000..fdc3a4d --- /dev/null +++ b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/imu.ino @@ -0,0 +1,68 @@ +#ifdef USE_IMU + + #ifdef ADAFRUIT_9DOF + + #include + #include + #include + #include + #include + + /* 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