How to make changes to read 3 sensors?

Hi everyone!

I've made a program to capture inertial data from 1 IMU (GY-80), now i need capture inertial data from 3 IMUs (GY-80) but i dont know how because i dont have experience with memory adresses (non-logic part of the program is whole copied :sweat_smile:). I'm using Wire, ADXL345 and L3G4200D libraries.

my program and all the libraries are available at: https://www.dropbox.com/sh/93rnoh460r2f7dc/BaKGx59D5U

How can i do that? Thank!! :open_mouth:

Post your code here using code tags. Use the hash button above the input box.

Hi!!

Sorry about delay. To get better to read im gonna post each code here in the following order: my program, ADXL345 library (.h/.cpp) and L3G4200D library (.h/.cpp).

The datasheets are available:
ADXL345: https://www.sparkfun.com/datasheets/Sensors/Accelerometer/ADXL345.pdf (I2C chapter page 10)
L3G4200D: http://www.pololu.com/file/0J491/L3G4200D.pdf (I2C chapter page 13)

I think is necessary to create a new object for each Acce and each Gyro but i’ve no idea what do beyond that. mayday mayday! XD
thank you!

My program:

#include <Wire.h>

//ACCE
#include <ADXL345.h>
ADXL345 acc;

const float alpha = 0.5;
double pitch, roll, yaw, xa=0, ya=0, za=0, xaa=0, yaa=0, zaa=0;

//GYRO
#include <L3G4200D.h>
L3G4200D gyro;

int sampleNum=500;
int dc_offset_x=0;
int dc_offset_y=0;
int dc_offset_z=0;
double noise=0;
unsigned long time;
int sampleTime=20;
int rate_x;
int rate_y;
int rate_z;
int prev_rate_x=0;
int prev_rate_y=0;
int prev_rate_z=0;
double x=0;
double y=0;
double z=0;

void setup() {

Serial.begin(9600);
Wire.begin();

//ACCE
acc.begin();

//GYRO
gyro.enableDefault();
//Calculate initial DC offset and noise level of gyro
for(int n=0;n<sampleNum;n++){
gyro.read();
dc_offset_x+=(int)gyro.g.x;
dc_offset_y+=(int)gyro.g.y;
dc_offset_z+=(int)gyro.g.z;
}
dc_offset_x=dc_offset_x/sampleNum;
dc_offset_y=dc_offset_y/sampleNum;
dc_offset_z=dc_offset_z/sampleNum;
for(int n=0;n<sampleNum;n++){
gyro.read();
if((int)gyro.g.z-dc_offset_z>noise)
noise=(int)gyro.g.z-dc_offset_z;
else if((int)gyro.g.z-dc_offset_z<-noise)
noise=-(int)gyro.g.z-dc_offset_z;
}
noise=noise/100; //gyro returns hundredths of degrees/sec
delay(100);
}

