MPU6050 and P gain

qpurr · July 16, 2024, 1:57am

Hello. I'm doing a drone project using ESP32 micro controller and MPU650 accelerator gyrometer. So everytime I turned on the drone which throttle is set to 400, the motor speed of the anti clockwise propellor(B and D) always became lower than the clockwise one(A and C) even when it is placed on a flat surface. I tried to see if there are something related to the code which i guess might due to the P gain or the balancing of roll, pitch and yaw, but i have no exact idea which part should i look into. Is it MPU 6050 problem? Anyone could help?

The motor speed

10:51:40.843 -> dt=0.000532: A = 460.3: B = 339.6: C = 460.1: D = 340.0
10:51:40.930 -> dt=0.000534: A = 460.3: B = 339.4: C = 460.2: D = 340.0

The code

//For PS4 Controller
/*#include <PS4Controller.h>
  #include <Wire.h>
  #include "esp_bt_main.h"
  #include "esp_bt_device.h"
  #include "esp_gap_bt_api.h"
  #include "esp_err.h"
*/

#include <Wire.h>

#define MOTOR_A 33
#define MOTOR_B 32
#define MOTOR_C 25
#define MOTOR_D 26
#define CHANNEL_A 0
#define CHANNEL_B 1
#define CHANNEL_C 2
#define CHANNEL_D 3
#define MOTOR_FREQ 5000 //20000
#define MOTOR_RESOLUTION 10

double throttle = 400;
//double Kp = 10.0, Ki = 4.0, Kd = 0.0; //角度のPI制御
//double  Krp = 2.0, Kri = 1.0;//角速度のPI制御 チューニングの順番 Kp → Krp → Kri → Ki
double tAngleX = 0.0, tAngleY = 0.0, tAngleZ = 0.0;

unsigned long lastTimeStamp = 0;
#define EVENTS 0
#define BUTTONS 0
#define JOYSTICKS 1
#define SENSORS 0

