Revision 0:7b3acf8e2a6f, committed 2013-07-05

Comitter:

YSB

Date:

Fri Jul 05 04:16:49 2013 +0000

Commit message:

ADXL345/ITG3200

Changed in this revision

diff -r 000000000000 -r 7b3acf8e2a6f IMU_I2C.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/IMU_I2C.cpp	Fri Jul 05 04:16:49 2013 +0000
@@ -0,0 +1,607 @@
+#include "IMU_I2C.h"
+#include <math.h>
+//#include "mbed.h"
+
+#define GYR_ADDRESS 0xD2
+
+IMU_I2C::IMU_I2C(PinName sda, PinName scl) : i2c_(sda, scl) {
+
+///////////////////ADXL345////////////////////////////////////////////////////
+    //400kHz, allowing us to use the fastest data rates.
+    i2c_.frequency(400000);   
+    // initialize the BW data rate
+    char tx[2];
+    tx[0] = ADXL345_BW_RATE_REG;
+    tx[1] = ADXL345_1600HZ; //value greater than or equal to 0x0A is written into the rate bits (Bit D3 through Bit D0) in the BW_RATE register 
+     i2c_.write( ADXL345_I2C_WRITE , tx, 2);  
+
+    //Data format (for +-16g) - This is done by setting Bit D3 of the DATA_FORMAT register (Address 0x31) and writing a value of 0x03 to the range bits (Bit D1 and Bit D0) of the DATA_FORMAT register (Address 0x31).
+   
+    char rx[2];
+    rx[0] = ADXL345_DATA_FORMAT_REG;
+    rx[1] = 0x0B; 
+    // full res and +_16g
+    i2c_.write( ADXL345_I2C_WRITE , rx, 2); 
+ 
+    // Set Offset  - programmed into the OFSX, OFSY, and OFXZ registers, respectively, as 0xFD, 0x03 and 0xFE.
+    char x[2];
+    x[0] = ADXL345_OFSX_REG ;
+    x[1] = 0xFD; 
+    i2c_.write( ADXL345_I2C_WRITE , x, 2);
+    char y[2];
+    y[0] = ADXL345_OFSY_REG ;
+    y[1] = 0x03; 
+    i2c_.write( ADXL345_I2C_WRITE , y, 2);
+    char z[2];
+    z[0] = ADXL345_OFSZ_REG ;
+    z[1] = 0xFE; 
+    i2c_.write( ADXL345_I2C_WRITE , z, 2);
+    
+//////////////////L3G4200D//////////////////////////////////////////////
+    // Turns on the L3G4200D's gyro and places it in normal mode.
+    // 0x0F = 0b00001111
+    // Normal power mode, all axes enabled
+    writeReg(L3G4200D_CTRL_REG1, 0x0F);
+    writeReg(L3G4200D_CTRL_REG4, 0x20); // 2000 dps full scale_device.frequency(400000);
+    // Turns on the L3G4200D's gyro and places it in normal mode.
+    // 0x0F = 0b00001111
+    // Normal power mode, all axes enabled
+    writeReg(L3G4200D_CTRL_REG1, 0x0F);
+    writeReg(L3G4200D_CTRL_REG4, 0x20); // 2000 dps full scale
+    
+/////////////////ITG3200//////////////////////////////////////////////////
+    //Set FS_SEL to 0x03 for proper operation.
+    //See datasheet for details.
+    char gtx[2];
+    gtx[0] = DLPF_FS_REG;
+    //FS_SEL bits sit in bits 4 and 3 of DLPF_FS register.
+    gtx[1] = 0x03 << 3;  
+    i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, gtx, 2);
+    
+}
+
+
+char IMU_I2C::SingleByteRead(char address){   
+   char tx = address;
+   char output; 
+    i2c_.write( ADXL345_I2C_WRITE , &tx, 1);  //tell it what you want to read
+    i2c_.read( ADXL345_I2C_READ , &output, 1);    //tell it where to store the data
+    return output;
+  
+}
+
+int IMU_I2C::SingleByteWrite(char address, char data){ 
+   int ack = 0;
+   char tx[2];
+   tx[0] = address;
+   tx[1] = data;
+   return   ack | i2c_.write( ADXL345_I2C_WRITE , tx, 2);   
+}
+
+void IMU_I2C::multiByteRead(char address, char* output, int size) {
+    i2c_.write( ADXL345_I2C_WRITE, &address, 1);  //tell it where to read from
+    i2c_.read( ADXL345_I2C_READ , output, size);      //tell it where to store the data read
+}
+
+int IMU_I2C::multiByteWrite(char address, char* ptr_data, int size) {
+        int ack;
+   
+               ack = i2c_.write( ADXL345_I2C_WRITE, &address, 1);  //tell it where to write to
+        return ack | i2c_.write( ADXL345_I2C_READ, ptr_data, size);  //tell it what data to write
+                                    
+}
+
+void IMU_I2C::getOutput(int* readings){
+    char buffer[6];    
+    multiByteRead(ADXL345_DATAX0_REG, buffer, 6);
+    
+    readings[0] = (int)buffer[1] << 8 | (int)buffer[0];
+    readings[1] = (int)buffer[3] << 8 | (int)buffer[2];
+    readings[2] = (int)buffer[5] << 8 | (int)buffer[4];
+
+}
+
+char IMU_I2C::getDeviceID() {  
+    return SingleByteRead(ADXL345_DEVID_REG);
+    }
+
+int IMU_I2C::setPowerMode(char mode) { 
+
+    //Get the current register contents, so we don't clobber the rate value.
+    char registerContents = (mode << 4) | SingleByteRead(ADXL345_BW_RATE_REG);
+
+   return SingleByteWrite(ADXL345_BW_RATE_REG, registerContents);
+
+}
+
+char IMU_I2C::getPowerControl() {    
+    return SingleByteRead(ADXL345_POWER_CTL_REG);
+}
+
+int IMU_I2C::setPowerControl(char settings) {    
+    return SingleByteWrite(ADXL345_POWER_CTL_REG, settings);
+
+}
+
+char IMU_I2C::getDataFormatControl(void){
+
+    return SingleByteRead(ADXL345_DATA_FORMAT_REG);
+}
+
+int IMU_I2C::setDataFormatControl(char settings){
+
+   return SingleByteWrite(ADXL345_DATA_FORMAT_REG, settings);
+    
+}
+
+int IMU_I2C::setDataRate(char rate) {
+
+    //Get the current register contents, so we don't clobber the power bit.
+    char registerContents = SingleByteRead(ADXL345_BW_RATE_REG);
+
+    registerContents &= 0x10;
+    registerContents |= rate;
+
+    return SingleByteWrite(ADXL345_BW_RATE_REG, registerContents);
+}
+
+char IMU_I2C::getOffset(char axis) {     
+
+    char address = 0;
+
+    if (axis == ADXL345_X) {
+        address = ADXL345_OFSX_REG;
+    } else if (axis == ADXL345_Y) {
+        address = ADXL345_OFSY_REG;
+    } else if (axis == ADXL345_Z) {
+        address = ADXL345_OFSZ_REG;
+    }
+
+   return SingleByteRead(address);
+}
+
+int IMU_I2C::setOffset(char axis, char offset) {        
+
+    char address = 0;
+
+    if (axis == ADXL345_X) {
+        address = ADXL345_OFSX_REG;
+    } else if (axis == ADXL345_Y) {
+        address = ADXL345_OFSY_REG;
+    } else if (axis == ADXL345_Z) {
+        address = ADXL345_OFSZ_REG;
+    }
+   return SingleByteWrite(address, offset);
+}
+
+char IMU_I2C::getFifoControl(void){
+
+    return SingleByteRead(ADXL345_FIFO_CTL);
+}
+
+int IMU_I2C::setFifoControl(char settings){
+   return SingleByteWrite(ADXL345_FIFO_STATUS, settings);
+
+}
+
+char IMU_I2C::getFifoStatus(void){
+    return SingleByteRead(ADXL345_FIFO_STATUS);
+}
+
+char IMU_I2C::getTapThreshold(void) {
+
+    return SingleByteRead(ADXL345_THRESH_TAP_REG);
+}
+
+int IMU_I2C::setTapThreshold(char threshold) {   
+
+   return SingleByteWrite(ADXL345_THRESH_TAP_REG, threshold);
+}
+
+float IMU_I2C::getTapDuration(void) {     
+
+    return (float)SingleByteRead(ADXL345_DUR_REG)*625;
+}
+
+int IMU_I2C::setTapDuration(short int duration_us) {
+
+    short int tapDuration = duration_us / 625;
+    char tapChar[2];
+     tapChar[0] = (tapDuration & 0x00FF);
+     tapChar[1] = (tapDuration >> 8) & 0x00FF;
+    return multiByteWrite(ADXL345_DUR_REG, tapChar, 2);
+}
+
+float IMU_I2C::getTapLatency(void) {
+
+    return (float)SingleByteRead(ADXL345_LATENT_REG)*1.25;
+}
+
+int IMU_I2C::setTapLatency(short int latency_ms) {
+
+    latency_ms = latency_ms / 1.25;
+    char latChar[2];
+     latChar[0] = (latency_ms & 0x00FF);
+     latChar[1] = (latency_ms << 8) & 0xFF00;
+    return multiByteWrite(ADXL345_LATENT_REG, latChar, 2);
+}
+
+float IMU_I2C::getWindowTime(void) {
+
+    return (float)SingleByteRead(ADXL345_WINDOW_REG)*1.25;
+}
+
+int IMU_I2C::setWindowTime(short int window_ms) {
+
+    window_ms = window_ms / 1.25;
+    char windowChar[2];
+    windowChar[0] = (window_ms & 0x00FF);
+    windowChar[1] = ((window_ms << 8) & 0xFF00);
+   return multiByteWrite(ADXL345_WINDOW_REG, windowChar, 2);
+}
+
+char IMU_I2C::getActivityThreshold(void) {
+
+    return SingleByteRead(ADXL345_THRESH_ACT_REG);
+}
+
+int IMU_I2C::setActivityThreshold(char threshold) {
+    return SingleByteWrite(ADXL345_THRESH_ACT_REG, threshold);
+}
+
+char IMU_I2C::getInactivityThreshold(void) {
+    return SingleByteRead(ADXL345_THRESH_INACT_REG);      
+}
+
+//int FUNCTION(short int * ptr_Output)
+//short int FUNCTION ()
+
+int IMU_I2C::setInactivityThreshold(char threshold) {
+    return SingleByteWrite(ADXL345_THRESH_INACT_REG, threshold);
+}
+
+char IMU_I2C::getTimeInactivity(void) {
+
+    return SingleByteRead(ADXL345_TIME_INACT_REG);
+}
+
+int IMU_I2C::setTimeInactivity(char timeInactivity) {
+    return SingleByteWrite(ADXL345_TIME_INACT_REG, timeInactivity);
+}
+
+char IMU_I2C::getActivityInactivityControl(void) {
+
+    return SingleByteRead(ADXL345_ACT_INACT_CTL_REG);
+}
+
+int IMU_I2C::setActivityInactivityControl(char settings) {
+    return SingleByteWrite(ADXL345_ACT_INACT_CTL_REG, settings);   
+}
+
+char IMU_I2C::getFreefallThreshold(void) {
+    return SingleByteRead(ADXL345_THRESH_FF_REG);
+}
+
+int IMU_I2C::setFreefallThreshold(char threshold) {
+   return SingleByteWrite(ADXL345_THRESH_FF_REG, threshold);
+}
+
+char IMU_I2C::getFreefallTime(void) {
+    return SingleByteRead(ADXL345_TIME_FF_REG)*5;
+}
+
+int IMU_I2C::setFreefallTime(short int freefallTime_ms) {
+     freefallTime_ms = freefallTime_ms / 5;
+     char fallChar[2];
+     fallChar[0] = (freefallTime_ms & 0x00FF);
+     fallChar[1] = (freefallTime_ms << 8) & 0xFF00;
+    
+    return multiByteWrite(ADXL345_TIME_FF_REG, fallChar, 2);
+}
+
+char IMU_I2C::getTapAxisControl(void) {
+    return SingleByteRead(ADXL345_TAP_AXES_REG);
+}
+
+int IMU_I2C::setTapAxisControl(char settings) {
+   return SingleByteWrite(ADXL345_TAP_AXES_REG, settings);
+}
+
+char IMU_I2C::getTapSource(void) {
+    return SingleByteRead(ADXL345_ACT_TAP_STATUS_REG);
+}
+
+char IMU_I2C::getInterruptEnableControl(void) {
+    return SingleByteRead(ADXL345_INT_ENABLE_REG);
+}
+
+int IMU_I2C::setInterruptEnableControl(char settings) {
+   return SingleByteWrite(ADXL345_INT_ENABLE_REG, settings);
+}
+
+char IMU_I2C::getInterruptMappingControl(void) {
+    return SingleByteRead(ADXL345_INT_MAP_REG);
+}
+
+int IMU_I2C::setInterruptMappingControl(char settings) {
+    return SingleByteWrite(ADXL345_INT_MAP_REG, settings);
+}
+
+char IMU_I2C::getInterruptSource(void){
+    return SingleByteRead(ADXL345_INT_SOURCE_REG);
+}
+
+//////////////L3G4200D//////////////////////////////////////////////////////////
+
+// Writes a gyro register
+void IMU_I2C::writeReg(byte reg, byte value)
+{
+    data[0] = reg;
+    data[1] = value;
+    
+    i2c_.write(GYR_ADDRESS, data, 2);
+}// Writes a gyro register
+
+// Reads a gyro register
+byte IMU_I2C::readReg(byte reg)
+{
+    byte value = 0;
+    
+    i2c_.write(GYR_ADDRESS, &reg, 1);
+    i2c_.read(GYR_ADDRESS, &value, 1);
+
+    return value;
+}// Reads a gyro register
+
+// Reads the 3 gyro channels and stores them in vector g
+void IMU_I2C::read(int g[3])
+{
+    // assert the MSB of the address to get the gyro 
+    // to do slave-transmit subaddress updating.
+    data[0] = L3G4200D_OUT_X_L | (1 << 7);
+    i2c_.write(GYR_ADDRESS, data, 1); 
+
+//    Wire.requestFrom(GYR_ADDRESS, 6);
+//    while (Wire.available() < 6);
+    
+    i2c_.read(GYR_ADDRESS, data, 6); 
+
+    uint8_t xla = data[0];
+    uint8_t xha = data[1];
+    uint8_t yla = data[2];
+    uint8_t yha = data[3];
+    uint8_t zla = data[4];
+    uint8_t zha = data[5];
+
+    g[0] = (short) (xha << 8 | xla);
+    g[1] = (short) (yha << 8 | yla);
+    g[2] = (short) (zha << 8 | zla);
+}
+
+char IMU_I2C::getWhoAmI(void){
+
+    //WhoAmI Register address.
+    char tx = WHO_AM_I_REG;
+    char rx;
+    
+    i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, &tx, 1);
+    
+    i2c_.read((ITG3200_I2C_ADDRESS << 1) | 0x01, &rx, 1);
+    
+    return rx;
+
+}
+
+void IMU_I2C::setWhoAmI(char address){
+
+    char tx[2];
+    tx[0] = WHO_AM_I_REG;
+    tx[1] = address;
+    
+    i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, tx, 2);
+
+}
+
+char IMU_I2C::getSampleRateDivider(void){
+
+    char tx = SMPLRT_DIV_REG;
+    char rx;
+    
+    i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, &tx, 1);
+    
+    i2c_.read((ITG3200_I2C_ADDRESS << 1) | 0x01, &rx, 1);
+
+    return rx;
+
+}
+
+void IMU_I2C::setSampleRateDivider(char divider){
+
+    char tx[2];
+    tx[0] = SMPLRT_DIV_REG;
+    tx[1] = divider;
+
+    i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, tx, 2);
+
+}
+
+int IMU_I2C::getInternalSampleRate(void){
+
+    char tx = DLPF_FS_REG;
+    char rx;
+    
+    i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, &tx, 1);
+    
+    i2c_.read((ITG3200_I2C_ADDRESS << 1) | 0x01, &rx, 1);
+    
+    //DLPF_CFG == 0 -> sample rate = 8kHz.
+    if(rx == 0){
+        return 8;
+    } 
+    //DLPF_CFG = 1..7 -> sample rate = 1kHz.
+    else if(rx >= 1 && rx <= 7){
+        return 1;
+    }
+    //DLPF_CFG = anything else -> something's wrong!
+    else{
+        return -1;
+    }
+    
+}
+
+void IMU_I2C::setLpBandwidth(char bandwidth){
+
+    char tx[2];
+    tx[0] = DLPF_FS_REG;
+    //Bits 4,3 are required to be 0x03 for proper operation.
+    tx[1] = bandwidth | (0x03 << 3);
+    
+    i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, tx, 2);
+
+}
+
+char IMU_I2C::getInterruptConfiguration(void){
+
+    char tx = INT_CFG_REG;
+    char rx;
+    
+    i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, &tx, 1);
+    
+    i2c_.read((ITG3200_I2C_ADDRESS << 1) | 0x01, &rx, 1);
+    
+    return rx;
+
+}
+
+void IMU_I2C::setInterruptConfiguration(char config){
+
+    char tx[2];
+    tx[0] = INT_CFG_REG;
+    tx[1] = config;
+    
+    i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, tx, 2);
+
+}
+
+bool IMU_I2C::isPllReady(void){
+
+    char tx = INT_STATUS;
+    char rx;
+    
+    i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, &tx, 1);
+    
+    i2c_.read((ITG3200_I2C_ADDRESS << 1) | 0x01, &rx, 1);
+    
+    //ITG_RDY bit is bit 4 of INT_STATUS register.
+    if(rx & 0x04){
+        return true;
+    }
+    else{
+        return false;
+    }
+    
+}
+
+bool IMU_I2C::isRawDataReady(void){
+
+    char tx = INT_STATUS;
+    char rx;
+    
+    i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, &tx, 1);
+    
+    i2c_.read((ITG3200_I2C_ADDRESS << 1) | 0x01, &rx, 1);
+    
+    //RAW_DATA_RDY bit is bit 1 of INT_STATUS register.
+    if(rx & 0x01){
+        return true;
+    }
+    else{
+        return false;
+    }
+    
+}
+
+float IMU_I2C::getTemperature(void){
+
+    char tx = TEMP_OUT_H_REG;
+    char rx[2];
+    
+    i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, &tx, 1);
+    
+    i2c_.read((ITG3200_I2C_ADDRESS << 1) | 0x01, rx, 2);
+    
+    int16_t temperature = ((int) rx[0] << 8) | ((int) rx[1]);
+    //Offset = -35 degrees, 13200 counts. 280 counts/degrees C.
+    return 35.0 + ((temperature + 13200)/280.0);
+    
+}
+
+int IMU_I2C::getGyroX(void){
+
+    char tx = GYRO_XOUT_H_REG;
+    char rx[2];
+    
+    i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, &tx, 1);
+    
+    i2c_.read((ITG3200_I2C_ADDRESS << 1) | 0x01, rx, 2);
+    
+    int16_t output = ((int) rx[0] << 8) | ((int) rx[1]);
+
+    return output;
+
+}
+
+int IMU_I2C::getGyroY(void){
+
+    char tx = GYRO_YOUT_H_REG;
+    char rx[2];
+    
+    i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, &tx, 1);
+    
+    i2c_.read((ITG3200_I2C_ADDRESS << 1) | 0x01, rx, 2);
+    
+    int16_t output = ((int) rx[0] << 8) | ((int) rx[1]);
+
+    return output;
+
+}
+
+int IMU_I2C::getGyroZ(void){
+
+    char tx = GYRO_ZOUT_H_REG;
+    char rx[2];
+    
+    i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, &tx, 1);
+    
+    i2c_.read((ITG3200_I2C_ADDRESS << 1) | 0x01, rx, 2);
+    
+    int16_t output = ((int) rx[0] << 8) | ((int) rx[1]);
+
+    return output;
+    
+}
+
+char IMU_I2C::getPowerManagement(void){
+
+    char tx = PWR_MGM_REG;
+    char rx;
+    
+    i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, &tx, 1);
+    
+    i2c_.read((ITG3200_I2C_ADDRESS << 1) | 0x01, &rx, 1);
+    
+    return rx;
+
+}
+
+void IMU_I2C::setPowerManagement(char config){
+
+    char tx[2];
+    tx[0] = PWR_MGM_REG;
+    tx[1] = config;
+
+    i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, tx, 2);
+
+}
+
+

