Self-Stabilizing Platform with MPU6050 problem

Hi guys,
I am new to the forum and here I have really found a world to discover.
I am trying to replicate the project https://howtomechatronics.com/projects/diy-arduino-gimbal-self-stabilizing-platform/
but without using the interrupt.
I found this code in a discussion on the forum

#include<Wire.h>
#define SDA 22//for esp32, commented out if you're not using this board

#define SCL 21//for esp32, commented out if you're not using this board


const int MPU_addr=0x68; // I2C address of the MPU-6050
int16_t AcX,AcY,AcZ,Tmp,GyX,GyY,GyZ;
float GyrX=0;
float GyrY=0;
float GyrZ=0;
float dt;
unsigned long MillisOld;

void setup(){
Wire.begin(SDA, SCL);//for esp32, commented out if you're not using this board
//Wire.begin();//for other board
Wire.beginTransmission(MPU_addr);
Wire.write(0x6B); // PWR_MGMT_1 register
Wire.write(0); // set to zero (wakes up the MPU-6050)
Wire.endTransmission(true);
MillisOld = millis();
Serial.begin(115200);
}
void loop(){
Wire.beginTransmission(MPU_addr);
Wire.write(0x3B); // starting with register 0x3B (ACCEL_XOUT_H)
Wire.endTransmission(false);
Wire.requestFrom(MPU_addr,14,true); // request a total of 14 registers

AcX=Wire.read()<<8|Wire.read(); // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)
AcY=Wire.read()<<8|Wire.read(); // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
AcZ=Wire.read()<<8|Wire.read(); // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
Tmp=Wire.read()<<8|Wire.read(); // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L)
GyX=Wire.read()<<8|Wire.read(); // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
GyY=Wire.read()<<8|Wire.read(); // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
GyZ=Wire.read()<<8|Wire.read(); // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)
GyX=GyX/131.;
GyY=GyY/131.;
GyZ=GyZ/131.;

dt = (millis()-MillisOld)/1000.;
MillisOld = millis();
GyrX = (GyrX + GyX*dt);
GyrY = (GyrY + GyY*dt);
GyrZ = (GyrZ + GyZ*dt);
/*
Serial.print("AcX = "); Serial.print(AcX);
Serial.print(" | AcY = "); Serial.print(AcY);
Serial.print(" | AcZ = "); Serial.print(AcZ);
*/
Serial.print(" | Tmp = "); Serial.print(Tmp/340.00+36.53); //equation for temperature in degrees C from datasheet

Serial.print(" | GyX = "); Serial.print(GyrX);
Serial.print(" | GyY = "); Serial.print(GyrY);
Serial.print(" | GyZ = "); Serial.println(GyrZ);

}

but there's a problem.
The value of GyrY and the other variables is always different, having to check the Servos I can't map the values.
For example, with the MPU6050 resting it marks 0
with MPU6050 at 90 degrees it marks 50
with MPU6050 resting sgna 10 now.
Please can you tell me why.
Thanks

You need to calibrate the gyro, as there are offsets that will cause the integration to drift very rapidly.

Collect several hundred gyro x,y,z readings while the sensor is sitting still on the table, average them, and in subsequent operation, subtract the average from each new axial data point.

Example:

  // collect gyro data for calibration
float gx = 0.0;
float gy = 0.0;
float gz = 0.0;
  for (int i = 0; i < 500; i++) {
    Wire.beginTransmission(MPU_addr);
    Wire.write(0x3B);  // starting with register 0x3B (ACCEL_XOUT_H)
    Wire.endTransmission(false);
    Wire.requestFrom(MPU_addr, 14); // request a total of 14 registers
    AcX = Wire.read() << 8 | Wire.read(); // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)
    AcY = Wire.read() << 8 | Wire.read(); // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
    AcZ = Wire.read() << 8 | Wire.read(); // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
    Tmp = Wire.read() << 8 | Wire.read(); // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L)
    GyX = Wire.read() << 8 | Wire.read(); // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
    GyY = Wire.read() << 8 | Wire.read(); // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
    GyZ = Wire.read() << 8 | Wire.read(); // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)
    gx += GyX; gy += GyY; gz += GyZ;
  }
  gx = gx / 500.0;
  gy = gy / 500.0;
  gz = gz / 500.0;

  Serial.print("Gyro offsets ");
  Serial.print(gx);
  Serial.print(", ");
  Serial.print(gy);
  Serial.print(", ");
  Serial.print(gz);
  Serial.println();

