Servomotor and mpu.getEvent() problems

Projects General Guidance
1

Hi !

First of all, sorry if I'm not in the good category, I'm new here :slight_smile:

I want to make a system that compensates for tremors suffered by a spoon.
So I have an mpu (mpu1) which is used to calculate the angle at which I lean my board and a servomotor which returns the opposite angle so that the spoon at the end of the servo remains straight (see image).

image
image734×409 63.2 KB

I have a second mpu (mpu2) which allows me to calculate the angle at the end of the spoon (which must therefore be smaller than that of mpu1), so that I can create a chart with the entry angle and the exit angle

Here is the code I made:

// ######## Declaration of Libraries ########
#include <Wire.h>
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Servo.h>


// ######## Variable initialization ########
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;


// ######## Program initialization ########
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() {
  // ######## Reading data ########
  sensors_event_t a1, g1, temp1;
  mpu1.getEvent(&a1, &g1, &temp1);

  // Calculation of the total 3D acceleration vector : √(X² + Y² + Z²)
  acc_total_vector1 = sqrt(pow(a1.acceleration.x, 2) + pow(a1.acceleration.y, 2) + pow(a1.acceleration.z, 2));

  // To verify that the value is within [-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);
    }
  */

  // ######## Data for the servo motor ########
  val = angx1;
  if (val != preVal) {
    servo.write(val);
    preVal = val;
  }


  // ######## Show values ########
  Serial.print(millis());
  Serial.print("/");
  Serial.print(angx1);
  Serial.print("/");
  Serial.println(angx2);

  delay(1);
}

I am having several problems:

  • angx1 varies on [-90;+90] but the servo only varies on [0;+90]
  • the engine does not respond fast enough to use it on tremors (I tested with delay(1) and delay(0), and it's the same); I can only use it on slow movements
  • when i show angx1 and angx2 values in serial monitor, angx2 is equal to angx1

Can someone please help me?

Thanks in advance !

2

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