Bras robotique avec 2 mpu6050, 1 adxl345 et 5 servomoteurs

Mesdames, messieurs, bonjour.

Je viens vers vous pour demander conseil au sujet d'un petit montage sur lequel je travail. La finalité de se mini projet est d'animer un bras robotique qui reproduit en temps réel les mouvements du bras de l'utilisateur afin de lui permettre de dessiner ou d’écrire par le biais de cette machine. Clin d’œil assumé à Stellarc.

D'ores et déjà je vous pris de m'excuser si mon poste et un peu long, je vais essayer de donner le plus de détails possible.

Pour le matériel, voici ce que j'utilise :

  • Deux mpu6050
  • Un adxl345
  • 5 servomoteur MG995 tower Pro
  • Un arduino méga
  • Une alim qui me permet d'alimenter tout ça

Pour les branchements ;

  • Les trois capteurs sont relié aux ports SCL, SDA, 3.3v et GND de ma carte. J'attribue à chaque mpu6050 deux adresses différentes en branchant les port ADO tantôt sur le 3.3v, tantôt sur LOW
  • Les servo sont branché sur les pin PWM 6, 9, 10, 11, 12 et sont alimenté par mon transpho externe.

L'ensemble des capteurs sont disposé sur des gantelets que je sangle au bras de l'utilisateur. Les capteur détectent les positions des différents membres (biceps, avant bras, main) que je transmet aux servo grâce a servo.write(). Ca n'est pas optimal, je pense à essayer plus tard avec un kinect plutôt qu'avec des accéléromètres mais pour l'instant je n'ai que ça sous la main donc j’essaie d'en tirer le meilleur parti.

Je vous joint le code dans un post séparé

Mon problème est le suivant :

immanquablement, les capteur finissent par ne plus faire d'acquisition. En revanche les temps avant le "plantage" fluctuent : les capteur peuvent faire des acquisitions pendant 30 min sans planter ( ce qui n'est déjà pas assez) comme ils peuvent planter au bout de 2 minutes. Ce qui est sur c'est qu'ils plantent beaucoup plus vite lorsqu'ils sont en mouvement que lorsqu'ils sont en position stable. Voyez vous une solution software ou un défaut dans le code qui pourrait expliquer ça?

Pour ce qui est des connexions physique je les ai toutes sécurisée et ce n'est pas ça. Cependant il y en a beaucoup ce qui augmente les facteurs de risques, je ne suis donc sur qu'a 90% ( c'est 10% de trop me direz vous...).

Enfin pensez vous comme moi que je devrai plutôt m'orienter vers la solution kinect? J'en ai un mais il me manque l'adaptateur usb qui n'est pas donné...

Voila, si vous êtes allés au bout de ce post c'est déjà beaucoup et je vous en remercie. Si vous voulez des précisions n'hésitez pas!

Merci d'avance.

avec le code ça ira mieux :slight_smile:

Zut il est trop long pour le forum... J'le post en plusieurs fois ou il y a une autre solution?

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;
  }
}

Et voici la loop() :

