yikestone/ros_arduino_bridge
1
0
Fork 0
mirror of https://github.com/YikeStone/ros_arduino_bridge.git synced 2025-08-03 19:24:09 +05:30
Code Issues Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
Patrick Goebel 2016-04-04 08:45:51 -07:00
parent 77a5a9a4d5
commit 6adb1bde26
4 changed files with 165 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

88
ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino
View File

@ -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;

1
ros_arduino_firmware/src/libraries/ROSArduinoBridge/commands.h
View File

@ -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'

26
ros_arduino_firmware/src/libraries/ROSArduinoBridge/imu.h Normal file
View File

@ -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

68
ros_arduino_firmware/src/libraries/ROSArduinoBridge/imu.ino Normal file
View File

@ -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