Robottino autobilanciante: problema imu mpu6050

Salve a tutti! Spero di aver postato nella sezione giusta : )

Uso per diletto arduino uno rev3.
Ho scaricato la iper sfruttata libreria di jeff rowberg, ma proprio non ne vuole sapere di funzionare con la mia imu mpu 6050 6dof della drotek eppure sul web moltissimi usano questa accoppiata oppure anche solo o la libreria o la imu. :roll_eyes:

La mia proprio non ne vuole sapere di andare:
Il risultato è questo:

Initializing I2C devices...
Testing device connections...
MPU6050 connection failed
Initializing DMP...


Resetting MPU6050...
Disabling sleep mode...
Selecting user bank 16...
Selecting memory byte 6...
Checking hardware revision...
Revision @ user[16][6] = 0
Resetting memory bank selection to 0...
Reading OTP bank valid flag...
OTP bank is valid!
Reading gyro offset TC values...
Setting slave 0 address to 0x7F...
Disabling I2C Master mode...
Setting slave 0 address to 0x68 (self)...
Resetting I2C Master control...
Writing DMP code to MPU memory banks (1929 bytes)
ERROR! DMP code verification failed.
DMP Initialization failed (code 1)
Waiting for DMP to stabilize...
1...256
2...256
3...256
4...256
5...256
6...256
7...256
8...256
9...256
10...256
11...256
12...256
13...256
14...256
15...256
16...256
17...256
18...256
19...256
20...256
21...256
22...256
23...256
24...256
25...256
26...256
27...256
28...256
29...256
30...256
31...256
32...256
33...256
34...256
35...256
36...256
37...256
38...256
39...256
40...256
41...256
42...256
43...256
44...256
45...256
46...256
47...256
48...256
49...256
50...256
51...256
52...256
53...256
54...256
55...256
56...256
57...256
58...256
59...256
60...256
61...256
62...256
63...256
64...256
65...256
66...256
67...256
68...256
69...256
70...256

Qualche consiglio?

Posto il codice (modificato rispetto all’iniziale di jeff):

// Include header for custom datatypes and function prototypes
#include "includes.h"

// Include the Arduino PID library
#include <PID_v1.h>

// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation
// is used in I2Cdev.h
#include "Wire.h"

// I2Cdev and MPU6050 must be installed as libraries, or else the .cpp/.h files
// for both classes must be in the include path of your project
#include "I2Cdev.h"

#include "MPU6050_6Axis_MotionApps20.h"
//#include "MPU6050.h" // not necessary if using MotionApps include file

// class default I2C address is 0x68 
// specific I2C addresses may be passed as a parameter here
// AD0 low = 0x68 (default for SparkFun breakout and InvenSense evaluation board)
// AD0 high = 0x69

MPU6050 mpu;

#define OUTPUT_READABLE_YAWPITCHROLL
#define LED_PIN 13
bool blinkState = false;

// MPU control/status vars
bool dmpReady = false; // set true if DMP init was successful
uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU
uint8_t devStatus; // return status after each device operation (0 = success, !0 = error)
uint16_t packetSize; // expected DMP packet size (default is 42 bytes)
uint16_t fifoCount; // count of all bytes currently in FIFO
uint8_t fifoBuffer[64]; // FIFO storage buffer

// orientation/motion vars
Quaternion q; // [w, x, y, z] quaternion container
VectorFloat gravity; // [x, y, z] gravity vector
float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector

// PID variables
double Setpoint, Input, Output;
unsigned long serialTime_FrontEnd; //this will help us know when to talk with processing
double aggKp=130, aggKi=22.0, aggKd=2.4;
double consKp=55.0, consKi=50.00, consKd=1.5;

//Specify the links and initial tuning parameters for PID
PID myPID(&Input, &Output, &Setpoint,consKp,consKi,consKd, DIRECT);

volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high
void dmpDataReady() {
mpuInterrupt = true;
}

void setup() {
  
        //Initialize PID parameters
        Setpoint = 0.0;	
        myPID.SetMode(AUTOMATIC);
        myPID.SetOutputLimits(-255.0,255.0);
        myPID.SetSampleTime(20);
        pinMode(rightMotorA, OUTPUT);
        pinMode(rightMotorB , OUTPUT);
        pinMode(leftMotorA , OUTPUT);
        pinMode(leftMotorB , OUTPUT);
        // join I2C bus (I2Cdev library doesn't do this automatically)
Wire.begin();

// initialize serial communication
Serial.begin(115200);

// initialize device
Serial.println(F("Initializing I2C devices..."));
mpu.initialize();

// verify connection
Serial.println(F("Testing device connections..."));
Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));

