#include <Wire.h>

#define ADDRESS 0x1E  //Numero dispositivo collegato

float x,y,z;
long t;
float xm=0,ym=0,zm=0;
 
void setup(){
  Serial.begin(9600); // initialise Serial communication
  Wire.begin();
 
  Wire.beginTransmission(ADDRESS);
  Wire.write(0x02); //select mode register
  Wire.write(0x00); //continuous measurement mode
  Wire.endTransmission();

  Wire.beginTransmission(ADDRESS);
  Wire.write(0x01); //select register 1, comfiguration register B
  Wire.endTransmission();
 
  //get current configuration
  Wire.requestFrom(ADDRESS, 1);
  if (Wire.available()) {
    int breg = Wire.read();
    Serial.print("Current configuration: ");
    Serial.println(breg);
    breg = (breg & 0x2F) | 0x60;
    Serial.print("New configuration: ");
    Serial.println(breg);
    Wire.beginTransmission(ADDRESS);
    Wire.write(0x01); //configuration register B
    Wire.write(breg); //set range +-2,5G
    Wire.endTransmission();
  }
 
  Wire.beginTransmission(ADDRESS);
  Wire.write(0x03); //select register 3, X MSB register
  Wire.endTransmission();
 
  //Read data from each axis, 2 registers per axis
  Wire.requestFrom(ADDRESS, 6);  
  for (int i=1;i<=1000;i++){
    if(Wire.available()) {
      x = Wire.read() * 256;
      x += Wire.read();
      z = Wire.read() * 256;
      z += Wire.read();
      y = Wire.read() * 256;
      y += Wire.read(); 
    }
    xm = xm+x;
    ym = ym+y;
    zm = zm+z;
  }
  xm = xm/1000.;
  ym = ym/1000.;
  zm = zm/1000.;
   
  Serial.print ("La componente x del campo magnetico terrestre vale:");
  Serial.println(xm);
  Serial.print ("La componente y del campo magnetico terrestre vale:");
  Serial.println(ym);
  Serial.print ("La componente z del campo magnetico terrestre vale:");
  Serial.println(zm); 
}

void loop() {  
  Wire.beginTransmission(ADDRESS);
  Wire.write(0x03); //select register 3, X MSB register
  Wire.endTransmission();
 
 //Read data from each axis, 2 registers per axis
  Wire.requestFrom(ADDRESS, 6);
  if(Wire.available()) {
    x = Wire.read() * 256;
    x += Wire.read();
    z = Wire.read() * 256;
    z += Wire.read();
    y = Wire.read() * 256;
    y += Wire.read();
  }
  t=micros();
  Serial.print(t);
  Serial.print("          ");
  Serial.print(x-xm);
  Serial.print("      ");
  Serial.println(y-ym);
}