The drift is temperature dependent, so using just the gyro, the platform level will drift over just a few minutes.

Keep in mind that the angles for 3D orientation are not additive, and that the order of applying rotations matters. If any of the total integrated angles is much different from zero, the combination of angles will quickly become meaningless.

Thanks bro
This should be the final code

#include<Wire.h>
#include <Servo.h>

Servo servoX;
Servo servoY;
Servo servoZ;
const int MPU_addr=0x68; // I2C address of the MPU-6050
int16_t AcX,AcY,AcZ,Tmp,GyX,GyY,GyZ;
float GyrX=0;
float GyrY=0;
float GyrZ=0;
float dt;
float gx = 0.0;
float gy = 0.0;
float gz = 0.0;
unsigned long MillisOld;

void setup(){
  
  
Wire.beginTransmission(MPU_addr);
Wire.write(0x6B); // PWR_MGMT_1 register
Wire.write(0); // set to zero (wakes up the MPU-6050)
Wire.endTransmission(true);
MillisOld = millis();
Serial.begin(9600);

  for (int i = 0; i < 600; i++) {
    Wire.beginTransmission(MPU_addr);
    Wire.write(0x3B);  // starting with register 0x3B (ACCEL_XOUT_H)
    Wire.endTransmission(false);
    Wire.requestFrom(MPU_addr, 14); // request a total of 14 registers
    AcX = Wire.read() << 8 | Wire.read(); // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)
    AcY = Wire.read() << 8 | Wire.read(); // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
    AcZ = Wire.read() << 8 | Wire.read(); // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
    Tmp = Wire.read() << 8 | Wire.read(); // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L)
    GyX = Wire.read() << 8 | Wire.read(); // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
    GyY = Wire.read() << 8 | Wire.read(); // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
    GyZ = Wire.read() << 8 | Wire.read(); // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)
    gx += GyX; gy += GyY; gz += GyZ;
  }
  
  gx = gx / 600.0;
  gy = gy / 600.0;
  gz = gz / 600.0;

  Serial.print("Gyro offsets ");
  Serial.print(gx);
  Serial.print(", ");
  Serial.print(gy);
  Serial.print(", ");
  Serial.print(gz);
  Serial.println();
  delay(500);
  
  servoX.attach(3);  //White Wire
  servoY.attach(5);  // Green Wire
  servoZ.attach(6);  //Orange  Wire
  delay(100);

}

  
void loop(){
  Wire.beginTransmission(MPU_addr);
Wire.write(0x3B); // starting with register 0x3B (ACCEL_XOUT_H)
Wire.endTransmission(false);
Wire.requestFrom(MPU_addr,14,true); // request a total of 14 registers

AcX=Wire.read()<<8|Wire.read(); // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)
AcY=Wire.read()<<8|Wire.read(); // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
AcZ=Wire.read()<<8|Wire.read(); // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
Tmp=Wire.read()<<8|Wire.read(); // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L)
GyX=Wire.read()<<8|Wire.read(); // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
GyY=Wire.read()<<8|Wire.read(); // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
GyZ=Wire.read()<<8|Wire.read(); // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)
GyX=(GyX-gx)/131.;
GyY=(GyY-gy)/131.;
GyZ=(GyZ-gz)/131.;

dt = (millis()-MillisOld)/1000.;
MillisOld = millis();
GyrX = (GyrX + GyX*dt);
GyrY = (GyrY + GyY*dt);
GyrZ = (GyrZ + GyZ*dt);
/*
Serial.print("AcX = "); Serial.print(AcX);
Serial.print(" | AcY = "); Serial.print(AcY);
Serial.print(" | AcZ = "); Serial.print(AcZ);
*/
Serial.print(" | Tmp = "); Serial.print(Tmp/340.00+36.53); //equation for temperature in degrees C from datasheet

Serial.print(" | GyX = "); Serial.print(GyrX);
  //int servo1Value = map((GyrY), -90, 0, 90, 0);
  //servoX.write(90); //servo0Value
Serial.print(" | GyY = "); Serial.print(GyrY);
  int servo0Value = map((GyrY), -90, 0, 90, 0);
  servoY.write(servo0Value); //servo0Value
Serial.print(" | GyZ = "); Serial.println(GyrZ);
 int servo1Value = map((GyrZ), -90, 90, 180, 0);
 servoZ.write(servo1Value); //servo0Value

}