void setup() {
  Serial.begin(115200);
//  PS4.attach(notify);
//  PS4.attachOnConnect(onConnect);
//  PS4.attachOnDisconnect(onDisConnect);
//  PS4.begin();
//  removePairedDevices(); // This helps to solve connection issues
//  Serial.print("This device MAC is: ");
//  printDeviceAddress();
//  Serial.println("");

  Wire.begin();
  Wire.setClock(400000);

  Wire.beginTransmission(0x68);
  Wire.write(0x6b);
  Wire.write(0x0);
  Wire.endTransmission(true);
}
  /*ledcAttachPin(MOTOR_A, CHANNEL_A);
  ledcAttachPin(MOTOR_B, CHANNEL_B);
  ledcAttachPin(MOTOR_C, CHANNEL_C);
  ledcAttachPin(MOTOR_D, CHANNEL_D);

  ledcSetup(CHANNEL_A, MOTOR_FREQ, MOTOR_RESOLUTION);
  ledcSetup(CHANNEL_B, MOTOR_FREQ, MOTOR_RESOLUTION);
  ledcSetup(CHANNEL_C, MOTOR_FREQ, MOTOR_RESOLUTION);
  ledcSetup(CHANNEL_D, MOTOR_FREQ, MOTOR_RESOLUTION);

  ledcWrite(CHANNEL_A, 0);
  ledcWrite(CHANNEL_B, 0);
  ledcWrite(CHANNEL_C, 0);
  ledcWrite(CHANNEL_D, 0);
  }
*/
  /*void removePairedDevices() {
  uint8_t pairedDeviceBtAddr[20][6];
  int count = esp_bt_gap_get_bond_device_num();
  esp_bt_gap_get_bond_device_list(&count, pairedDeviceBtAddr);
  for (int i = 0; i < count; i++) {
    esp_bt_gap_remove_bond_device(pairedDeviceBtAddr[i]);
  }
  }

  void printDeviceAddress() {
  const uint8_t* point = esp_bt_dev_get_address();
  for (int i = 0; i < 6; i++) {
    char str[3];
    sprintf(str, "%02x", (int)point[i]);
    Serial.print(str);
    if (i < 5) {
      Serial.print(":");
    }
  }
  }

  //void onConnect() {
  //  Serial.println("Connected!");
  }

  //void notify() {
  //limit stick is 127
  /*
  if (-PS4.data.analog.stick.ly > 60) throttle = throttle + 0.7; //left fw =throttle(move atas)
  if (-PS4.data.analog.stick.ly < -60) throttle = throttle - 0.7; //leftbw (move bawah)
  if (-PS4.data.analog.stick.lx > 60) tAngleZ = tAngleZ + 0.2; //left r (move z angle atas kanan )
  if (-PS4.data.analog.stick.lx < -60) tAngleZ = tAngleZ - 0.2; //left l
  if (-PS4.data.analog.stick.rx > 60) tAngleX = 7.0; //right fw (x angle move tepi)
  if (-PS4.data.analog.stick.rx < -60) tAngleX = -7.0; //right bw
  if (-PS4.data.analog.stick.rx < 60 && -PS4.data.analog.stick.rx > -60) tAngleX = 0.0; // in btwn no change
  if (-PS4.data.analog.stick.ry > 60) tAngleY = -10.0; //go left?? supposed to be -60 no?
  if (-PS4.data.analog.stick.ry < -60) tAngleY = 10.0; //go right
  if (-PS4.data.analog.stick.ry < 60 && -PS4.data.analog.stick.ry > -60) tAngleY = 0.0;
  if (throttle > 1000) throttle = 1000;
  if (PS4.data.button.circle) throttle = 600;
  if (PS4.data.button.cross) throttle = 0;
  if (PS4.data.button.triangle) throttle = 750;
  if (PS4.data.button.l1 && PS4.data.button.up) Kp += 0.01;
  if (PS4.data.button.l1 && PS4.data.button.down) Kp -= 0.01;
  if (PS4.data.button.l2 && PS4.data.button.up) Ki += 0.01;
  if (PS4.data.button.l2 && PS4.data.button.down) Ki -= 0.01;
  if (PS4.data.button.r1 && PS4.data.button.up) Kd += 0.01;
  if (PS4.data.button.r1 && PS4.data.button.down) Kd -= 0.01;
  if (PS4.data.button.r2 && PS4.data.button.up) Kri += 0.01;
  if (PS4.data.button.r2 && PS4.data.button.down) Kri -= 0.01;

  if (-PS4.data.analog.stick.ly > 60) throttle = throttle + 0.7;
  if (-PS4.data.analog.stick.ly < -60) throttle = throttle - 0.7;
  if (-PS4.data.analog.stick.lx > 60) tAngleY = +10.0; //right
  if (-PS4.data.analog.stick.lx > -60) tAngleY = -10.0; //left
  if (PS4.data.button.circle) tAngleY = 0.0; //balance

  }

  //void onDisConnect() {
  //  Serial.println("Disconnected!");
  }
*/
void loop() {
  Wire.beginTransmission(0x68);
  Wire.write(0x3B);
  Wire.endTransmission(false);
  Wire.requestFrom((uint16_t)0x68, (uint8_t)14, true);

  int16_t AcXH = Wire.read();
  int16_t AcXL = Wire.read();
  int16_t AcYH = Wire.read();
  int16_t AcYL = Wire.read();
  int16_t AcZH = Wire.read();
  int16_t AcZL = Wire.read();
  int16_t TmpH = Wire.read();
  int16_t TmpL = Wire.read();
  int16_t GyXH = Wire.read();
  int16_t GyXL = Wire.read();
  int16_t GyYH = Wire.read();
  int16_t GyYL = Wire.read();
  int16_t GyZH = Wire.read();
  int16_t GyZL = Wire.read();

  int16_t AcX = AcXH << 8 | AcXL;
  int16_t AcY = AcYH << 8 | AcYL;
  int16_t AcZ = AcZH << 8 | AcZL;
  int16_t GyX = GyXH << 8 | GyXL;
  int16_t GyY = GyYH << 8 | GyYL;
  int16_t GyZ = GyZH << 8 | GyZL;

  static int32_t AcXSum = 0, AcYSum = 0, AcZSum = 0;
  static int32_t GyXSum = 0, GyYSum = 0, GyZSum = 0;
  static double AcXOff = 0.0, AcYOff = 0.0, AcZOff = 0.0;
  static double GyXOff = 0.0, GyYOff = 0.0, GyZOff = 0.0;
  static int cnt_sample = 1000;
  if (cnt_sample > 0) {
    AcXSum += AcX; AcYSum += AcY; AcZSum += AcZ;
    GyXSum += GyX; GyYSum += GyY; GyZSum += GyZ;
    cnt_sample --;
    if (cnt_sample == 0) {
      AcXOff = AcXSum / 1000.0;
      AcYOff = AcYSum / 1000.0;
      AcZOff = AcZSum / 1000.0;
      GyXOff = GyXSum / 1000.0;
      GyYOff = GyYSum / 1000.0;
      GyZOff = GyZSum / 1000.0;
    }
    delay(1);
    return;
  }

  double AcXD = AcX - AcXOff;
  double AcYD = AcY - AcYOff;
  double AcZD = AcZ - AcZOff + 16384;

  double GyXD = GyX - GyXOff;
  double GyYD = GyY - GyYOff;
  double GyZD = GyZ - GyZOff;

  static unsigned long t_prev = 0;
  unsigned long t_now = micros();
  double dt = (t_now - t_prev) / 1000000.0;
  t_prev = t_now;

  const float GYROXYZ_TO_DEGREES_PER_SEC = 131;
  double GyXR = GyXD / GYROXYZ_TO_DEGREES_PER_SEC;
  double GyYR = GyYD / GYROXYZ_TO_DEGREES_PER_SEC;
  double GyZR = GyZD / GYROXYZ_TO_DEGREES_PER_SEC;

  static double gyAngleX = 0.0, gyAngleY = 0.0, gyAngleZ = 0.0;
  gyAngleX += GyXR * dt;
  gyAngleY += GyYR * dt;
  gyAngleZ += GyZR * dt;

  const float RADIANS_TO_DEGREES = 180 / 3.14159;
  double AcYZD = sqrt(pow(AcY, 2) + pow(AcZ, 2));
  double AcXZD = sqrt(pow(AcX, 2) + pow(AcZ, 2));
  double acAngleY = atan(-AcXD / AcYZD) * RADIANS_TO_DEGREES;
  double acAngleX = atan(AcYD / AcXZD) * RADIANS_TO_DEGREES;
  double acAngleZ = 0;

  const double ALPHA = 0.96;
  static double cmAngleX = 0.0, cmAngleY = 0.0, cmAngleZ = 0.0;
  cmAngleX = ALPHA * (cmAngleX + GyXR * dt) + (1.0 - ALPHA) * acAngleX;
  cmAngleY = ALPHA * (cmAngleY + GyYR * dt) + (1.0 - ALPHA) * acAngleY;
  cmAngleZ = gyAngleZ;
  //if (throttle <= 0) cmAngleX = cmAngleY = cmAngleZ = 0.0;

  static double tAngleX = 0.0, tAngleY = 0.0, tAngleZ = 0.0;
  double eAngleX = tAngleX - cmAngleX;
  double eAngleY = tAngleY - cmAngleY;
  double eAngleZ = tAngleZ - cmAngleZ;

  /* static double tRateX = 0.0, tRateY = 0.0, tRateZ = 0.0;
    tRateX = Kp * eAngleX;
    tRateY = Kp * eAngleY;
    tRateZ = Kp * eAngleZ;

    double eRateX = tRateX - GyXR;
    double eRateY = tRateY - GyYR;
    double eRateZ = tRateZ - GyZR;
    if (throttle <= 0)eRateX = eRateY = eRateZ = 0.0;

    double BalX = Krp * eRateX;
    double BalY = Krp * eRateY;
    double BalZ = Krp * eRateZ;
  */
  ///////
  double Kp = 1.0;
  double BalX = Kp * eAngleX;
  double BalY = Kp * eAngleY;
  double BalZ = Kp * eAngleZ;
  ////
  /* double ResRateX = 0.0, ResRateY = 0.0, ResRateZ = 0.0;
    ResRateX += Kri * eRateX * dt;
    ResRateY += Kri * eRateY * dt;
    ResRateZ += Kri * eRateZ * dt;
    if (throttle <= 0) ResRateX =  ResRateY =  ResRateZ = 0.0;

    BalX +=  ResRateX;
    BalY +=  ResRateY;
    BalZ +=  ResRateZ;
    if (throttle <= 0) BalX = BalY = BalZ = 0.0;
  */
  /////
  /*
    double Kd=1.0;
    BalX += Kd*-GyXR;
    BalY += Kd*-GyYR;
    BalZ += Kd*-GyZR;
    if (throttle <= 0) BalX = BalY = BalZ = 0.0;
    ////

    //add Ki
    double Ki = 1.0;
    double ResX = 0.0, ResY = 0.0, ResZ = 0.0;
    ResX += Ki * eAngleX * dt;
    ResY += Ki * eAngleY * dt;
    ResZ += Ki * eAngleZ * dt;
    if (throttle <= 0) ResX = ResY = ResZ = 0.0;

    BalX += ResX;
    BalY += ResY;
    BalZ += ResZ;
  
    if(Serial.available()>0){
        while(Serial.available()>0){
          char userInput = Serial.read();
            if(userInput>='0' && userInput<='9'){
                throttle =(userInput - '0')*100;
            }
        }
}
  
  double speedA = throttle + BalX - BalY + BalZ;
  double speedB = throttle - BalX - BalY - BalZ;
  double speedC = throttle - BalX + BalY + BalZ;
  double speedD = throttle + BalX + BalY - BalZ;
  */
    double speedA = throttle - BalX + BalY + BalZ;
    double speedB = throttle + BalX + BalY - BalZ;
    double speedC = throttle + BalX - BalY + BalZ;
    double speedD = throttle - BalX - BalY - BalZ;
 /*
  double iSpeedA = constrain((int)speedA, 0, 1000);
  double iSpeedB = constrain((int)speedB, 0, 1000);
  double iSpeedC = constrain((int)speedC, 0, 1000);
  double iSpeedD = constrain((int)speedD, 0, 1000);

   ledcWrite(CHANNEL_A, iSpeedA);
   ledcWrite(CHANNEL_B, iSpeedB);
   ledcWrite(CHANNEL_C, iSpeedC);
   ledcWrite(CHANNEL_D, iSpeedD);
*/
  //Serial.print(eAngleX); Serial.print("  "); Serial.print(eAngleY); Serial.print("  "); Serial.print(eAngleZ); Serial.println("  ");
  //Serial.print(BalX); Serial.print("  "); Serial.print(BalY); Serial.print("  "); Serial.print(BalZ); Serial.print("  ");
  //Serial.print(iSpeedA); Serial.print("  "); Serial.print(iSpeedB); Serial.print("  "); Serial.print(iSpeedC); Serial.print("  "); Serial.print(iSpeedD); Serial.println("  ");
  //Serial.print(AcX);Serial.print("  ");Serial.print(AcY);Serial.print("  ");Serial.print(AcZ);Serial.print("  ");Serial.print(GyX);Serial.print("  ");Serial.print(GyY);Serial.print("  ");Serial.print(GyZ);Serial.println("  ");
 // Serial.print(cmAngleX); Serial.print("  "); Serial.print(cmAngleY); Serial.print("  "); Serial.print(cmAngleZ); Serial.println(" ");
  //Serial.println(dt,6);

  static int cnt_loop;
  cnt_loop ++;
  if (cnt_loop % 200 != 0) return;

  Serial.printf("dt=%8.6f", dt);
  Serial.printf(": A =%6.1f", speedA);
  Serial.printf(": B =%6.1f", speedB);
  Serial.printf(": C =%6.1f", speedC);
  Serial.printf(": D =%6.1f", speedD);
  Serial.println();
}