//GYRO
void giroscopio(){

if(millis() - time > sampleTime){
time = millis(); // update the time to get the next sample
gyro.read();
rate_x=((int)gyro.g.x-dc_offset_x)/100;
rate_y=((int)gyro.g.y-dc_offset_y)/100;
rate_z=((int)gyro.g.z-dc_offset_z)/100;
if(rate_x >= noise || rate_x <= -noise)
x += ((double)(prev_rate_x + rate_x) * sampleTime * 1.32) / 2000;
if(rate_y >= noise || rate_y <= -noise)
y += ((double)(prev_rate_y + rate_y) * sampleTime * 1.32) / 2000;
if(rate_z >= noise || rate_z <= -noise)
z += ((double)(prev_rate_z + rate_z) * sampleTime * 1.32) / 2000;

// remember the current speed for the next loop rate integration.
prev_rate_x = rate_x;
prev_rate_y = rate_y;
prev_rate_z = rate_z;

// Keep our angle between 0-359 degrees
if (x < 0)
x += 360;
else if (x >= 360)
x -= 360;
if (y < 0)
y += 360;
else if (y >= 360)
y -= 360;
if (z < 0)
z += 360;
else if (z >= 360)
z -= 360;
}
}
//ACCE
void acelerometro(){
acc.read(&xa, &ya, &za);

//Low Pass Filter
xa = xa * alpha + (xa * (1.0 - alpha));
ya = ya * alpha + (ya * (1.0 - alpha));
za = za * alpha + (za * (1.0 - alpha));

xa=xa/sqrt(xa*xa+ya*ya+za*za);
ya=ya/sqrt(xa*xa+ya*ya+za*za);
za=za/sqrt(xa*xa+ya*ya+za*za);

}
void acelerometro_angulo(){
xaa=acos(xa)*180/PI;
yaa=acos(ya)*180/PI;
zaa=acos(za)*180/PI; 
}
void acelerometro_pr(){
	//Roll & Pitch Equations
	roll  = (atan2(-ya, za)*180.0)/M_PI;
	pitch = (atan2(xa, sqrt(ya*ya + za*za))*180.0)/M_PI;
        yaw=0;
if(roll<0) roll+=360;
if((zaa>90)&&(zaa<180)) pitch=(pitch-180)*(-1); //está no segundo ou terceiro quadrante. força range de 90 ate 270
else if((zaa<90)&&(pitch<0)) pitch+=360; //está no quarto quadrante. força range de 270 ate 0
}
void loop(){
  acelerometro(); // medidas de aceleraçao
  acelerometro_angulo(); //medidas de angulos entre o sensor e os eixos
  acelerometro_pr(); //medidas em pitch e roll
  giroscopio();

//ANGULOS DE ROTACAO (ACELEROMETRO) - PROCESSING/MATLAB:        
      Serial.print(roll, 4); //sobre eixo x
      Serial.print(",");
      Serial.print(-pitch, 4);
      Serial.print(",");
      Serial.print(yaw); // sobre eixo 
//      Serial.print('\n'); 
      Serial.print(",");
//ANGULOS DE ROTACAO (GIROSCOPIO) - PROCESSING:        
        Serial.print(-x, 4); //sobre eixo x
        Serial.print(",");
        Serial.print(y, 4);
        Serial.print(",");
        Serial.print(-z, 4); // sobre eixo 
        Serial.print(",");
        //
        Serial.print(-x, 4); //sobre eixo x
        Serial.print(",");
        Serial.print(y, 4);
        Serial.print(",");
        Serial.print(-z, 4); // sobre eixo 
        Serial.print('\n');
 delay(1);
}

ADXL345 library (.h)

/********************************************************************************
* ADXL345 Library - ADXL345.h                                                   *
*                                                                               *
* Copyright (C) 2012 Anil Motilal Mahtani Mirchandani(anil.mmm@gmail.com)       *
*                                                                               *
* License GPLv3+: GNU GPL version 3 or later <http://gnu.org/licenses/gpl.html> *
* This is free software: you are free to change and redistribute it.            *
* There is NO WARRANTY, to the extent permitted by law.                         *
*                                                                               *
*********************************************************************************/

#ifndef _ADXL345_H
#define _ADXL345_H

#include <Arduino.h>

// All functions return -1 on error and 0 on success
class ADXL345
{
	private:
		/* ADXL345 Registers */
		enum info_t 
		{
			DEVID          = 0x00,
			THRESH_TAP     = 0x1D,
			OFSX           = 0x1E,
			OFSY           = 0x1F,
			OFSZ           = 0x20,
			DUR            = 0x21,
			Latent         = 0x22,
			Window         = 0x23,
			THRESH_ACT     = 0x24,
			THRESH_INACT   = 0x25,
			TIME_INACT     = 0x26,
			ACT_INACT_CTL  = 0x27,
			THRESH_FF      = 0x28,
			TIME_FF        = 0x29,
			TAP_AXES       = 0x2A,
			ACT_TAP_STATUS = 0x2B,
			BW_RATE        = 0x2C,
			POWER_CTL      = 0x2D,
			INT_ENABLE     = 0x2E,
			INT_MAP        = 0x2F,
			INT_SOURCE     = 0x30,
			DATA_FORMAT    = 0x31,
			DATAX0         = 0x32,
			DATAX1         = 0x33,
			DATAY0         = 0x34,
			DATAY1         = 0x35,
			DATAZ0         = 0x36,
			DATAZ1         = 0x37,
			FIFO_CTL       = 0x38,
			FIFO_STATUS    = 0x39,
			ADDRESS        = 0xA7 >> 1
		};


		int zG[3];
		int readRegister(byte reg_addr, int nbytes, byte *buffer);
		int writeRegister(byte reg_addr, int nbytes, byte *buffer);
	
	public:
		enum range_t
		{
			RANGE_16G = 0x0B,
			RANGE_8G  = 0x0A,
			RANGE_4G  = 0x09,
			RANGE_2G  = 0x08
		};

