Arduino code to control motor speed based on MPU-6050 sensor

I have been trying to create arduino code where my MPU6050 reads its angle and based on that angle, change the motor speed. For example, 45° is the max motor speed, 22° is roughly half speed, and so on.

I have a 5065 200kv brushless motor going to a ESC with a bec which I can attach to the Arduino. I have modified some self-balancing drone code from a tutorial that used a proportional-integral-derivative controller. What I want to do is take out the PID and do a comparison: if the angle is between 5° and 45° then servo write (angle × 44), because if I multiply the maximum angle by 44 I will get around 2000, which is the original top speed of the code. Although mine can go faster, and I will change it later after testing. I am fairly new with Arduino and I have not used the MPU6050 before.

#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;

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);

time = millis();
}
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);

/*---Y---*/

Wire.beginTransmission(0x68);
Wire.endTransmission(false);
Wire.requestFrom(0x68,4,true); //Just 4 registers

/*---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]*44);
}
else{
right_prop.write(0);
}
}

//end of loop void

• i have modified code using the code of a self balancing drone which used the mpu6050 and made a PID control system to balance the drone between two motors spinning at different speeds to counteract the other motor and make the drone level. My modified code is my attempt at removing the PID control and make it so the arduino reads what angle the sensor is at and tells my one motor to go a certain speed depending on the angle. If you would like i could post the original code or the video about it on youtube. – miles Feb 27 '19 at 22:55
• Where did the original code come from? – 200_success Feb 27 '19 at 22:56
• A youtube video youtu.be/AN3yxIBAxTA – miles Feb 27 '19 at 22:59
• I have gone back and corrected all the errors from copying it. I appologize if i did this completely wrong. I believe i got rid of anything related to the PID and kept everything for reading the angle of the sensor to tell the motor what to do. Would it be simpler to make code that purely reads the angle of the motor and just add a “servo” to the code then do my if-then statement in the loop? – miles Feb 28 '19 at 0:02