/*
  void loop() {
  //if (Kp < 0.0)Kp = 0.0; if (Ki < 0.0)Ki = 0.0; if (Kd < 0.0)Kd = 0.0;
  drone_loop();
  delay(5);
  }*/

qpurr · July 16, 2024, 7:27am

I've figured it out

    double speedA = throttle - BalX + BalY + BalZ;
    double speedB = throttle + BalX + BalY - BalZ;
    double speedC = throttle + BalX - BalY + BalZ;
    double speedD = throttle - BalX - BalY - BalZ;

the speed for B and D should be

double speedA = throttle - BalX + BalY + BalZ;
double speedB = throttle + BalX + BalY + BalZ;
double speedC = throttle + BalX - BalY + BalZ;
double speedD = throttle - BalX - BalY + BalZ;

this seemed to solve the problem but now yaw balancing is not done rightly. Still trying to figure out why though

alto777 · July 16, 2024, 1:26pm

If you are still working to get the algorithm correct, then you haven't come to the hard part.

Google will show you that tuning a quadcopter, by which is meant adjusting the PIDs, is an art mixed with science, and no easy task.

If you watch a few videos, you will see variously clever setups for doing the tuning, and perhaps get an idea about the general process, which is from what I have seen, the way any PID system is adjusted.

If your quadcopter is flying in a fully stabilised mode, it's worse, as getting the stabilization working relies on an inner system that should work well first for rate-orientated control.

It is even a challenge to fly and observe and correct a tune. Quadcopter users have tools like FPV cameras, which can make spotting issues much easier, and on board "black box" recorders that can be viewed after flying to show remaining areas in need of improvement.

Some bad tunes are obvious, some you can see or hear easily, but to truly dial in a tune is something I've always left to others.

a7

Topic		Replies	Views
ESC slowing down after time using Servo library Programming Questions	6	831	May 24, 2022
Drone Spinning counter-clockwise Robotics	3	32	August 11, 2024
ESC Motors, Mechanics, Power and CNC	17	2014	May 6, 2021
Hobbywing 30A ESC speed controller Motors, Mechanics, Power and CNC	17	4411	May 6, 2021
L9110s Speed and direction ESP32 with ps4 controller Motors, Mechanics, Power and CNC	8	1208	March 10, 2024

MPU6050 and P gain

Related Topics