// wait for ready
//Serial.println(F("\nSend any character to begin DMP programming and demo: "));
while (Serial.available() && Serial.read()); // empty buffer
//while (!Serial.available()); // wait for data
//while (Serial.available() && Serial.read()); // empty buffer again

// load and configure the DMP
Serial.println(F("Initializing DMP..."));
devStatus = mpu.dmpInitialize();

// make sure it worked (returns 0 if so)
if (devStatus == 0) {
// turn on the DMP, now that it's ready
Serial.println(F("Enabling DMP..."));
mpu.setDMPEnabled(true);

// enable Arduino interrupt detection
Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
attachInterrupt(0, dmpDataReady, RISING);
mpuIntStatus = mpu.getIntStatus();

// set our DMP Ready flag so the main loop() function knows it's okay to use it
Serial.println(F("DMP ready! Waiting for first interrupt..."));
dmpReady = true;

// get expected DMP packet size for later comparison
packetSize = mpu.dmpGetFIFOPacketSize();
}
else {
// ERROR!
// 1 = initial memory load failed
// 2 = DMP configuration updates failed
// (if it's going to break, usually the code will be 1)
Serial.print(F("DMP Initialization failed (code "));
Serial.print(devStatus);
Serial.println(F(")"));
}

        

        // wait 8 seconds for DMP to stabilize
        Serial.println("Waiting for DMP to stabilize...");
        for(int i=0; i<70; i++)
        {
          Serial.print(i+1);
          Serial.print("...");
          Serial.println(mpu.getFIFOCount());
          
          delay(100);
          mpu.resetFIFO();
                    
          blinkState = !blinkState;
digitalWrite(LED_PIN, blinkState);
                    
        }
        delay(10);
        // configure LED for output
pinMode(LED_PIN, OUTPUT);
           
}


long time = millis();
// ================================================================
// === MAIN PROGRAM LOOP ===
// ================================================================

void loop() {
// if programming failed, don't try to do anything
if (!dmpReady) return;
        
        
// Serial.println("loop starts, last loop time :");
// Serial.println(millis() - time);
// Serial.println(fifoCount);
// time = millis();
// // wait for MPU interrupt or extra packet(s) available
while (!mpuInterrupt && fifoCount < packetSize) {

                Input = ypr[1] * 180/M_PI;

                if(abs(Input) > KILLZONE)
                {
                  myPID.SetMode(MANUAL);
                  //Serial.println("bang");
                  Output = 0;
                }
                else
                {
                  myPID.SetMode(AUTOMATIC);
                  //Serial.println("BOOYAH");
                  myPID.Compute();
                }
                                  
                               
                .......................................................... //cancello la parte del controllo dei motori x eccessiva lunghezza
}

// reset interrupt flag and get INT_STATUS byte
mpuInterrupt = false;
mpuIntStatus = mpu.getIntStatus();

// get current FIFO count
fifoCount = mpu.getFIFOCount();

// check for overflow (this should never happen unless our code is too inefficient)
if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
// reset so we can continue cleanly
mpu.resetFIFO();
Serial.println(F("FIFO overflow!"));

// otherwise, check for DMP data ready interrupt (this should happen frequently)
}
else if (mpuIntStatus & 0x02) {
// wait for correct available data length, should be a VERY short wait
while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();

// read a packet from FIFO
mpu.getFIFOBytes(fifoBuffer, packetSize);

// track FIFO count here in case there is > 1 packet available
// (this lets us immediately read more without waiting for an interrupt)
fifoCount -= packetSize;

#ifdef OUTPUT_READABLE_YAWPITCHROLL
// display Euler angles in degrees
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
#endif


// blink LED to indicate activity
blinkState = !blinkState;
digitalWrite(LED_PIN, blinkState);
}
}

Il classico I2C scanner funziona dando 68 come risultato e da quello che ho capito è giusto. Ma qui dice connection failed!
I collegamenti dovrebbero quindi essere giusti :
int---->pin D2 (interrupt 0);
5v----->5v
gnd---->gnd
sdi—>A4
scl—>A5
Non ha senso che nn vada! Avete qualche idea?