Problèmes servomoteur et mpu.getEvent()

International Français
1

Bonjour,

Je veux faire un système qui compense des tremblements subis par une cuillère.
J'ai donc un mpu (mpu1) qui me sert à calculer l'angle auquel je penche ma board et un servomoteur qui renvoie l'angle inverse pour que la cuillère au bout du servo reste droite (voir image).

image
image734×409 62.9 KB

J'ai un deuxième mpu (mpu2) qui me permet de calculer l'angle au bout de la cuillère (qui doit donc être plus petit que celui de mpu1).

Voici le code que j'ai fait :

// ######## Déclaration des bibliothèques ########
#include <Wire.h>
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Servo.h>


// ######## Initialisation des variables ########
Adafruit_MPU6050 mpu1;
Adafruit_MPU6050 mpu2;
Servo servo;

float angx1, angy1, angz1;
float angx2, angy2, angz2;
float acc_total_vector1, acc_total_vector2;

int val;
int preVal;
int temps;


// ######## Initialisation du programme ########
void setup() {
  Serial.begin(9600);
  servo.attach(6);
  Serial.println(F("Initialize System"));
  if (!mpu1.begin(0x68) && !mpu2.begin(0x69)) {
    Serial.println("Failed to find MPU6050 chip");
  }

  mpu1.setAccelerometerRange(MPU6050_RANGE_16_G);
  mpu2.setAccelerometerRange(MPU6050_RANGE_16_G);
  mpu1.setGyroRange(MPU6050_RANGE_250_DEG);
  mpu2.setGyroRange(MPU6050_RANGE_250_DEG);
  mpu1.setFilterBandwidth(MPU6050_BAND_21_HZ);
  mpu2.setFilterBandwidth(MPU6050_BAND_21_HZ);
}


void loop() {
  // ######## Lecture des données ########
  sensors_event_t a1, g1, temp1;
  mpu1.getEvent(&a1, &g1, &temp1);

  // Calcul du vecteur d'accélération 3D total : √(X² + Y² + Z²)
  acc_total_vector1 = sqrt(pow(a1.acceleration.x, 2) + pow(a1.acceleration.y, 2) + pow(a1.acceleration.z, 2));

  // Pour vérifier que la valeur est comprise dans [-1;+1]
  if (abs(a1.acceleration.x) < acc_total_vector1) {
    angx1 = asin((float)a1.acceleration.y / acc_total_vector1) * (180 / PI);
  }

  /*
    if (abs(a1.acceleration.y) < acc_total_vector1) {
      angy1 = asin((float)a1.acceleration.x / acc_total_vector1) * (180 / PI);
    }
  */

  sensors_event_t a2, g2, temp2;
  mpu2.getEvent(&a2, &g2, &temp2);

  acc_total_vector2 = sqrt(pow(a2.acceleration.x, 2) + pow(a2.acceleration.y, 2) + pow(a2.acceleration.z, 2));

  if (abs(a2.acceleration.x) < acc_total_vector2) {
    angx2 = asin((float)a2.acceleration.y / acc_total_vector2) * (180 / PI);
  }

  /*
    if (abs(a2.acceleration.y) < acc_total_vector2) {
    angy2 = asin((float)a2.acceleration.x / acc_total_vector2) * (180 / PI);
    }
  */

  // ######## Données pour le servomoteur ########
  val = angx1;
  if (val != preVal) {
    servo.write(val);
    preVal = val;
  }


  // ######## Afficher les valeurs ########
  Serial.print(millis());
  Serial.print("/");
  Serial.print(angx1);
  Serial.print("/");
  Serial.println(angx2);

  delay(1);
}

Je rencontre plusieurs problèmes :

  • angx1 varie sur [-90;+90] mais le servomoteur ne varie que sur [0;+90]
  • le moteur ne répond pas assez vite pour l'utiliser sur des tremblements (j'ai testé avec delay(1) et delay(0), et c'est pareil) ; je ne peux que l'utiliser sur des mouvements lents
  • lorsque j'affiche les valeurs angx1 et angx2 dans le moniteur série, angx2 est égal à angx1

Est-ce que quelqu'un peut m'aider svp ?

Merci d'avance !

2

This topic was automatically closed 180 days after the last reply. New replies are no longer allowed.