		ADXL345();
	
		void begin();
		void end();
	
		//G Reading
		void read(double *x, double *y, double *z);
	
		//Raw reading
		void read(int *x, int *y, int *z);
	
		//Unit must be res_t
		void setRange(range_t range);

		//Unit must be G
		void setZeroG(double x, double y, double z);
	
		//Unit must be "Raw"
		void setZeroG(int x, int y, int z);
};

#endif // _ADXL345_H

ADXL345 library (.cpp)

/********************************************************************************
* ADXL345 Library - ADXL345.cpp                                                 *
*                                                                               *
* Copyright (C) 2012 Anil Motilal Mahtani Mirchandani(anil.mmm@gmail.com)       *
*                                                                               *
* License GPLv3+: GNU GPL version 3 or later <http://gnu.org/licenses/gpl.html> *
* This is free software: you are free to change and redistribute it.            *
* There is NO WARRANTY, to the extent permitted by law.                         *
*                                                                               *
*********************************************************************************/

#include <ADXL345.h>
#include <Wire.h>

int ADXL345::writeRegister(byte reg_addr, int nbytes, byte *buffer)
{
	int written_bytes;
	
	Wire.beginTransmission(ADXL345::ADDRESS);
	Wire.write(reg_addr);
	written_bytes = Wire.write(buffer, nbytes);
	Wire.endTransmission();
	
	return written_bytes;
}

int ADXL345::readRegister(byte reg_addr, int nbytes, byte *buffer)
{
	int idx = 0;

	Wire.beginTransmission(ADXL345::ADDRESS);
	Wire.write(reg_addr);
	Wire.endTransmission(); 
	
	Wire.requestFrom(ADXL345::ADDRESS, nbytes);

	while(Wire.available() && idx < nbytes)
	{ 
		buffer[idx++] = Wire.read();
	}
	
	return idx;
}

ADXL345::ADXL345()
{
	Wire.begin();
	
	zG[0] = -20;
	zG[1] =  15;
	zG[2] = -23;
}

void ADXL345::begin()
{
	byte data = 0x08;
	writeRegister(ADXL345::POWER_CTL, 1, &data);
}

void ADXL345::end()
{
	byte data = 0x00;
	writeRegister(ADXL345::POWER_CTL, 1, &data);
}

void ADXL345::read(double *x, double *y, double *z)
{
	byte buffer[6];

	readRegister(ADXL345::DATAX0, 6, buffer);
	
	*x = ((buffer[0] + (buffer[1] << 8)) - zG[0])/256.0;
	*y = ((buffer[2] + (buffer[3] << 8)) - zG[1])/256.0;
	*z = ((buffer[4] + (buffer[5] << 8)) - zG[2])/256.0;
}

void ADXL345::read(int *x, int *y, int *z)
{
	byte buffer[6];

	readRegister(ADXL345::DATAX0, 6, buffer);
	
	*x = buffer[0] + (buffer[1] << 8);
	*y = buffer[2] + (buffer[3] << 8);
	*z = buffer[4] + (buffer[5] << 8);
}


void ADXL345::setRange(range_t range)
{	
	switch(range)
	{
		case RANGE_16G:
		case RANGE_8G:
		case RANGE_4G:
		case RANGE_2G:
			writeRegister(ADXL345::DATA_FORMAT, 1, (byte *)&range);
			break;
	}
}

void ADXL345::setZeroG(double x, double y, double z)
{
	zG[0] = x*256.0;
	zG[1] = y*256.0;
	zG[2] = z*256.0;
}

void ADXL345::setZeroG(int x, int y, int z)
{
	zG[0] = x;
	zG[1] = y;
	zG[2] = z;
}

L3G4200D library (.h)

#ifndef L3G4200D_h
#define L3G4200D_h

#include <Arduino.h> // for byte data type

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

class L3G4200D
{
	public:
		typedef struct vector
		{
			float x, y, z;
		} vector;
		
		vector g; // gyro angular velocity readings

		void enableDefault(void);
		
		void writeReg(byte reg, byte value);
		byte readReg(byte reg);
		
		void read(void);
		
		// vector functions
		static void vector_cross(const vector *a, const vector *b, vector *out);
		static float vector_dot(const vector *a,const vector *b);
		static void vector_normalize(vector *a);
};

#endif

L3G4200D library (.cpp)

#include <L3G4200D.h>
#include <Wire.h>
#include <math.h>

// Defines ////////////////////////////////////////////////////////////////

