1

I have been trying to create code where my MPU6050 is constantly reading the angle it is at and based on that angle, increase or decrease the motor speed. For example the motor will be off when the sensor is level and maximum speed at 45 degrees, and 22 degrees is roughly half speed.

I have a 5065 200kv brushless motor going to a ESC with a bec which i can attach to the Arduino. I have tried to modify a self balancing drone code that used PID. I took out the PID and tried to do an if-then for if the angle is > 5 < 45 then servo write (anglex36) because the maximum angle is 45 and if i multiply 45 by 36 it will be 1620. It says in the code the maximum rpm is 2000, although mine can go faster, and I will change it later after testing. It is recommended to have the max motor speed be at most 85% motor power to prevent damage so that is why I have it at 1620 for now and not 2000.

I am fairly new with Arduino and have no experience with the MPU6050 so I apologize for any misunderstanding of information or code.

Here is my code :

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

Servo right_prop;

int16_t Acc_rawX, Acc_rawY, Acc_rawZ,Gyr_rawX, Gyr_rawY, Gyr_rawZ;

float Acceleration_angle[2];
float Gyro_angle[2];
float Total_angle[2];

float elapsedTime, time, timePrev;
int i;
float rad_to_deg = 180/3.141592654;

double throttle=0; //initial value of throttle to the motors

void setup() {
  Wire.begin(); //begin the wire comunication
  Wire.beginTransmission(0x68);
  Wire.write(0x6B);
  Wire.write(0);
  Wire.endTransmission(true);
  Serial.begin(250000);
  right_prop.attach(3); //attatch the right motor to pin 3

  time = millis(); 

  right_prop.writeMicroseconds(1000);
  delay(7000);

void loop() {

/////////////////////////////I M U/////////////////////////////////////
    timePrev = time;  
    time = millis();  
    elapsedTime = (time - timePrev) / 1000;

     Wire.beginTransmission(0x68);
     Wire.write(0x3B); 
     Wire.endTransmission(false);
     Wire.requestFrom(0x68,6,true); 

     Acc_rawX=Wire.read()<<8|Wire.read(); 
     Acc_rawY=Wire.read()<<8|Wire.read();
     Acc_rawZ=Wire.read()<<8|Wire.read();


     Acceleration_angle[0] = atan((Acc_rawY/16384.0)/sqrt(pow((Acc_rawX/16384.0),2) + pow((Acc_rawZ/16384.0),2)))*rad_to_deg;
     /*---Y---*/
     Acceleration_angle[1] = atan(-1*(Acc_rawX/16384.0)/sqrt(pow((Acc_rawY/16384.0),2) + pow((Acc_rawZ/16384.0),2)))*rad_to_deg;

   Wire.beginTransmission(0x68);
   Wire.write(0x43); //Gyro data first adress
   Wire.endTransmission(false);
   Wire.requestFrom(0x68,4,true); //Just 4 registers

   Gyr_rawX=Wire.read()<<8|Wire.read();
   Gyr_rawY=Wire.read()<<8|Wire.read();

   /*---X---*/
   Gyro_angle[0] = Gyr_rawX/131.0; 
   /*---Y---*/
   Gyro_angle[1] = Gyr_rawY/131.0;

   /*---X axis angle---*/
   Total_angle[0] = 0.98 *(Total_angle[0] + Gyro_angle[0]*elapsedTime) + 0.02*Acceleration_angle[0];
   /*---Y axis angle---*/
   Total_angle[1] = 0.98 *(Total_angle[1] + Gyro_angle[1]*elapsedTime) + 0.02*Acceleration_angle[1];

    Serial.println(Total_angle[1]);

if( Total_angle[1]> 5 && Total_angle[1]< 45)
{
right_prop.write(Total_angle[1]*37);
}
else{
right_prop.write(0);
}
}

//end of loop void

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Browse other questions tagged or ask your own question.