void loop() {
  float fax, fay, faz;
  float fbx, fby, fbz;
  
  //if DMP not ready don't do anything 
  if (!dmpReady) {
    printOnlyOnce("MAIN LOOP: DMP disabled");
    return;
  } else {
    //testing overflow 
    if (fifoCount == 1024) {
      mpu.resetFIFO(); 
      mpu2.resetFIFO();     
      Serial.println("FIFO overflow");
    } else {

    //wait for enough avaliable data length
    while ((fifoCount < packetSize)||(fifoCount2 < packetSize2)) {
      //waiting until get enough
      fifoCount = mpu.getFIFOCount();
      fifoCount2 = mpu2.getFIFOCount();
    }
    
    //read this packet from FIFO buffer 
    mpu.getFIFOBytes(fifoBuffer,packetSize);
    mpu2.getFIFOBytes(fifoBuffer2,packetSize2);
    //track FIFO count here is more then one packeage avalible 

    //reset fifo count 
    fifoCount -= packetSize ;
    fifoCount2 -= packetSize2 ;
   // Serial.println(fifoCount);

    if ((fifoCount > 2)||(fifoCount2 > 2)) {
    mpu.resetFIFO();
    mpu2.resetFIFO();
    }
    DivideAxis();
    //display stage 
                // display Euler angles in degrees
            mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
            mpu2.getMotion6(&bx, &by, &bz, &hx, &hy, &hz);         
            adxl.readAccel(&x, &y, &z); //read the accelerometer values and store them in variables  x,y,z
    
    // convert int16 to float 
    fax=float(ax);
    fay=float(ay);
    faz=float(az);

    fbx=float(bx);
    fby=float(by);
    fbz=float(bz);

    rawX = x - 7;
    rawY = y - 6;
    rawZ = z + 10;

  if (rawX < -255) rawX = -255; else if (rawX > 255) rawX = 255;
  if (rawY < -255) rawY = -255; else if (rawY > 255) rawY = 255;
  if (rawZ < -255) rawZ = -255; else if (rawZ > 255) rawZ = 255;
    
    // perform the sum for the average
    if(loopcpt == 0) {
       sumax = 0.0;
       sumay = 0.0;
       sumaz = 0.0;
       sumbx = 0.0;
       sumby = 0.0;
       sumbz = 0.0;
    }
    mappedRawX = map(rawX, -255, 255, 0, 180);
    mappedRawY = map(rawY, -255, 255, 0, 180);
    mappedRawZ = map(rawZ, -255, 255, 0, 180);
    
    sumax = sumax + fax;
    sumay = sumay + fay;
    sumaz = sumaz + faz;
    sumbx = sumbx + fbx;
    sumby = sumby + fby;
    sumbz = sumbz + fbz;
    // increment the acquisition counter
    loopcpt = loopcpt + 1;
             loopcpt = loopcpt + 1;
    
 if (loopcpt == MX_ACQUISITION){
       // test the gyro connection and if the connection is lost try to retablish it. This is done 3 times
       if (GetGyroDeviceConnection() && GetGyro2DeviceConnection()){
          if(loopcpt != 0) {
             // loopcpt is set to 0 by GetGyroDeviceConnection in case of gyro disconnection detection
             // loopcpt is not zero, the nbloop acquisitions are good, compute the mean axis values
             SetMeanAxis();
             // reset the bad loop counter to 0
             badcpt=0;
             if((!aginit) && (!bhinit)) {
                 SetInitAxisValue();
                 Serial.print("a/g zero:\t");
                 Serial.print("b/h zero:\t");
                
                   Serial.print(agx0); Serial.print("\t");
                   Serial.print(agy0); Serial.print("\t");
                   Serial.print(agz0); Serial.print("\t");
                   Serial.print(bhx0); Serial.print("\t");
                   Serial.print(bhy0); Serial.print("\t");
                   Serial.println(bhz0);
                aginit=true;
                bhinit=true;
             } else {    
                Serial.print("a/g:\t");
                Serial.print("b/h:\t");
                   Serial.print(agx); Serial.print("\t");
                   Serial.print(agy); Serial.print("\t");
                   Serial.print(agz); Serial.print("\t");
                   Serial.print(bhx); Serial.print("\t");
                   Serial.print(bhy); Serial.print("\t");
                   Serial.println(bhz);
                   
            mpu.dmpGetQuaternion(&q, fifoBuffer);
            mpu.dmpGetGravity(&gravity, &q);
            mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
            Serial.print("ypr\t");
            Serial.print(yaw); Serial.print("\t");
            Serial.print(pitch); Serial.print("\t");
            Serial.println(roll); 

            Serial.print(" mappedRawX = "); Serial.print(mappedRawX); // raw data with offset
            Serial.print(" mappedRawY = "); Serial.print(mappedRawY); // raw data with offset
            Serial.print(" mappedRawZ = "); Serial.println(mappedRawZ);

            myservo.write(bhy);  
            myservo2.write(agy);
            myservo3.write(agx);  
            myservo4.write(-yaw);
            myservo5.write(mappedRawY);

            mpu.resetFIFO(); 
            mpu2.resetFIFO();
            
         }
            
        }
 
       }
  
      }
  
     }
   
    }
  
   }

Ça doit vous paraitre bordélique et empirique ; je ne suis pas un bon codeur. Je bricole bien mais niveau programmation je suis un noob.

quelle librairie ADXL345.h ?

Ce n'est pas celle d'adafruit. Je ne sait de qui elle est mais je peut la poster:

/**************************************************************************
 *                                                                         *
 * ADXL345 Driver for Arduino                                              *
 *                                                                         *
 ***************************************************************************
 *                                                                         * 
 * This program is free software; you can redistribute it and/or modify    *
 * it under the terms of the GNU License.                                  *
 * This program is distributed in the hope that it will be useful,         *
 * but WITHOUT ANY WARRANTY; without even the implied warranty of          *
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the           *
 * GNU License V2 for more details.                                        *
 *                                                                         *
 ***************************************************************************/
#include "Arduino.h"

#ifndef ADXL345_h
#define ADXL345_h

/* ------- Register names ------- */
#define ADXL345_DEVID 0x00
#define ADXL345_RESERVED1 0x01
#define ADXL345_THRESH_TAP 0x1d
#define ADXL345_OFSX 0x1e
#define ADXL345_OFSY 0x1f
#define ADXL345_OFSZ 0x20
#define ADXL345_DUR 0x21
#define ADXL345_LATENT 0x22
#define ADXL345_WINDOW 0x23
#define ADXL345_THRESH_ACT 0x24
#define ADXL345_THRESH_INACT 0x25
#define ADXL345_TIME_INACT 0x26
#define ADXL345_ACT_INACT_CTL 0x27
#define ADXL345_THRESH_FF 0x28
#define ADXL345_TIME_FF 0x29
#define ADXL345_TAP_AXES 0x2a
#define ADXL345_ACT_TAP_STATUS 0x2b
#define ADXL345_BW_RATE 0x2c
#define ADXL345_POWER_CTL 0x2d
#define ADXL345_INT_ENABLE 0x2e
#define ADXL345_INT_MAP 0x2f
#define ADXL345_INT_SOURCE 0x30
#define ADXL345_DATA_FORMAT 0x31
#define ADXL345_DATAX0 0x32
#define ADXL345_DATAX1 0x33
#define ADXL345_DATAY0 0x34
#define ADXL345_DATAY1 0x35
#define ADXL345_DATAZ0 0x36
#define ADXL345_DATAZ1 0x37
#define ADXL345_FIFO_CTL 0x38
#define ADXL345_FIFO_STATUS 0x39

#define ADXL345_BW_1600 0xF // 1111
#define ADXL345_BW_800  0xE // 1110
#define ADXL345_BW_400  0xD // 1101  
#define ADXL345_BW_200  0xC // 1100
#define ADXL345_BW_100  0xB // 1011  
#define ADXL345_BW_50   0xA // 1010 
#define ADXL345_BW_25   0x9 // 1001 
#define ADXL345_BW_12   0x8 // 1000 
#define ADXL345_BW_6    0x7 // 0111
#define ADXL345_BW_3    0x6 // 0110


/* 
 Interrupt PINs
 INT1: 0
 INT2: 1
 */
#define ADXL345_INT1_PIN 0x00
#define ADXL345_INT2_PIN 0x01

/* 
 Interrupt bit position
 */
#define ADXL345_INT_DATA_READY_BIT 0x07
#define ADXL345_INT_SINGLE_TAP_BIT 0x06
#define ADXL345_INT_DOUBLE_TAP_BIT 0x05
#define ADXL345_INT_ACTIVITY_BIT   0x04
#define ADXL345_INT_INACTIVITY_BIT 0x03
#define ADXL345_INT_FREE_FALL_BIT  0x02
#define ADXL345_INT_WATERMARK_BIT  0x01
#define ADXL345_INT_OVERRUNY_BIT   0x00

#define ADXL345_DATA_READY 0x07
#define ADXL345_SINGLE_TAP 0x06
#define ADXL345_DOUBLE_TAP 0x05
#define ADXL345_ACTIVITY   0x04
#define ADXL345_INACTIVITY 0x03
#define ADXL345_FREE_FALL  0x02
#define ADXL345_WATERMARK  0x01
#define ADXL345_OVERRUNY   0x00




#define ADXL345_OK    1 // no error
#define ADXL345_ERROR 0 // indicates error is predent

#define ADXL345_NO_ERROR   0 // initial state
#define ADXL345_READ_ERROR 1 // problem reading accel
#define ADXL345_BAD_ARG    2 // bad method argument


class ADXL345
{
public:
	bool status;           // set when error occurs 
	// see error code for details
	byte error_code;       // Initial state
	double gains[3];        // counts to Gs
	