// The Arduino two-wire interface uses a 7-bit number for the address, 
// and sets the last bit correctly based on reads and writes
#define GYR_ADDRESS (0xD2 >> 1)

// Public Methods //////////////////////////////////////////////////////////////

// Turns on the L3G4200D's gyro and places it in normal mode.
void L3G4200D::enableDefault(void)
{
	// 0x0F = 0b00001111
	// Normal power mode, all axes enabled
	writeReg(L3G4200D_CTRL_REG1, 0x0F);
}

// Writes a gyro register
void L3G4200D::writeReg(byte reg, byte value)
{
	Wire.beginTransmission(GYR_ADDRESS);
	Wire.write(reg);
	Wire.write(value);
	Wire.endTransmission();
}

// Reads a gyro register
byte L3G4200D::readReg(byte reg)
{
	byte value;
	
	Wire.beginTransmission(GYR_ADDRESS);
	Wire.write(reg);
	Wire.endTransmission();
	Wire.requestFrom(GYR_ADDRESS, 1);
	value = Wire.read();
	Wire.endTransmission();
	
	return value;
}

// Reads the 3 gyro channels and stores them in vector g
void L3G4200D::read()
{
	Wire.beginTransmission(GYR_ADDRESS);
	// assert the MSB of the address to get the gyro 
	// to do slave-transmit subaddress updating.
	Wire.write(L3G4200D_OUT_X_L | (1 << 7)); 
	Wire.endTransmission();
	Wire.requestFrom(GYR_ADDRESS, 6);

	while (Wire.available() < 6);
	
	uint8_t xla = Wire.read();
	uint8_t xha = Wire.read();
	uint8_t yla = Wire.read();
	uint8_t yha = Wire.read();
	uint8_t zla = Wire.read();
	uint8_t zha = Wire.read();

	g.x = xha << 8 | xla;
	g.y = yha << 8 | yla;
	g.z = zha << 8 | zla;
}

void L3G4200D::vector_cross(const vector *a,const vector *b, vector *out)
{
  out->x = a->y*b->z - a->z*b->y;
  out->y = a->z*b->x - a->x*b->z;
  out->z = a->x*b->y - a->y*b->x;
}

float L3G4200D::vector_dot(const vector *a,const vector *b)
{
  return a->x*b->x+a->y*b->y+a->z*b->z;
}

void L3G4200D::vector_normalize(vector *a)
{
  float mag = sqrt(vector_dot(a,a));
  a->x /= mag;
  a->y /= mag;
  a->z /= mag;
}

as the brakout has no chipselect/address pin(s) I belive you will have to use (if you dont want to modify the boards) a) a software I2c to enable more comm-pins b) a bidirectioal multiplekser

I really dont want to modify the boards.

a) a software I2c to enable more comm-pins b) a bidirectioal multiplekser

if i chose the "a", i don't need a multiplexer, sure? may be this the simplest way? i'm a little bit lost yet

I've just read from datasheet (page 18), the ADXL345 provides only two output pins for driving interrupts but i want to use 3 accelerometers (or maybe more), so it doesnt have enough address.

So without mux I dont achieve to my purpose.

Could someone tell me how choose a mux?

As I understand you have a card with acc, gyro and compass. (9DOF)
And you need to read data from at least 3 of such devices (27 values)!?

As all modules are identical, with no way to change addresses the units must be connected to different pins, or connectet on at time…
Before beeing hasty, do you really to read 3*3 devices ? (what is your setup/idea)

take a look at this device: (up to 4 devices connected)
http://www.nxp.com/documents/data_sheet/74HC_HCT4052.pdf
There are several similar ICs that can do bidirectional multiplexing/demultiplexing
You will need one chip for every 2 dataline. (keep interrupts away for now).

software aspect…:
you will need extra sets of variables (may be arrays). Program code will also increase in volume and executios speed will be affected.
arduino due ? (has a double set of i2c.interface)

mux.jpg

Tks!! you are right, I cant change the devices address so the best way for me is to use a mux/demux or use more I2C interfaces, but im using an Arduino Uno, so i cant because it's only one I2C interface XD

I'm not into the details, but someone has tried software I2C http://todbot.com/blog/2010/09/25/softi2cmaster-add-i2c-to-any-arduino-pins/ It may be possible to connect 3 units to 6 pins, then use same software sequence to communicate (one single line to change pins. If this sentence take variables as parameters, it might work quite simple)