I thinks that there is a problem, after the calibration the values of GyX, GyY and GyZ are 0, when i Incline the MPU i get the right values but If i put the MPU in 100 degrees of inclination ant then back on a flat surface i don't get the value 0 anymore.
Is the "map((GyrY), -90, 0, 90, 0);" the problem?

If you have any suggestions they're really apreciated

You are using the simplest possible numerical integration, and it is not very accurate.

Is the "map((GyrY), -90, 0, 90, 0);" the problem?

I don't know. What is that line of code supposed to do, and why do you think you need it?

Most people use PID control to level a platform, and there are plenty of projects on line for you to study.

Yes, i want to level a 3 axys platform
I need the map((GyrY), -90, 0, 90, 0); for controlling Servo

If you have any valid link of project that can help me please link

Google "arduino self stabilizing platform" for a large number of valid links.

Ok bro, I probably found a solution, let's have a look

#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps_V6_12.h"
#include "Wire.h"
#include <Servo.h>

///////////////////////////////////   CONFIGURATION   /////////////////////////////
int buffersize = 2000;   //Amount of readings used to average, make it higher to get more precision but sketch will be slower  (default:1000)
int acel_deadzone = 8;   //Acelerometer error allowed, make it lower to get more precision, but sketch may not converge  (default:8)
int giro_deadzone = 1;   //Giro error allowed, make it lower to get more precision, but sketch may not converge  (default:1)

// the MPU
MPU6050 mpu;

// DMP buffer
uint8_t fifoBuffer[64];

Servo servo0;
Servo servo1;
Servo servo2;

// constants
const float rad_to_deg = 180 / M_PI;

// global variables
float min_roll = 0;
float max_roll = 0;

float yaw, pitch, rolld, correct;
int16_t ax, ay, az, gx, gy, gz;

int mean_ax, mean_ay, mean_az, mean_gx, mean_gy, mean_gz, j = 0;
int ax_offset, ay_offset, az_offset, gx_offset, gy_offset, gz_offset;

void setup()
{
  Wire.begin();
  Wire.setClock(400000);

  Serial.begin(9600);
  servo0.attach(3);
  servo1.attach(5);
  servo2.attach(6);
  // initialize the MPU
  Serial.println(F("Initializing MPU..."));
  mpu.initialize();

  Serial.println(F("Initializing DMP..."));
  uint8_t mpuStatus = mpu.dmpInitialize();



  if (mpuStatus == 0) {
    Serial.println("\nMPU6050 Calibration Sketch");
    delay(2000);
    Serial.println("\nYour MPU6050 should be placed in horizontal position, with package letters facing up. \nDon't touch it until you see a finish message.\n");
    delay(3000);

    // verify connection
    Serial.println(mpu.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");
    delay(1000);
    // reset offsets
    mpu.setXAccelOffset(0);
    mpu.setYAccelOffset(0);
    mpu.setZAccelOffset(0);
    mpu.setXGyroOffset(0);
    mpu.setYGyroOffset(0);
    mpu.setZGyroOffset(0);

    Serial.println("\nReading sensors for first time...");
    meansensors();
    mpuStatus++;
    delay(1000);
  }
  if (mpuStatus == 1) {
    Serial.println("\nCalculating offsets...");
    calibration();
    mpuStatus++;
    delay(1000);
  }

  if (mpuStatus == 2) {
    meansensors();
    Serial.println("\nFINISHED!");
    Serial.print("\nSensor readings with offsets:\t");
    Serial.print(mean_ax);
    Serial.print("\t");
    Serial.print(mean_ay);
    Serial.print("\t");
    Serial.print(mean_az);
    Serial.print("\t");
    Serial.print(mean_gx);
    Serial.print("\t");
    Serial.print(mean_gy);
    Serial.print("\t");
    Serial.println(mean_gz);
    Serial.print("Your offsets:\t");
    Serial.print(ax_offset);
    Serial.print("\t");
    Serial.print(ay_offset);
    Serial.print("\t");
    Serial.print(az_offset);
    Serial.print("\t");
    Serial.print(gx_offset);
    Serial.print("\t");
    Serial.print(gy_offset);
    Serial.print("\t");
    Serial.println(gz_offset);
    Serial.println("\nData is printed as: acelX acelY acelZ giroX giroY giroZ");
    Serial.println("Calibrando con i valori ottenuti");
    mpu.setXAccelOffset(ax_offset);
    mpu.setYAccelOffset(ay_offset);
    mpu.setZAccelOffset(az_offset);
    mpu.setXGyroOffset(gx_offset);
    mpu.setYGyroOffset(gy_offset);
    mpu.setZGyroOffset(gz_offset);
    mpu.setDMPEnabled(true);

    loop();

  }
}

void loop() {
  if (mpu.dmpGetCurrentFIFOPacket(fifoBuffer)) {
    Quaternion q;
    mpu.dmpGetQuaternion(&q, fifoBuffer);

    VectorFloat gravity;
    mpu.dmpGetGravity(&gravity, &q);

    float ypr[3];
    mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);

    const float roll = ypr[2] * rad_to_deg;
    /*min_roll = min(roll, min_roll);
      max_roll = max(roll, max_roll);
    */
    yaw = (ypr[0] * rad_to_deg);
    pitch = (ypr[1] * rad_to_deg);
    rolld = (roll);
    if (j <= 500) {
      correct = yaw; // Yaw starts at random value, so we capture last value after 300 readings
      j++;
    }
    // After 500 readings
    else {
      yaw = yaw - correct; // Set the Yaw to 0 deg - subtract  the last random Yaw value from the currrent value to make the Yaw 0 degrees
      Serial.print(yaw , 1);
      Serial.print(" ");
      int servo0Value = map(yaw, -90, 90, 0, 180);
      Serial.print(pitch, 1);
      int servo1Value = map(pitch, -90, 90, 0, 180);
      Serial.print(" ");
      Serial.println(rolld, 1);
      int servo2Value = map(rolld, -90, 90, 180, 0);

      servo2.write(servo0Value);
      servo1.write(servo1Value);
      servo0.write(servo2Value);
    }
  }
}