	ADXL345();
	void powerOn();
	void readAccel(int* xyx);
	void readAccel(int* x, int* y, int* z);
	void get_Gxyz(double *xyz);
	
	void setTapThreshold(int tapThreshold);
	int getTapThreshold();
	void setAxisGains(double *_gains);
	void getAxisGains(double *_gains);
	void setAxisOffset(int x, int y, int z);
	void getAxisOffset(int* x, int* y, int*z);
	void setTapDuration(int tapDuration);
	int getTapDuration();
	void setDoubleTapLatency(int doubleTapLatency);
	int getDoubleTapLatency();
	void setDoubleTapWindow(int doubleTapWindow);
	int getDoubleTapWindow();
	void setActivityThreshold(int activityThreshold);
	int getActivityThreshold();
	void setInactivityThreshold(int inactivityThreshold);
	int getInactivityThreshold();
	void setTimeInactivity(int timeInactivity);
	int getTimeInactivity();
	void setFreeFallThreshold(int freeFallthreshold);
	int getFreeFallThreshold();
	void setFreeFallDuration(int freeFallDuration);
	int getFreeFallDuration();
	
	bool isActivityXEnabled();
	bool isActivityYEnabled();
	bool isActivityZEnabled();
	bool isInactivityXEnabled();
	bool isInactivityYEnabled();
	bool isInactivityZEnabled();
	bool isActivityAc();
	bool isInactivityAc();
	void setActivityAc(bool state);
	void setInactivityAc(bool state);
	
	bool getSuppressBit();
	void setSuppressBit(bool state);
	bool isTapDetectionOnX();
	void setTapDetectionOnX(bool state);
	bool isTapDetectionOnY();
	void setTapDetectionOnY(bool state);
	bool isTapDetectionOnZ();
	void setTapDetectionOnZ(bool state);
	
	void setActivityX(bool state);
	void setActivityY(bool state);
	void setActivityZ(bool state);
	void setInactivityX(bool state);
	void setInactivityY(bool state);
	void setInactivityZ(bool state);
	
	bool isActivitySourceOnX();
	bool isActivitySourceOnY();
	bool isActivitySourceOnZ();
	bool isTapSourceOnX();
	bool isTapSourceOnY();
	bool isTapSourceOnZ();
	bool isAsleep();
	
	bool isLowPower();
	void setLowPower(bool state);
	double getRate();
	void setRate(double rate);
	void set_bw(byte bw_code);
	byte get_bw_code();  
	
	
	bool triggered(byte interrupts, int mask);
	
	
	byte getInterruptSource();
	bool getInterruptSource(byte interruptBit);
	bool getInterruptMapping(byte interruptBit);
	void setInterruptMapping(byte interruptBit, bool interruptPin);
	bool isInterruptEnabled(byte interruptBit);
	void setInterrupt(byte interruptBit, bool state);
	
	void getRangeSetting(byte* rangeSetting);
	void setRangeSetting(int val);
	bool getSelfTestBit();
	void setSelfTestBit(bool selfTestBit);
	bool getSpiBit();
	void setSpiBit(bool spiBit);
	bool getInterruptLevelBit();
	void setInterruptLevelBit(bool interruptLevelBit);
	bool getFullResBit();
	void setFullResBit(bool fullResBit);
	bool getJustifyBit();
	void setJustifyBit(bool justifyBit);
	void printAllRegister();
	
private:
	void writeTo(byte address, byte val);
	void readFrom(byte address, int num, byte buff[]);
	void setRegisterBit(byte regAdress, int bitPos, bool state);
	bool getRegisterBit(byte regAdress, int bitPos);  
	byte _buff[6] ;    //6 bytes buffer for saving data read from the device
};
void print_byte(byte val);
#endif

Merci d'essayer de m'aider!

