tradurre sketch da un sensore ad un altro

Buongiorno a tutti!
mi trovo nella spiacevole condizione in cui non riesco a comprendere come poter tradurre un codice realizzato per funzionare con uno specifico sensore e quindi sulla relativa libreria (Adafruit_ADXL345_U.h), per poterlo sfruttare su un altro sensore: (MPU_6050).

Se qualcuno riuscisse a darmi una mano glie ne sarei davvero molto grato, sono disperato =( =( =(

Il codice l'ho trovato su instructables; nel seguente articolo:

Il codice lo ritrascrivo qui sotto:

#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_ADXL345_U.h>


// Gyro variables
#define CTRL_REG1 0x20
#define CTRL_REG2 0x21
#define CTRL_REG3 0x22
#define CTRL_REG4 0x23
#define CTRL_REG5 0x24

int L3G4200D_Address = 105; 

// Accel
Adafruit_ADXL345_Unified accel = Adafruit_ADXL345_Unified(12345);

//Set up accelerometer variables
float accBiasX, accBiasY, accBiasZ;
float accAngleX, accAngleY;
double accRoll = 0;

//Set up gyroscope variables
float gyroBiasX, gyroBiasY, gyroBiasZ;
float gyroRateX, gyroRateY, gyroRateZ;
float gyroRoll = 0;
//double gyro_sensitivity = 70;  //From datasheet, depends on Scale, 2000DPS = 70, 500DPS = 17.5, 250DPS = 8.75. 

// Gyro Variables
int x;
int y;
int z;

void InitSensors(){
  
  Wire.begin();      // Initialize I2C
  
  // ¿200?
  setupL3G4200D(2000);     // Configure L3G4200  - 250, 500 or 2000 deg/sec
  delay(1500); 

  if(!accel.begin())       // Initialize Accelerometer
  {
    /* There was a problem detecting the ADXL345 ... check your connections */
    Serial.println("Ooops, no ADXL345 detected ... Check your wiring!");
    while(1);
  }
  
  accel.setRange(ADXL345_RANGE_2_G);    // Configure accel
  
  sensors_event_t event;     // Variable for accel data
  
  // Calculate bias for the Gyro and accel, the values it gives when it's not moving
  // You have to keep the robot vertically and static for a few seconds
  
  for(int i=1; i < 100; i++){      // Takes 100 values to get more precision
    
    getGyroValues();              // Get gyro data
    gyroBiasX += (int)x;
    gyroBiasY += (int)y;
    gyroBiasZ += (int)z;  

    accel.getEvent(&event);       // Get accel data
    accBiasX += event.acceleration.x;
    accBiasY += event.acceleration.y;
    accBiasZ += event.acceleration.z;

    delay(1);
    
  }
  
  // Final bias values for every axis
  gyroBiasX = gyroBiasX / 100;
  gyroBiasY = gyroBiasY / 100;
  gyroBiasZ = gyroBiasZ / 100;

  accBiasX = accBiasX / 100;
  accBiasY = accBiasY / 100;
  accBiasZ = accBiasZ / 100;
  

  //Get Starting Pitch and Roll
  accel.getEvent(&event);
  accRoll = (atan2(event.acceleration.y,-event.acceleration.z)+PI)*RAD_TO_DEG;

  if (accRoll <= 360 & accRoll >= 180){
    accRoll = accRoll - 360;
  }  
  
  //gyroRoll = accRoll;
  
}


void InitialValues(){
  
  //  Accelerometer   
  sensors_event_t event; 
  double InitialAngle = 0;
  double dGyro = 0;
  for(int i=1; i < 100; i++){      // Takes 100 values to get more precision
    
    accel.getEvent(&event);
    accRoll = (atan2(event.acceleration.y-accBiasY,-(event.acceleration.z-accBiasZ))+PI)*RAD_TO_DEG;
        
    getGyroValues();
    gyroRateX = ((int)x - gyroBiasX)*.07; 
  
    dGyro = gyroRateX * ((double)(micros() - timer)/1000000);
    
    InitialAngle = 0.98* (InitialAngle + dGyro) + 0.02 * (accRoll);
    timer = micros();
    
    delay(1);
    
  }
 
  InitialRoll = InitialAngle;
  
  Serial.print("Roll Inicial: ");
  Serial.println(InitialRoll);
  
}


// Roll from accelerometer
double getAccelRoll(){
  
  sensors_event_t event; 
  
  accRoll = (atan2(event.acceleration.y-accBiasY,-(event.acceleration.z-accBiasZ))+PI)*RAD_TO_DEG;   // Calculate the value of the angle 
  
  if (accRoll <= 360 & accRoll >= 180){
    accRoll = accRoll - 360;
  }
  
  return accRoll;
  
}

// Roll from gyroscope
double getGyroRoll(){
  
  getGyroValues();     // Get values from gyro
  
  // read raw angular velocity measurements from device  
  gyroRateX = -((int)x - gyroBiasX)*.07; 
  //gyroRateY = -((int)y - gyroBiasY)*.07; 
  //gyroRateZ = ((int)z - gyroBiasZ)*.07; 
  
  gyroRoll += gyroRateX * ((double)(micros() - timer)/1000000);
  
  return gyroRoll;
  
}

// Angular velocity of Roll by gyroscope
double getDGyroRoll(){
  
  getGyroValues();     // Get values from gyro
  
  // read raw angular velocity measurements from device  
  gyroRateX = -((int)x - gyroBiasX)*.07; 
  //gyroRateY = -((int)y - gyroBiasY)*.07; 
  //gyroRateZ = ((int)z - gyroBiasZ)*.07; 
  
  double dgyroRoll = gyroRateX * ((double)(micros() - timer)/1000000);
  
  return dgyroRoll;
  
}

/////////////////////////////////////////////////////////////
//////////////        Gyro Sensor Code     //////////////////
/////////////////////////////////////////////////////////////


// This part of code if from Jim Lindblom of Sparkfun's code
// used for read the data from the gyro sensor: L3G4200D

void getGyroValues(){

  byte xMSB = readRegister(L3G4200D_Address, 0x29);
  byte xLSB = readRegister(L3G4200D_Address, 0x28);
  x = ((xMSB << 8) | xLSB);

  byte yMSB = readRegister(L3G4200D_Address, 0x2B);
  byte yLSB = readRegister(L3G4200D_Address, 0x2A);
  y = ((yMSB << 8) | yLSB);

  byte zMSB = readRegister(L3G4200D_Address, 0x2D);
  byte zLSB = readRegister(L3G4200D_Address, 0x2C);
  z = ((zMSB << 8) | zLSB);
}

int setupL3G4200D(int scale){
  
  //From  Jim Lindblom of Sparkfun's code

  // Enable x, y, z and turn off power down:
  writeRegister(L3G4200D_Address, CTRL_REG1, 0b00001111);

  // If you'd like to adjust/use the HPF, you can edit the line below to configure CTRL_REG2:
  writeRegister(L3G4200D_Address, CTRL_REG2, 0b00000000);

  // Configure CTRL_REG3 to generate data ready interrupt on INT2
  // No interrupts used on INT1, if you'd like to configure INT1
  // or INT2 otherwise, consult the datasheet:
  writeRegister(L3G4200D_Address, CTRL_REG3, 0b00001000);

  // CTRL_REG4 controls the full-scale range, among other things:

  if(scale == 250){
    writeRegister(L3G4200D_Address, CTRL_REG4, 0b00000000);
  }else if(scale == 500){
    writeRegister(L3G4200D_Address, CTRL_REG4, 0b00010000);
  }else{
    writeRegister(L3G4200D_Address, CTRL_REG4, 0b00110000);
  }

  // CTRL_REG5 controls high-pass filtering of outputs, use it
  // if you'd like:
  writeRegister(L3G4200D_Address, CTRL_REG5, 0b00000000);
  
}

void writeRegister(int deviceAddress, byte address, byte val) {
    Wire.beginTransmission(deviceAddress); // start transmission to device 
    Wire.write(address);       // send register address
    Wire.write(val);         // send value to write
    Wire.endTransmission();     // end transmission
}

int readRegister(int deviceAddress, byte address){

    int v;
    Wire.beginTransmission(deviceAddress);
    Wire.write(address); // register to read
    Wire.endTransmission();

    Wire.requestFrom(deviceAddress, 1); // read a byte

    while(!Wire.available()) {
        // waiting
    }

    v = Wire.read();
    return v;
    
}

questa è solo la parte dello sketch inerente appunto i sensori (accelerometro-giroscopio) dell'intero programma che serve per il controllo del robot.

Se servono altre informazioni vi prego di farmi sapere, vi ringrazio moltissimo per la pazienza e le disponibilità.

Io parto dal seguente codice... Associato a filtro di Kalman, il quale mi fornisce su LCD l'angolo rispetto all'asse Y...

/* Copyright (C) 2012 Kristian Lauszus, TKJ Electronics. All rights reserved.

 This software may be distributed and modified under the terms of the GNU
 General Public License version 2 (GPL2) as published by the Free Software
 Foundation and appearing in the file GPL2.TXT included in the packaging of
 this file. Please note that GPL2 Section 2[b] requires that all works based
 on this software must also be made publicly available under the terms of
 the GPL2 ("Copyleft").

 Contact information
 -------------------

 Kristian Lauszus, TKJ Electronics
 Web      :  http://www.tkjelectronics.com
 e-mail   :  kristianl@tkjelectronics.com
 */

#include <Wire.h>
#include <LCD.h>
#include <LiquidCrystal_I2C.h>
#include "Kalman.h" // Source: https://github.com/TKJElectronics/KalmanFilter

#define I2C_ADDR    0x27

#define  LED_OFF  0
#define  LED_ON  1

LiquidCrystal_I2C lcd(I2C_ADDR, 4, 5, 6, 0, 1, 2, 3, 7, NEGATIVE);

Kalman kalmanX; // Create the Kalman instances
Kalman kalmanY;

/* IMU Data */
int16_t accX, accY, accZ;
int16_t tempRaw;
int16_t gyroX, gyroY, gyroZ;

double accXangle, accYangle; // Angle calculate using the accelerometer
double temp; // Temperature
double gyroXangle, gyroYangle; // Angle calculate using the gyro
double compAngleX, compAngleY; // Calculate the angle using a complementary filter
double kalAngleX, kalAngleY; // Calculate the angle using a Kalman filter

uint32_t timer;
uint8_t i2cData[14]; // Buffer for I2C data

void setup() {
  
   lcd.begin (16,2);  // inicializar lcd 
   // Activamos la retroiluminacion
   lcd.setBacklight(LED_ON);
  
  Serial.begin(115200);
  Wire.begin();
  TWBR = ((F_CPU / 400000L) - 16) / 2; // Set I2C frequency to 400kHz

  i2cData[0] = 7; // Set the sample rate to 1000Hz - 8kHz/(7+1) = 1000Hz
  i2cData[1] = 0x00; // Disable FSYNC and set 260 Hz Acc filtering, 256 Hz Gyro filtering, 8 KHz sampling
  i2cData[2] = 0x00; // Set Gyro Full Scale Range to ±250deg/s
  i2cData[3] = 0x00; // Set Accelerometer Full Scale Range to ±2g
  while (i2cWrite(0x19, i2cData, 4, false)); // Write to all four registers at once
  while (i2cWrite(0x6B, 0x01, true)); // PLL with X axis gyroscope reference and disable sleep mode

  while (i2cRead(0x75, i2cData, 1));
  if (i2cData[0] != 0x68) { // Read "WHO_AM_I" register
    Serial.print(F("Error reading sensor"));
    while (1);
  }

  delay(100); // Wait for sensor to stabilize

  /* Set kalman and gyro starting angle */
  while (i2cRead(0x3B, i2cData, 6));
  accX = ((i2cData[0] << 8) | i2cData[1]);
  accY = ((i2cData[2] << 8) | i2cData[3]);
  accZ = ((i2cData[4] << 8) | i2cData[5]);
  // atan2 outputs the value of -? to ? (radians) - see http://en.wikipedia.org/wiki/Atan2
  // We then convert it to 0 to 2? and then from radians to degrees
  accYangle = (atan2(accX, accZ) + PI) * RAD_TO_DEG;
  accXangle = (atan2(accY, accZ) + PI) * RAD_TO_DEG;

  kalmanX.setAngle(accXangle); // Set starting angle
  kalmanY.setAngle(accYangle);
  gyroXangle = accXangle;
  gyroYangle = accYangle;
  compAngleX = accXangle;
  compAngleY = accYangle;

  timer = micros();
}

void loop() {
  /* Update all the values */
  while (i2cRead(0x3B, i2cData, 14));
  accX = ((i2cData[0] << 8) | i2cData[1]);
  accY = ((i2cData[2] << 8) | i2cData[3]);
  accZ = ((i2cData[4] << 8) | i2cData[5]);
  tempRaw = ((i2cData[6] << 8) | i2cData[7]);
  gyroX = ((i2cData[8] << 8) | i2cData[9]);
  gyroY = ((i2cData[10] << 8) | i2cData[11]);
  gyroZ = ((i2cData[12] << 8) | i2cData[13]);

  // atan2 outputs the value of -? to ? (radians) - see http://en.wikipedia.org/wiki/Atan2
  // We then convert it to 0 to 2? and then from radians to degrees
  accXangle = (atan2(accY, accZ) + PI) * RAD_TO_DEG;
  accYangle = (atan2(accX, accZ) + PI) * RAD_TO_DEG;

  double gyroXrate = (double)gyroX / 131.0;
  double gyroYrate = -((double)gyroY / 131.0);
  gyroXangle += gyroXrate * ((double)(micros() - timer) / 1000000); // Calculate gyro angle without any filter
  gyroYangle += gyroYrate * ((double)(micros() - timer) / 1000000);
  //gyroXangle += kalmanX.getRate()*((double)(micros()-timer)/1000000); // Calculate gyro angle using the unbiased rate
  //gyroYangle += kalmanY.getRate()*((double)(micros()-timer)/1000000);

  compAngleX = (0.93 * (compAngleX + (gyroXrate * (double)(micros() - timer) / 1000000))) + (0.07 * accXangle); // Calculate the angle using a Complimentary filter
  compAngleY = (0.93 * (compAngleY + (gyroYrate * (double)(micros() - timer) / 1000000))) + (0.07 * accYangle);

  kalAngleX = kalmanX.getAngle(accXangle, gyroXrate, (double)(micros() - timer) / 1000000); // Calculate the angle using a Kalman filter
  kalAngleY = kalmanY.getAngle(accYangle, gyroYrate, (double)(micros() - timer) / 1000000);
  timer = micros();

  temp = ((double)tempRaw + 12412.0) / 340.0;

  /* Print Data */
#if 0 // Set to 1 to activate
  Serial.print(accX); Serial.print("\t");
  Serial.print(accY); Serial.print("\t");
  Serial.print(accZ); Serial.print("\t");

  Serial.print(gyroX); Serial.print("\t");
  Serial.print(gyroY); Serial.print("\t");
  Serial.print(gyroZ); Serial.print("\t");
#endif
//  Serial.print(accXangle); Serial.print("\t");
//  Serial.print(gyroXangle); Serial.print("\t");
//  Serial.print(compAngleX); Serial.print("\t");
//  Serial.print(kalAngleX); Serial.print("\t");

//  Serial.print("\t");

//  Serial.print(accYangle); Serial.print("\t");
//  Serial.print(gyroYangle); Serial.print("\t");
//  Serial.print(compAngleY); Serial.print("\t");
//  Serial.print(kalAngleY); Serial.print("\t");

//  Serial.print(temp);Serial.print("\t");

  lcd.clear();
  //lcd.setBacklight(LED_OFF);//Backlight OFF 
  // delay(500);  
  //lcd.backlight(); //Backlight ON 
  lcd.setCursor(0,0);
  lcd.print("Ang Y");
  lcd.setCursor(9,0); 
  lcd.print  ("Temp");
  lcd.setCursor(0,1);
  lcd.print  (kalAngleY);
  lcd.setCursor(9,1);
  lcd.print  (temp);
  
  delay(800); 
  
  //Serial.print("\r\n");
  //delay(1);
}