Жироскоп, акселерометър и магнитометър MinIMU-9 и Arduino

/*

 Описание: 
 * Инициализира жироскопа, акселерометъра и магнитометъра 
 * В програмния цикъл е активна само функцията за четене 
   от жироскопа
 * В Serial Monitor връща данни за X, Y и Z само от него

 Свързване:
 SCL -> A5
 SDA -> A4
 VIN -> 5V
 GND -> GND

 Tihomir Trifonov (11-2011)
 email:tisho@inbox.com

 с подкрепата на Robotev.com

*/

#include <Wire.h>//библиотека за I2C комуникация

int x;
int y;
int z;

int readRegister(int deviceAddress, byte regAddress);// чете от регистър
void writeRegister(int deviceAddress, byte regAddress, byte val);// записва
void readACC(); // чете данните за X, Y и Z от акселерометъра
void readGYRO();// X, Y и Z от жироскопа
void readMAG();//  X, Y и Z от магнитометъра

void setup(){ // начални инициализации

  Wire.begin();// започва комуникация по I2C 
  Serial.begin(9600);// серийна комуникация с PC

  // инициализира жироскопа L3G4200D
  writeRegister(0b01101001, 0x23, 0b00110000);// чувствителност 2000dps
  writeRegister(0b01101001, 0x20, 0b00001111);// normal mode, enable X, Y, Z

  // инициализира акселерометъра в LSM303DLH
  writeRegister(0b0011000, 0x23, 0b00110000);// чувствителност 8g
  writeRegister(0b0011000, 0x20, 0b00100111);// enable, normal mode, 50 Hz

  // инициализира магнитометъра в LSM303DLH
  writeRegister(0b0011110, 0x01, 0b11100000);// чувствителност 8.1 Gauss
  writeRegister(0b0011110, 0x02, 0b00000000);// Continuous-conversion mode

  delay(1500);
}

void readACC(){// чете данните за X, Y и Z от акселерометъра

  byte xMSB = readRegister(0b0011000, 0x29);// чете старшия байт
  byte xLSB = readRegister(0b0011000, 0x28);// чете младшия байт
  x = ((xMSB << 8) | xLSB)>>4;// 12 бита за X

  byte yMSB = readRegister(0b0011000, 0x2B);
  byte yLSB = readRegister(0b0011000, 0x2A);
  y = ((yMSB << 8) | yLSB)>>4;// 12 бита за Y

  byte zMSB = readRegister(0b0011000, 0x2D);
  byte zLSB = readRegister(0b0011000, 0x2C);
  z = ((zMSB << 8) | zLSB)>>4;// 12 бита за Z
}

void readGYRO(){// чете данните за X, Y и Z от жироскопа

  byte xMSB = readRegister(0b1101001, 0x29);
  byte xLSB = readRegister(0b1101001, 0x28);
  x = ((xMSB << 8) | xLSB);// 16 бита за X

  byte yMSB = readRegister(0b1101001, 0x2B);
  byte yLSB = readRegister(0b1101001, 0x2A);
  y = ((yMSB << 8) | yLSB);// 16 бита за Y

  byte zMSB = readRegister(0b1101001, 0x2D);
  byte zLSB = readRegister(0b1101001, 0x2C);
  z = ((zMSB << 8) | zLSB);// 16 бита за Z
}

void readMAG(){//  чете данните за X, Y и Z от магнитометъра

  byte xMSB = readRegister(0b0011110, 0x03);
  byte xLSB = readRegister(0b0011110, 0x04);
  x = ((xMSB << 8) | xLSB);// 16 бита за X

  byte yMSB = readRegister(0b0011110, 0x05);
  byte yLSB = readRegister(0b0011110, 0x06);
  y = ((yMSB << 8) | yLSB);// 16 бита за Y

  byte zMSB = readRegister(0b0011110, 0x07);
  byte zLSB = readRegister(0b0011110, 0x08);
  z = ((zMSB << 8) | zLSB);// 16 бита за Z
}

void loop(){ // програмен цикъл

 //// само 1 ф-я трябва да е активна 
   readGYRO();// чете от жироскопа
 //readACC(); // чете от акселерометъра
 //readMAG(); // чете от магнитометъра
 ////

 // Извжежда X, Y и Z в Serial Monitor
  Serial.print("X:");
  Serial.print(x);
  Serial.print(" Y:");
  Serial.print(y);
  Serial.print(" Z:");
  Serial.println(z);

  delay(100); // забавя изпълнението
}

void writeRegister(int deviceAddress, byte address, byte val) {// записва в регистър
    Wire.beginTransmission(deviceAddress);  //адрес на устройството
    Wire.send(address); // aдрес на регистъра
    Wire.send(val); // стойност за запис в регистъра
    Wire.endTransmission();// край 
}

int readRegister(int deviceAddress, byte address){// чете съдържанието на регистър

    int v;// съхранява 1 байт
    Wire.beginTransmission(deviceAddress);// адрес на устройството
    Wire.send(address); // адрес на регистъра
    Wire.endTransmission();// край

    Wire.requestFrom(deviceAddress, 1); // изисква 1 байт
    while(!Wire.available()) {} // изчаква получаването на байта

    v = Wire.receive(); // присвоява го на промелива
    return v; // връща байта през името на функцията
}