éventuellement je creuserai cela:

   if (fifoCount == 1024) {
      mpu.resetFIFO();
      mpu2.resetFIFO();
      Serial.println("FIFO overflow");

vous faites un reset arbitraire sur les 2 MPU si le fifoCount du premier vaut 1024 mais vous ne testez jamais le fifoCount du second.

au minimum je ferai un

   if ((fifoCount == 1024) || (fifoCount2 == 1024)) {

voir idéalement séparément et un reset séparé

vous avez aussi énormément de Serial.print et même à 115200 bauds, vous risquez de saturer le buffer de 64 caractères qui rend ensuite les appels à Serial.print bloquants, ce qui ajoute au risque de saturer le buffer du MPU

donc je serai tenté de commenter le Serial.print pour vérifier si ça change le comportement

Ok pour le serial.print, je vais tester ça. En revanche le second MPU ne me donne pas d’acquisition si j’utilise la commande getFIFOcount avec lui. Je pense que la gestion du compte FIFO ne fonctionne que pour le mpu auquel je demande les Yaw Pitch Roll, le second utilise une autre bibliothèque et me donne les donnée gyro/acceleromètre combiné. J’ai déja essayé de demander le Yaw Pitch Roll aux deux acceleromètre mais sans résultat. Le premier fait ses acquisitions normalement et le second me renvoi quelque chose du genre ; 180, nan, nan. Voici le code :

#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#include "Wire.h"
#include <math.h>
#include<Servo.h>
MPU6050 mpu(0x68); //AD0=Low
MPU6050 mpu2(0x69); //AD0=High

Servo myservo; 
Servo myservo2;
Servo myservo3;
Servo myservo4;

//MPU control/status vars 
bool dmpReady = false; 
bool dmp2Ready = false; 
uint8_t mpuIntStatus;
uint8_t mpuIntStatus2;//actual interrupt status byte 
uint8_t deviceStatus;
uint8_t deviceStatus2;//device status , 0 = success , 
uint16_t packetSize; //expected DMP packet size (defult 42) -- ?
uint16_t packetSize2;
uint16_t fifoCount; //count of all bytes currently in FIFO
uint16_t fifoCount2;
uint8_t fifoBuffer[64]; // FIFO buffer storage 
uint8_t fifoBuffer2[64];

// orientation/motion vars
Quaternion q;
Quaternion q2;// [w, x, y, z]         quaternion container
VectorInt16 aa;         // [x, y, z]            accel sensor measurements
VectorInt16 aa2;
VectorInt16 aaReal;     // [x, y, z]            gravity-free accel sensor measurements
VectorInt16 aaReal2;
VectorInt16 aaWorld;    // [x, y, z]            world-frame accel sensor measurements
VectorInt16 aaWorld2;
VectorFloat gravity;    // [x, y, z]            gravity vector
VectorFloat gravity2;
float euler[3];         // [psi, theta, phi]    Euler angle container
float euler2[3];
float ypr[3];    // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector
float zqs[3]; 
float yaw, pitch, roll;
float yaw2, pitch2, roll2;
float yaw0, pitch0, roll0;
float yaw02, pitch02, roll02;
volatile bool mpuInterrupt = false;
volatile bool mpu2Interrupt = false;

void DivideAxis(){
  
  yaw = (ypr[0] * 180/M_PI);
  pitch = (ypr[1] * 180/M_PI);
  roll = (ypr[2] * 180/M_PI);

  yaw2 = (zqs[0] * 180/M_PI);
  pitch2 = (zqs[1] * 180/M_PI);
  roll2 = (zqs[2] * 180/M_PI);
}

void SetInitAxisValue() {
    yaw0 = yaw;
    pitch0 = pitch;
    roll0 = roll;  

    yaw02 = yaw2;
    pitch02 = pitch2;
    roll02 = roll2;
}

void setup() {

    myservo.attach(11);
    myservo2.attach(10);
    myservo3.attach(9);
    myservo4.attach(6);

  //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

  //initialize the mpu 
  Serial.println("Call MPU6050 Lib to initialize devices...");
  mpu.initialize();
  mpu2.initialize();//initialize I2C device by using MPU6050 library 

  //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() ? F("MPU6050 2 connection test successed ") : F("MPU6050 2 connection test failed"));

//  //wait for begin , uncomment if we need wait user interruption
//  Serial.println("Press any button to begin");
//  while (Serial.available() && Serial.read()); // empty buffer
//  while (!Serial.available());                 // wait for data
//  while (Serial.available() && Serial.read()); // empty buffer again


  //load and configure DMP 
  Serial.println("initializing DMP"); 
  deviceStatus = mpu.dmpInitialize(); //use MPU6050 library to inilisalize the dmp 
  deviceStatus2 = mpu2.dmpInitialize();
 //feed offsets 
  mpu.setXGyroOffset(220);
  mpu.setYGyroOffset(76);
  mpu.setZGyroOffset(-85);
  mpu.setZAccelOffset(1788); // 1688 factory default for my test chip

  mpu2.setXGyroOffset(220);
  mpu2.setYGyroOffset(76);
  mpu2.setZGyroOffset(-85);
  mpu2.setZAccelOffset(1788);
  
  //make sure it works 
  if ((deviceStatus == 0)||(deviceStatus2 == 0)) {
    Serial.println("DMP initialization success, now enable DMP for use");
    Serial.println("DMP 2 initialization success, now enable DMP for use");
    //turn on DMP 
    mpu.setDMPEnabled(true); //use MPU6050 library to enable DMP)
    mpu2.setDMPEnabled(true);
    //wait for first interrup . currently just leave it false automatically 
    mpuInterrupt == false; 
    mpu2Interrupt == false; 
    //let main llop know it's ok to use DMP, set dmpRead flag to ture 
    dmpReady = true;
    dmp2Ready = true;
    Serial.println("DMP is ready to use.");
    Serial.println("DMP 2 is ready to use.");
    //get expected DMP packet size for later comparison 
    packetSize = mpu.dmpGetFIFOPacketSize();
    packetSize = mpu2.dmpGetFIFOPacketSize();
    
  } else {
   //ERROR! , device status !=0 when initializing DMP
    Serial.print("DMP initialization failed when using MPU6050 library:");
    if ((deviceStatus == 1)||(deviceStatus2 == 1)) { 
       Serial.println(" intial memory load failed");
       Serial.println(" intial memory 2 load failed");
       } else if ((deviceStatus == 2)||(deviceStatus2 == 2)){ 
            Serial.println(" failed to update DMP configuration");
            Serial.println(" failed to update DMP 2 configuration");
            } else {
               Serial.print(" unknow error with code: ");
               Serial.println(deviceStatus);
               Serial.println(deviceStatus2);
}
}
}



int ticket = 1;
void printOnlyOnce (String message){
  if (ticket == 1){
   Serial.println(message);
   ticket = 0 ;
  } else {
    return;
  }
}

void loop() {
  //if DMP not ready don't do anything 
  if ((!dmpReady)||(!dmp2Ready)) {
    printOnlyOnce("MAIN LOOP: DMP disabled");
    return;
  } else {
    //testing overflow 
    if ((fifoCount == 1024)||(fifoCount2 == 1024)) {
      mpu.resetFIFO(); 
      mpu2.resetFIFO();     
      Serial.println("FIFO overflow");
    } else {

    //wait for enough avaliable data length
    while ((fifoCount < packetSize)||(fifoCount2 < packetSize2)) {
      //waiting until get enough
      fifoCount = mpu.getFIFOCount();
      fifoCount2 = mpu2.getFIFOCount();
    }
    
    //read this packet from FIFO buffer 
    mpu.getFIFOBytes(fifoBuffer,packetSize);
    mpu2.getFIFOBytes(fifoBuffer2,packetSize2);



    //track FIFO count here is more then one packeage avalible 

    //reset fifo count 
    fifoCount -= packetSize ;
    fifoCount2 -= packetSize2 ;
    //Serial.println(fifoCount);

    if ((fifoCount > 2)||(fifoCount2 > 2)) {
    mpu.resetFIFO();
    mpu2.resetFIFO();
    }
    DivideAxis();
    SetInitAxisValue();
    //display stage 
                // display Euler angles in degrees
            mpu.dmpGetQuaternion(&q, fifoBuffer);
            mpu.dmpGetGravity(&gravity, &q);
            mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);

            mpu2.dmpGetQuaternion(&q2, fifoBuffer2);
            mpu2.dmpGetGravity(&gravity2, &q2);
            mpu2.dmpGetYawPitchRoll(zqs, &q2, &gravity2);

            Serial.print("ypr\t");
            Serial.print("zqs\t");
            Serial.print(yaw); Serial.print("\t");
            Serial.print(pitch); Serial.print("\t");
            Serial.print(roll); Serial.print("\t");
            Serial.print(yaw2); Serial.print("\t");
            Serial.print(pitch2); Serial.print("\t");
            Serial.println(roll2); 
            
          /*  myservo.write(pitch2);
            myservo2.write(roll);
            myservo3.write(pitch);
            myservo4.write(yaw);*/

  }
  }
}

Bonjour,

Il y a au moins ce problème:

    packetSize = mpu.dmpGetFIFOPacketSize();
    packetSize = mpu2.dmpGetFIFOPacketSize();

alors que ça devrait être:

    packetSize = mpu.dmpGetFIFOPacketSize();
    packetSize2 = mpu2.dmpGetFIFOPacketSize();

le second utilise une autre bibliothèque et me donne les donnée gyro/acceleromètre combiné.

euh vous utilisez la même bibliothèque vu que vous les déclarez comme MPU6050 tous les 2

MPU6050 mpu(0x68); //AD0=Low
MPU6050 mpu2(0x69);

sinon je me souviens avoir eu des soucis avec Fastwire - avez vous essayé avec la librairie Wire de base? (il faudra faire que I2CDEV_IMPLEMENTATION = I2CDEV_ARDUINO_WIRE)

Bien vu 3Sigma, c'est peut être une faute de frappe qui pourrait causer bien des problèmes! Je teste...

Et oui JML, pardon, en effet j'utilise la mème bibliothèque c'est ma faute. J'ai un autre bout de code qui utilise

mpu6050.h et j'ai confondu... Je vais réessayer très attentivement de suivre vos conseils y compris pour la

librairie wire.

Pensez vous qu'en ne faisant qu'un serial.print toutes les 100 acquisitions ( au hasard ) cela

aiderai mon buffer? Et que pensez vous de la solution kinect, serai ce viable? Si j'en demande trop dite le moi,

c'est déjà très sympa de m’aider, merci les gars!

Pensez vous qu'en ne faisant qu'un serial.print toutes les 100 acquisitions ( au hasard ) cela aiderai mon buffer?

oui ce serait déjà ça - chaque caractère compte :slight_smile:
à 115200 vous pouvez envoyer 11 caractère par milliseconds en gros - donc regardez combien vous imprimez de caractère par ligne de reporting et la durée d'une boucle et il ne faut pas demander d'imprimer si le buffer risque d'être plein (en fait avant d'imprimer vous pourriez faire un test avec la fonction Serial.AvailableForWrite() qui vous dit combien vous pouvez encore en mettre dans le buffer avant de bloquer... mais bon appeler cette fonction prend un peu de temps..

l'idéal c'est de ne pas balancer plus de 64 caractères et attendre qu'ils soient tous partis avant d'en remettre.

Et que pensez vous de la solution kinect, serai ce viable? Si j'en demande trop dite le moi,

Oui bien sûr - ça a été déjà fait de nombreuses fois. si vous lisez l'anglais allez voir cela

Merci beaucoup pour votre aide, je ne comprenais pas bien le coup des 64 caractères et maintenant je me lèverai moins bête demain. J'irai lire le topic que vous me conseillez. Je donnerai des nouvelles demain après essai. Merci!

Bonsoir, pardon de ne pas avoir donné de résultat plus tôt. Alors j’ai testé plusieurs de vos suggestions :

-3Sigma tu avais raison, cette ligne de code dans mon “code plan B” était problématique, ce qui m’ouvre de

nouvelles options. En revanche ce n’est toujours pas très stable…

  • JML j’ai appliqué plusieurs de tes conseils ; commenté les serial.print, testé un compteur du genre

" if printcpt == MxPRINT" , essayé la librairie “wire” et non “fast wire”. Pour l’instant rien n’y fait mais vos

conseils à tout deux m’ont donné des idées qu’il me reste à tester.

Il me semble que, l’entreprise a tendance à planter plus vite lorsque les servo moteur sont mis sous

tension. Pourtant à part les pin PWM les servo sont alimenté indépendamment par une alim suffisante ( 5v,

20A). De mème les connexions ne sont pas à incrimminer puisque lorsque je fait plusieurs redémarrages

successif, les accéléro reprennent leurs acquisitions sans que j’ai à toucher au câblage…

Demain ne sera pas une journée productive car j’ai des obligations, mais je pense sérieusement à tester le

kinect qui doit être la seul bonne façon de faire.

Je donnerai des nouvelles au fur et à mesure de ma progression. Messieurs merci encore.

P.S : si on se tutoyai?