diff -r 000000000000 -r 7b3acf8e2a6f IMU_I2C.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/IMU_I2C.h	Fri Jul 05 04:16:49 2013 +0000
@@ -0,0 +1,226 @@
+#ifndef IMU_I2C_H
+#define IMU_I2C_H
+
+#include "mbed.h"
+
+/////////////ADXL345///////////////////////////////////////////////
+//Registers.
+#define ADXL345_DEVID_REG          0x00
+#define ADXL345_THRESH_TAP_REG     0x1D
+#define ADXL345_OFSX_REG           0x1E
+#define ADXL345_OFSY_REG           0x1F
+#define ADXL345_OFSZ_REG           0x20
+#define ADXL345_DUR_REG            0x21
+#define ADXL345_LATENT_REG         0x22
+#define ADXL345_WINDOW_REG         0x23
+#define ADXL345_THRESH_ACT_REG     0x24
+#define ADXL345_THRESH_INACT_REG   0x25
+#define ADXL345_TIME_INACT_REG     0x26
+#define ADXL345_ACT_INACT_CTL_REG  0x27
+#define ADXL345_THRESH_FF_REG      0x28
+#define ADXL345_TIME_FF_REG        0x29
+#define ADXL345_TAP_AXES_REG       0x2A
+#define ADXL345_ACT_TAP_STATUS_REG 0x2B
+#define ADXL345_BW_RATE_REG        0x2C
+#define ADXL345_POWER_CTL_REG      0x2D
+#define ADXL345_INT_ENABLE_REG     0x2E
+#define ADXL345_INT_MAP_REG        0x2F
+#define ADXL345_INT_SOURCE_REG     0x30
+#define ADXL345_DATA_FORMAT_REG    0x31
+#define ADXL345_DATAX0_REG         0x32
+#define ADXL345_DATAX1_REG         0x33
+#define ADXL345_DATAY0_REG         0x34
+#define ADXL345_DATAY1_REG         0x35
+#define ADXL345_DATAZ0_REG         0x36
+#define ADXL345_DATAZ1_REG         0x37
+#define ADXL345_FIFO_CTL           0x38
+#define ADXL345_FIFO_STATUS        0x39
+
+//Data rate codes.
+#define ADXL345_3200HZ      0x0F
+#define ADXL345_1600HZ      0x0E
+#define ADXL345_800HZ       0x0D
+#define ADXL345_400HZ       0x0C
+#define ADXL345_200HZ       0x0B
+#define ADXL345_100HZ       0x0A
+#define ADXL345_50HZ        0x09
+#define ADXL345_25HZ        0x08
+#define ADXL345_12HZ5       0x07
+#define ADXL345_6HZ25       0x06
+
+// read or write bytes
+#define ADXL345_I2C_READ    0xA7  
+#define ADXL345_I2C_WRITE   0xA6 
+#define ADXL345_I2C_ADDRESS 0x53   //the ADXL345 7-bit address is 0x53 when ALT ADDRESS is low as it is on the sparkfun chip: when ALT ADDRESS is high the address is 0x1D
+
+/////////////when ALT ADDRESS pin is high:
+//#define ADXL345_I2C_READ    0x3B   
+//#define ADXL345_I2C_WRITE   0x3A
+//#define ADXL345_I2C_ADDRESS 0x1D 
+
+#define ADXL345_X           0x00
+#define ADXL345_Y           0x01
+#define ADXL345_Z           0x02
+
+// modes
+#define MeasurementMode     0x08
+
+////////////////L3G4200D/////////////////////////////////////////////////////////////////
+// register addresses
+#define L3G4200D_WHO_AM_I      0x0F
+
+#define L3G4200D_CTRL_REG1     0x20
+#define L3G4200D_CTRL_REG2     0x21
+#define L3G4200D_CTRL_REG3     0x22
+#define L3G4200D_CTRL_REG4     0x23
+#define L3G4200D_CTRL_REG5     0x24
+#define L3G4200D_REFERENCE     0x25
+#define L3G4200D_OUT_TEMP      0x26
+#define L3G4200D_STATUS_REG    0x27
+
+#define L3G4200D_OUT_X_L       0x28
+#define L3G4200D_OUT_X_H       0x29
+#define L3G4200D_OUT_Y_L       0x2A
+#define L3G4200D_OUT_Y_H       0x2B
+#define L3G4200D_OUT_Z_L       0x2C
+#define L3G4200D_OUT_Z_H       0x2D
+
+#define L3G4200D_FIFO_CTRL_REG 0x2E
+#define L3G4200D_FIFO_SRC_REG  0x2F
+
+#define L3G4200D_INT1_CFG      0x30
+#define L3G4200D_INT1_SRC      0x31
+#define L3G4200D_INT1_THS_XH   0x32
+#define L3G4200D_INT1_THS_XL   0x33
+#define L3G4200D_INT1_THS_YH   0x34
+#define L3G4200D_INT1_THS_YL   0x35
+#define L3G4200D_INT1_THS_ZH   0x36
+#define L3G4200D_INT1_THS_ZL   0x37
+#define L3G4200D_INT1_DURATION 0x38
+
+///////////////ITG3200/////////////////////////////////////////////////////
+#define ITG3200_I2C_ADDRESS 0x68 //7-bit address.
+
+#define WHO_AM_I_REG    0x00
+#define SMPLRT_DIV_REG  0x15
+#define DLPF_FS_REG     0x16
+#define INT_CFG_REG     0x17
+#define INT_STATUS      0x1A
+#define TEMP_OUT_H_REG  0x1B
+#define TEMP_OUT_L_REG  0x1C
+#define GYRO_XOUT_H_REG 0x1D
+#define GYRO_XOUT_L_REG 0x1E
+#define GYRO_YOUT_H_REG 0x1F
+#define GYRO_YOUT_L_REG 0x20
+#define GYRO_ZOUT_H_REG 0x21
+#define GYRO_ZOUT_L_REG 0x22
+#define PWR_MGM_REG     0x3E
+
+//----------------------------
+// Low Pass Filter Bandwidths
+//----------------------------
+#define LPFBW_256HZ 0x00
+#define LPFBW_188HZ 0x01
+#define LPFBW_98HZ  0x02
+#define LPFBW_42HZ  0x03
+#define LPFBW_20HZ  0x04
+#define LPFBW_10HZ  0x05
+#define LPFBW_5HZ   0x06
+
+typedef char byte;
+
+///////////////class///////////////////////////////////////////////////////
+
+class IMU_I2C {
+
+public:
+
+    /**
+     * Constructor.
+     */
+    IMU_I2C(PinName sda, PinName scl);
+
+//////ADXL345_function///////////////////////////////////////////////////
+    void getOutput(int* readings);
+    char getDeviceID(void);
+    int setPowerMode(char mode);
+    int setPowerControl(char settings);     
+    char getPowerControl(void);
+    char getDataFormatControl(void);
+    int setDataFormatControl(char settings);
+    int setDataRate(char rate);
+    char getOffset(char axis);
+    int setOffset(char axis, char offset);
+    char getFifoControl(void);
+    int setFifoControl(char settings);
+    char getFifoStatus(void);
+    char getTapThreshold(void);
+    int setTapThreshold(char threshold);
+    float getTapDuration(void);
+    int setTapDuration(short int duration_us);
+    float getTapLatency(void);
+    int setTapLatency(short int latency_ms);
+    float getWindowTime(void);
+    int setWindowTime(short int window_ms);
+    char getActivityThreshold(void);
+    int setActivityThreshold(char threshold);
+    char getInactivityThreshold(void);
+    int setInactivityThreshold(char threshold);
+    char getTimeInactivity(void);
+    int setTimeInactivity(char timeInactivity);
+    char getActivityInactivityControl(void);
+    int setActivityInactivityControl(char settings);
+    char getFreefallThreshold(void);
+    int setFreefallThreshold(char threshold);
+    char getFreefallTime(void);
+    int setFreefallTime(short int freefallTime_ms);
+    char getTapAxisControl(void);
+    int setTapAxisControl(char settings);
+    char getTapSource(void);
+    char getInterruptEnableControl(void);
+    int setInterruptEnableControl(char settings);
+    char getInterruptMappingControl(void);
+    int setInterruptMappingControl(char settings);
+    char getInterruptSource(void);
+    
+///////////////L3G4200D_function///////////////////////////////////////////////
+    void read(int g[3]);
+    
+///////////////ITG3200_function////////////////////////////////////////////////
+    char getWhoAmI(void);
+    void setWhoAmI(char address);
+    char getSampleRateDivider(void);
+    void setSampleRateDivider(char divider);
+    int getInternalSampleRate(void);
+    void setLpBandwidth(char bandwidth);
+    char getInterruptConfiguration(void);
+    void setInterruptConfiguration(char config);
+    bool isPllReady(void);
+    bool isRawDataReady(void);
+    float getTemperature(void);
+    int getGyroX(void);
+    int getGyroY(void);
+    int getGyroZ(void);
+    char getPowerManagement(void);
+    void setPowerManagement(char config);
+   
+private:
+
+    I2C i2c_;
+ 
+////////////ADXL345////////////////////////////////////////////////////////////
+    char SingleByteRead(char address);
+    int SingleByteWrite(char address, char data);
+    void multiByteRead(char startAddress, char* ptr_output, int size);
+    int multiByteWrite(char startAddress, char* ptr_data, int size);
+
+////////////L3G4200D///////////////////////////////////////////////////////////
+    byte data[6];
+    int _rates[3];
+    void writeReg(byte reg, byte value);
+    byte readReg(byte reg);
+    void enableDefault(void);
+    
+};
+
+#endif