Allez j’essaie en plusieurs fois.Voici le code jusqu’à la fin du setup :
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#include <math.h>
#include<Servo.h>
#include <ADXL345.h>
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include "Wire.h"
#endif
MPU6050 mpu(0x68); //AD0=Low
MPU6050 mpu2(0x69);
Servo myservo;
Servo myservo2;
Servo myservo3;
Servo myservo4;
Servo myservo5;
ADXL345 adxl;
const char ADXL345 = (0x53);
//MPU control/status vars
bool dmpReady = false;
uint8_t mpuIntStatus; //actual interrupt status byte
uint8_t deviceStatus;//device status , 0 = success ,
uint16_t packetSize; //expected DMP packet size (defult 42) -- ?
uint16_t fifoCount; //count of all bytes currently in FIFO
uint8_t fifoBuffer[64]; // FIFO buffer storage
uint8_t mpuIntStatus2; //actual interrupt status byte
uint8_t deviceStatus2;//device status , 0 = success ,
uint16_t packetSize2; //expected DMP packet size (defult 42) -- ?
uint16_t fifoCount2; //count of all bytes currently in FIFO
uint8_t fifoBuffer2[64]; // FIFO buffer storage
// orientation/motion vars
Quaternion q; // [w, x, y, z] quaternion container
VectorInt16 aa; // [x, y, z] accel sensor measurements
VectorInt16 aaReal; // [x, y, z] gravity-free accel sensor measurements
VectorInt16 aaWorld; // [x, y, z] world-frame accel sensor measurements
VectorFloat gravity; // [x, y, z] gravity vector
float euler[3]; // [psi, theta, phi] Euler angle container
float ypr[3];
float zqs[3];// [yaw, pitch, roll] yaw/pitch/roll container and gravity vector
float yaw, pitch, roll;
float yaw2, pitch2, roll2;
float yaw0, pitch0, roll0;
volatile bool mpuInterrupt = false;
int16_t ax, ay, az;
int16_t gx, gy, gz;
int16_t bx, by, bz;
int16_t hx, hy, hz;
int x, y, z;
int rawX, rawY, rawZ;
int mappedRawX, mappedRawY, mappedRawZ ;
#define LED_PIN 13
bool blinkState = false;
// initial acquisition state flag
bool aginit=false;
bool bhinit=false;
// initial motor state flag
// acqisition loop counter
#define MX_ACQUISITION 10
int loopcpt = 0;
// bad acquistion counter
#define MX_BAD_LOOP 10
int badcpt = 0;
// "pseudo-angle" current mean value
float agx , agy , agz ;
float bhx , bhy , bhz ;
// "pseudo-angle" initial mean value
float agx0, agy0, agz0;
float bhx0, bhy0, bhz0;
// sum of "pseudo-angle"
float sumax = 0.0;
float sumay = 0.0;
float sumaz = 0.0;
float sumbx = 0.0;
float sumby = 0.0;
float sumbz = 0.0;
// no motion counter
#define MX_BUG_GYRO 10
int bugGyro=0;
// system global state flags; We start in the error state
bool GyroDisconnected=true;
bool Gyro2Disconnected=true;
bool GyroFailure=true;
bool Gyro2Failure=true;
int statpin = 13;
void DivideAxis(){
yaw = (ypr[0] * 180/M_PI);
pitch = (ypr[1] * 180/M_PI);
roll = (ypr[2] * 180/M_PI);
}
void SetMeanAxis() {
float modg;
float modg2;
sumax = sumax/loopcpt;
sumay = sumay/loopcpt;
sumaz = sumaz/loopcpt;
sumbx = sumbx/loopcpt;
sumby = sumby/loopcpt;
sumbz = sumbz/loopcpt;
modg = sqrt(sumax*sumax+sumay*sumay+sumaz*sumaz);
agx = acos(sumax/modg)*180/M_PI;
agy = acos(sumay/modg)*180/M_PI;
agz = acos(sumaz/modg)*180/M_PI;
modg2 = sqrt(sumbx*sumbx+sumby*sumby+sumbz*sumbz);
bhx = acos(sumbx/modg2)*180/M_PI;
bhy = acos(sumby/modg2)*180/M_PI;
bhz = acos(sumbz/modg2)*180/M_PI;
loopcpt = 0;
}
void SetInitAxisValue() {
agx0=agx;
agy0=agy;
agz0=agz;
bhx0=bhx;
bhy0=bhy;
bhz0=bhz;
}
bool GetGyroDeviceConnection(){
int i = 0;
while ((!mpu.testConnection())&&(i < 3)){
i++;
loopcpt=0;
mpu.initialize();
}
return mpu.testConnection();
}
bool GetGyro2DeviceConnection(){
int i = 0;
while ((!mpu2.testConnection())&&(i < 3)){
i++;
loopcpt=0;
mpu2.initialize();
}
return mpu2.testConnection();
}
}
void setup() {
myservo.attach(11);
myservo2.attach(10);
myservo3.attach(9);
myservo4.attach(6);
myservo5.attach(12);
//join I2C bus
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
Wire.begin();
TWBR = 48; // 400kHz I2C clock (200kHz if CPU is 8MHz)
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
Fastwire::setup(400, true);
#endif
//initialize serial communication
Serial.begin(115200);
while(!Serial); //common for Leonardo issues
pinMode(statpin, OUTPUT);
adxl.powerOn();
//initialize the mpu
Serial.println("Call MPU6050 Lib to initialize devices...");
mpu.initialize();
mpu2.initialize();
//verify connection
Serial.println("Tesing device connections");
Serial.println(mpu.testConnection() ? F("MPU6050 connection test successed ") : F("MPU6050 connection test failed"));
Serial.println(mpu2.testConnection() ? "MPU6050 n°2 connection successful" : "MPU6050 n°2 connection failed");
GyroDisconnected=false;
Gyro2Disconnected=false;
//load and configure DMP
Serial.println("initializing DMP");
deviceStatus = mpu.dmpInitialize(); //use MPU6050 library to inilisalize the dmp
//feed offsets
mpu.setXGyroOffset(220);
mpu.setYGyroOffset(76);
mpu.setZGyroOffset(-85);
mpu.setZAccelOffset(1788); // 1688 factory default for my test chip
//make sure it works
if (deviceStatus == 0) {
Serial.println("DMP initialization success, now enable DMP for use");
//turn on DMP
mpu.setDMPEnabled(true); //use MPU6050 library to enable DMP)
//wait for first interrup . currently just leave it false automatically
mpuInterrupt == false;
//let main llop know it's ok to use DMP, set dmpRead flag to ture
dmpReady = true;
Serial.println("DMP is ready to use.");
//get expected DMP packet size for later comparison
packetSize = mpu.dmpGetFIFOPacketSize();
} else {
//ERROR! , device status !=0 when initializing DMP
Serial.print("DMP initialization failed when using MPU6050 library:");
if (deviceStatus == 1) {
Serial.println(" intial memory load failed");
} else if (deviceStatus == 2) {
Serial.println(" failed to update DMP configuration");
} else {
Serial.print(" unknow error with code: ");
Serial.println(deviceStatus);
}
}
}
int ticket = 1;
void printOnlyOnce (String message){
if (ticket == 1){
Serial.println(message);
ticket = 0 ;
} else {
return;
}
}
