Drone stabilization with MPU-6065, is this code is a good start ?

Hello,

I am actually trying to build a simple drone stabilization and “controller” with 4 ESC and a MPU-6065 as gyro and accelerometer.

The code is quite simple, there is a desired Z speed and desired X/Y/Z rotation, if the drone do not have desired rotation the speed of motors is adapted.

But I don’t know if this code will work at all, do you think it will ?
Do you have any advice for me ?

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

#define AMOTOR1 0x01 // top left
#define AMOTOR2 0x02 // top right
#define AMOTOR3 0x03 // bottom left
#define AMOTOR4 0x04 // bottom right
const int MPU=0x68;  // I2C address of the MPU-6050
int16_t AcX,AcY,AcZ,GyX,GyY,GyZ;

int16_t Desired_AcX = 0,Desired_AcY = 0,Desired_AcZ = 0,Desired_GyX = 0,Desired_GyY = 0,Desired_GyZ = 0; // Desired values
int16_t motor1_intensity = 0, motor2_intensity = 0, motor3_intensity = 0, motor4_intensity = 0;
Servo motor1, motor2, motor3, motor4;

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

  motor1.attach(AMOTOR1);
  motor2.attach(AMOTOR2);
  motor3.attach(AMOTOR3);
  motor4.attach(AMOTOR4);

  Serial.begin(9600);
}

void motor_control(){

  Wire.beginTransmission(MPU);
  Wire.write(0x3B);  // starting with register 0x3B (ACCEL_XOUT_H)
  Wire.endTransmission(false);
  Wire.requestFrom(MPU,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)
  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)
  
  // Altitude regulation
  if(Desired_AcZ > AcZ){
    motor1_intensity += 1;
    motor2_intensity += 1;
    motor3_intensity += 1;
    motor4_intensity += 1;
  }else if(Desired_AcZ < AcZ){
    motor1_intensity -= 1;
    motor2_intensity -= 1;
    motor3_intensity -= 1;
    motor4_intensity -= 1;
  }

  // X axis regulation
  if(Desired_GyX > GyX){
    if(Desired_AcZ >= AcZ){
      motor1_intensity += 1;
      motor2_intensity += 1;
    }else{
      motor3_intensity -= 1;
      motor4_intensity -= 1;
    }
    modification = true;
  }else if(Desired_GyX < GyX){

    if(Desired_AcZ < AcZ){
      motor1_intensity -= 1;
      motor2_intensity -= 1;
    }else{
      motor3_intensity += 1;
      motor4_intensity += 1;
    }
  }

  // Y axis regulation
  if(Desired_GyY < GyY){
    if(Desired_AcZ >= AcZ){
      motor2_intensity += 1;
      motor4_intensity += 1;
    }else{
      motor1_intensity -= 1;
      motor3_intensity -= 1;
    }
  }else if(Desired_GyY > GyY){
    if(Desired_AcZ >= AcZ){
      motor1_intensity += 1;
      motor3_intensity += 1;
    }else{
      motor2_intensity -= 1;
      motor4_intensity -= 1;
    }
  }

  // Z axis regulation
  if(Desired_GyZ < GyZ){
    
    if(Desired_AcZ >= AcZ){
      motor2_intensity += 1;
      motor3_intensity += 1;
    }else{
      motor1_intensity -= 1;
      motor4_intensity -= 1;
    }
  }else if(Desired_GyZ > GyZ){
    if(Desired_AcZ >= AcZ){
      motor1_intensity += 1;
      motor4_intensity += 1;
    }else{
      motor2_intensity -= 1;
      motor3_intensity -= 1;
    }
    
  }

   // check min / max power
  if(motor1_intensity > 179)
    motor1_intensity = 179;
  else if(motor1_intensity < 0)
    motor1_intensity = 0;

  if(motor2_intensity > 179)
    motor2_intensity = 179;
  else if(motor2_intensity < 0)
    motor2_intensity = 0;

  if(motor3_intensity > 179)
    motor3_intensity = 179;
  else if(motor3_intensity < 0)
    motor3_intensity = 0;

  if(motor4_intensity > 179)
    motor4_intensity = 179;
  else if(motor4_intensity < 0)
    motor4_intensity = 0;

  motor1.write(motor1_intensity);
  motor2.write(motor2_intensity);
  motor3.write(motor3_intensity);
  motor4.write(motor4_intensity);

  Serial.println('--------------');
  Serial.println(motor1_intensity);
  Serial.println(motor2_intensity);
  Serial.println(motor3_intensity);
  Serial.println(motor4_intensity);
  Serial.println('--------------');

}

void loop() {
  
  motor_control();

}

Thanks in advance :slight_smile:

Try it and let us know.

i ordered my pieces but it will take a while to come and i have to finish my exam first, it’s why i wanted to have some advice before ^^