void meansensors() {
  long i = 0, buff_ax = 0, buff_ay = 0, buff_az = 0, buff_gx = 0, buff_gy = 0, buff_gz = 0;

  while (i < (buffersize + 101)) {
    // read raw accel/gyro measurements from device
    mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

    if (i > 100 && i <= (buffersize + 100)) { //First 100 measures are discarded
      buff_ax = buff_ax + ax;
      buff_ay = buff_ay + ay;
      buff_az = buff_az + az;
      buff_gx = buff_gx + gx;
      buff_gy = buff_gy + gy;
      buff_gz = buff_gz + gz;
    }
    if (i == (buffersize + 100)) {
      mean_ax = buff_ax / buffersize;
      mean_ay = buff_ay / buffersize;
      mean_az = buff_az / buffersize;
      mean_gx = buff_gx / buffersize;
      mean_gy = buff_gy / buffersize;
      mean_gz = buff_gz / buffersize;
    }
    i++;
    delay(2); //Needed so we don't get repeated measures
  }
}

void calibration() {
  ax_offset = -mean_ax / 8;
  ay_offset = -mean_ay / 8;
  az_offset = (16384 - mean_az) / 8;

  gx_offset = -mean_gx / 4;
  gy_offset = -mean_gy / 4;
  gz_offset = -mean_gz / 4;
  while (1) {
    int ready = 0;
    mpu.setXAccelOffset(ax_offset);
    mpu.setYAccelOffset(ay_offset);
    mpu.setZAccelOffset(az_offset);

    mpu.setXGyroOffset(gx_offset);
    mpu.setYGyroOffset(gy_offset);
    mpu.setZGyroOffset(gz_offset);

    meansensors();
    Serial.println("...");

    if (abs(mean_ax) <= acel_deadzone) ready++;
    else ax_offset = ax_offset - mean_ax / acel_deadzone;

    if (abs(mean_ay) <= acel_deadzone) ready++;
    else ay_offset = ay_offset - mean_ay / acel_deadzone;

    if (abs(16384 - mean_az) <= acel_deadzone) ready++;
    else az_offset = az_offset + (16384 - mean_az) / acel_deadzone;

    if (abs(mean_gx) <= giro_deadzone) ready++;
    else gx_offset = gx_offset - mean_gx / (giro_deadzone + 1);

    if (abs(mean_gy) <= giro_deadzone) ready++;
    else gy_offset = gy_offset - mean_gy / (giro_deadzone + 1);

    if (abs(mean_gz) <= giro_deadzone) ready++;
    else gz_offset = gz_offset - mean_gz / (giro_deadzone + 1);

    if (ready == 6) break;
  }
}

it works, i gots the offset values integrating the calibration ini found on this site.
if you have any advertisment it would be apreciated

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