Calculate Angle with MPU6050

Hi there.
I would like to implement a project in which I have three MPU6050 gyroscope sensors connected to a Teensy and I want the individual angles in the X, Y, and Z-axis as output.
Unfortunately I do not have much experience with the programming language and have found a code for oe sensor in the internet. I extended the Code to three senses.

#include<Wire.h>
const int MPU_addr1=0x68,MPU_addr2=0x69,MPU_addr3=0x68;
int16_t axis_X1,axis_Y1,axis_Z1;
int16_t axis_X2,axis_Y2,axis_Z2;
int16_t axis_X3,axis_Y3,axis_Z3;

int minVal=265;
int maxVal=402;
 
void setup(){
  
  pinMode(taster1,INPUT);
  pinMode(taster2,INPUT);
  
  Wire.begin();                                      //begin the wire communication
  Wire.beginTransmission(MPU_addr1);                 //begin, send the slave adress (in this case 68)
  Wire.write(0x6B);                                  //make the reset (place a 0 into the 6B register)
  Wire.write(0);
  Wire.endTransmission(true);                        //end the transmission

  Wire.begin(); 
  Wire.beginTransmission(MPU_addr2);
  Wire.write(0x6B);
  Wire.write(0);
  Wire.endTransmission(true);

  Wire1.begin();
  Wire1.beginTransmission(MPU_addr3);
  Wire1.write(0x6B);
  Wire1.write(0);
  Wire1.endTransmission(true);
  
  Serial.begin(9600);
}

void loop(){

    
  GetAngle1(MPU_addr1);
  GetAngle2(MPU_addr2);
  GetAngle3(MPU_addr3);
  delay(50);
 
} 


void GetAngle1(const int MPU) {

Wire.beginTransmission(MPU_addr1);
  Wire.write(0x3B);
  Wire.endTransmission(false);
  Wire.requestFrom(MPU_addr1,14,true);
  axis_X1=Wire.read()<<8|Wire.read();
  axis_Y1=Wire.read()<<8|Wire.read();
  axis_Z1=Wire.read()<<8|Wire.read();
    int xAng = map(axis_X1,minVal,maxVal,-90,90);
    int yAng = map(axis_Y1,minVal,maxVal,-90,90);
    int zAng = map(axis_Z1,minVal,maxVal,-90,90);
       double x1= RAD_TO_DEG * (atan2(-yAng, -zAng)+PI);
       double y1= RAD_TO_DEG * (atan2(-xAng, -zAng)+PI);
       double z1= RAD_TO_DEG * (atan2(-yAng, -xAng)+PI);
     Serial.print(x1);
     Serial.print(",");
     Serial.print(y1);
     Serial.print(",");
     Serial.print(z1);
     Serial.print(",");
}

void GetAngle2(const int MPU) {

  Wire.beginTransmission(MPU_addr2);
  Wire.write(0x3B);
  Wire.endTransmission(false);
  Wire.requestFrom(MPU_addr2,14,true);
  axis_X2=Wire.read()<<8|Wire.read();
  axis_Y2=Wire.read()<<8|Wire.read();
  axis_Z2=Wire.read()<<8|Wire.read();
    int xAng = map(axis_X2,minVal,maxVal,-90,90);
    int yAng = map(axis_Y2,minVal,maxVal,-90,90);
    int zAng = map(axis_Z2,minVal,maxVal,-90,90);
       double x2= RAD_TO_DEG * (atan2(-yAng, -zAng)+PI);
       double y2= RAD_TO_DEG * (atan2(-xAng, -zAng)+PI);
       double z2= RAD_TO_DEG * (atan2(-yAng, -xAng)+PI);
     Serial.print(x2);
     Serial.print(",");
     Serial.print(y2);
     Serial.print(",");
     Serial.print(z2);
     Serial.print(",");
}

void GetAngle3(const int MPU) {

  Wire1.beginTransmission(MPU_addr3);
  Wire1.write(0x3B);
  Wire1.endTransmission(false);
  Wire1.requestFrom(MPU_addr3,14,true);
  axis_X3=Wire1.read()<<8|Wire1.read();
  axis_Y3=Wire1.read()<<8|Wire1.read();
  axis_Z3=Wire1.read()<<8|Wire1.read();
    int xAng = map(axis_X3,minVal,maxVal,-90,90);
    int yAng = map(axis_Y3,minVal,maxVal,-90,90);
    int zAng = map(axis_Z3,minVal,maxVal,-90,90);
       double x3= RAD_TO_DEG * (atan2(-yAng, -zAng)+PI);
       double y3= RAD_TO_DEG * (atan2(-xAng, -zAng)+PI);
       double z3= RAD_TO_DEG * (atan2(-yAng, -xAng)+PI);
     Serial.print(x3);
     Serial.print(",");
     Serial.print(y3);
     Serial.print(",");
     Serial.println(z3);
}

Unfortunately, the second sensor only outputs the value 225 and the values for the x-axis for the other two sensors vary considerably. So, maybe someone can explain the Code, so that I may find the problem by myself. Or maybe someone of you has a better idea of how to read the angle.

I am thankful for any kind of help!

Unfortunately I do not have much experience with the programming language

But you are attempting a very challenging project anyway.

Expect endless frustration with that approach, or consider posting on the Gigs & Collaborations forum section and hiring someone to write code for you.

This is a hobby. If you want to have fun, first learn to blink an LED, read a button or a sensor, learn the programming language and special features of the Arduino by following the examples that come with it.

The values being read from the MPU6050 are raw accelerometer values and cannot be converted directly to Pitch, Roll, or Yaw angles. You should probably download one of the many MPU6050 libraries that include the formulas to calculate Pitch and Roll angles from the accelerometer values.

You can't calculate a Yaw angle (heading?) using an accelerometer. You will need a magnetometer to do that.

johnwasser:
The values being read from the MPU6050 are raw accelerometer values and cannot be converted directly to Pitch, Roll, or Yaw angles. You should probably download one of the many MPU6050 libraries that include the formulas to calculate Pitch and Roll angles from the accelerometer values.

You can't calculate a Yaw angle (heading?) using an accelerometer. You will need a magnetometer to do that.

Okay thank you, so in your opinion, I should completely reject the code above and look for a new approach? However, I am not really sure how to extend the code to three sensors with the MPU6050 libraries.
However, the code above gives the correct values for the angle, so it seems to work that the angles are calculated directly from the raw data.

Why do you want three sensors?

Two tilt angles for one sensor can be computed from the raw accelerometer data , following this tutorial.

Yaw cannot be calculated without a magnetometer or other external reference.

jremington:
Why do you want three sensors?

Two tilt angles for one sensor can be computed from the raw accelerometer data , following this tutorial.

Yaw cannot be calculated without a magnetometer or other external reference.

Yes I already know this tutorial thank you, but I do not quite understand the code. At least I do not know exactly how to insert the small program piece into my code.
I also have a code that outputs the raw data of the sensor, so the accelerometer values and the gyroscope values and additionally the temperature, which is unimportant for me.
But as it looks in the tutorial, you can calculate the angle directly from the raw data without any MPU6050 library, right?

First, it would be nice if it works well for a sensor. If possible I would extend it to three later. But that is at first negligible.

So here is my Code for the raw-data of the sensor,which I found in a sensor book, I bought for this projekt.

#include <Wire.h> // <1>

const char i2c_address =  0x68; // <2>

const unsigned char sleep_mgmt = 0x6B; // <3>
const unsigned char accel_x_out = 0x3B;

struct data_pdu // <4>
{
  int16_t x_accel; // <5>
  int16_t y_accel;
  int16_t z_accel;
  int16_t temperature; // <6>
  int16_t x_gyro; // <7>
  int16_t y_gyro;
  int16_t z_gyro;
};


void setup() {
  Serial.begin(115200); 
  Wire.begin(); // <8>
  write_i2c(sleep_mgmt,0x00);   // <9>
}

int16_t swap_int16_t(int16_t value) // <10>
{
  int16_t left = value << 8; // <11>
  int16_t right = value >> 8; // <12>
  right = right & 0xFF; // <13>
  return left | right; // <14>
}

void loop() {
  data_pdu pdu; // <15>
  read_i2c(accel_x_out, (uint8_t *)&pdu, sizeof(data_pdu)); // <16>

  pdu.x_accel = swap_int16_t(pdu.x_accel); // <17>
  pdu.y_accel = swap_int16_t(pdu.y_accel);
  pdu.z_accel = swap_int16_t(pdu.z_accel);
  pdu.temperature = swap_int16_t(pdu.temperature); // <18>
  pdu.x_gyro = swap_int16_t(pdu.x_gyro);
  pdu.y_gyro = swap_int16_t(pdu.y_gyro);
  pdu.z_gyro = swap_int16_t(pdu.z_gyro);

  float acc_x = pdu.x_accel / 16384.0f; // <19>
  float acc_y = pdu.y_accel / 16384.0f;
  float acc_z = pdu.z_accel / 16384.0f;
  Serial.print("Accelerometer: x,y,z (");
  Serial.print(acc_x,3); Serial.print("g, "); // <20>
  Serial.print(acc_y,3); Serial.print("g, ");
  Serial.print(acc_z,3); Serial.println("g)");
  
  int zero_point = -512 - (340 * 35); // <21>
  double temperature = (pdu.temperature - zero_point) / 340.0; // <22>
  Serial.print("Temperature (C): ");
  Serial.println(temperature,2);
  
  Serial.print("Gyro: x,y,z (");
  Serial.print(pdu.x_gyro / 131.0f); Serial.print(" deg/s, "); // <23>
  Serial.print(pdu.y_gyro / 131.0f); Serial.print(" deg/s, ");
  Serial.print(pdu.z_gyro / 131.0f); Serial.println(" deg/s)"); 
  
  delay(1000);
}

void read_i2c(unsigned char reg, uint8_t *buffer, int size) // <24>
{
  Wire.beginTransmission(i2c_address); // <25>
  Wire.write(reg); // <26>
  Wire.endTransmission(false); // <27>
  Wire.requestFrom(i2c_address,size,true); // <28>

  int i = 0; // <29>
  while(Wire.available() && i < size) { // <30>
    buffer[i] = Wire.read(); // <31>
    i++;
  }
  if(i != size) { // <32>
    Serial.println("Error reading from i2c"); 
  }

}

void write_i2c(unsigned char reg, const uint8_t data) // <33>
{
  Wire.beginTransmission(i2c_address); // <34>
  Wire.write(reg);
  Wire.write(data);
  Wire.endTransmission(true);
}

I would be very thankful, if anyone could just tell me, where to insert the programm code from the tutorial in my code, to calculate the angle.

Thank you!

Marvikov:
But as it looks in the tutorial, you can calculate the angle directly from the raw data without any MPU6050 library, right?

You can calculate the roll and pitch using accelerometer data that has been scaled and corrected (optional at this point)

This code will give you angle calculations from scaled and corrected accelerometer data.

//Roll & Pitch Equations from accelerometers only
float roll  = (atan2(-_ay, _az)*180.0)/M_PI;
float pitch = (atan2(_ax, sqrt(_ay*_ay + _az*_az))*180.0)/M_PI;

If you look at your first post this formula is included with the posted code.

Idahowalker:
If you look at your first post this formula is included with the posted code.

Thank you! But I don't see this formula in the first Code I posted.

But it looks like the first code is not really good, because the values vary a lot (even when restless).
In addition, only the value of 225 is displayed to with the second sensor and I have already seen many comments on the code, which had the same problem. So I would like to try to work with the second code I sent.

But it looks like I can just insert the acc data into the formula and it should work.

However, the acc values in my second code are converted to g. I'll try to implement it tomorrow and see if it works.

Thank you so far!

In your posted code, the first three lines are unnecessary nonsense, and the second three are all wrong.

    int xAng = map(axis_X1,minVal,maxVal,-90,90);
    int yAng = map(axis_Y1,minVal,maxVal,-90,90);
    int zAng = map(axis_Z1,minVal,maxVal,-90,90);
       double x1= RAD_TO_DEG * (atan2(-yAng, -zAng)+PI);
       double y1= RAD_TO_DEG * (atan2(-xAng, -zAng)+PI);
       double z1= RAD_TO_DEG * (atan2(-yAng, -xAng)+PI);

Marvikov:
But it looks like the first code is not really good, because the values vary a lot (even when restless).

You will find that just using accelerometer data your angles will be very restless. So you might look, more MPU power, to filter that data, such as using a low pass filter like this:

 //Low Pass Filter
 fXg = Xg * alpha + (fXg * (1.0 - alpha));
 fYg = Yg * alpha + (fYg * (1.0 - alpha));
 fZg = Zg * alpha + (fZg * (1.0 - alpha));

Which uses scaled and corrected gyro and accelerometer data to calm that restless factor down.
Where alpha is used to determine the ratio of gyro to accelerometer data used. I'd use 97 to 92 for alpha. You have to figure out which works best for you. At that point you may want more MPU power by using a complementary filter, use the words "mpu 6050 complementary filter" to do a search on.

Then you may want more MPU power so you'll look to onehorse | Mbed for deeper insight as your quest for more MPU power continues. Then you will find you are neck deep in floats. Yuppers floats and the Uno just falls flat; so onto a Due or STM32 Bluepill or ESP32. For even more MPU power you will add a Magnetometer and, after figuring out the calibration routines for the magnetometer, you'll be near the max to the power of the MPU. The last MPU power move would be to use 2 magnetometers mounted so that each axis is 180 degrees out of phase with its corresponding axis for auto correcting magnetometer readings. Then you'll be there. Well almost.

Then there is the step into MPU power madness. To use a LSM9DS1 on the SPI buss; the magnetometer is on the SPI buss. You see there are many examples of using the LSM9DS1 on the I2C bus but very little on using the SPI buss and, with the ESP32 API (which I got working nicely), your on your own. By using The ESP32 SPI API you can talk to the LSM9DS1, using freeRTOS (more madness), using bidirectional communications and background reads/writes to the LSM9DS1. After that, all that is left is to apply 1.21 gigawatts.

jremington:
In your posted code, the first three lines are unnecessary nonsense, and the second three are all wrong.

    int xAng = map(axis_X1,minVal,maxVal,-90,90);

int yAng = map(axis_Y1,minVal,maxVal,-90,90);
   int zAng = map(axis_Z1,minVal,maxVal,-90,90);
      double x1= RAD_TO_DEG * (atan2(-yAng, -zAng)+PI);
      double y1= RAD_TO_DEG * (atan2(-xAng, -zAng)+PI);
      double z1= RAD_TO_DEG * (atan2(-yAng, -xAng)+PI);

Yes I have already thought that the code can not be quite right and I have found him only on the internet and I thought nothing more.
Can you help me with the other code I sent?
Is it possible to calculate the angle from this?

Idahowalker:
You will find that just using accelerometer data your angles will be very restless. So you might look to filter that data, such as using a low pass filter like this:

 //Low Pass Filter

fXg = Xg * alpha + (fXg * (1.0 - alpha));
fYg = Yg * alpha + (fYg * (1.0 - alpha));
fZg = Zg * alpha + (fZg * (1.0 - alpha));



Which uses scaled and corrected gyro and accelerometer data to calm that restless factor down.
Where alpha is used to determine the ratio of gyro to accelerometer data to use. I'd use 97 to 92 for alpha. You have to figure out which works best for you. At that point you may want more filtering by using a complementary filter, use the words "mpu 6050 complementary filter" to do a search on.

okay, I'll take a look at the filter. Do you think that I can realize this project with the second code that I have sent by simply adding the formula for the angle and the filter into the code? Is that possible?

Marvikov:
okay, I'll take a look at the filter. Do you think that I can realize this project with the second code that I have sent by simply adding the formula for the angle and the filter into the code? Is that possible?

Yes. Yes.

Can you help me with the other code I sent?

The CORRECT tilt angle formulas are given in the DFROBOT web site I linked earlier.

Try the following. I don't have an MPU-6050 to test.

For best accuracy, calibrate the accelerometer according to this tutorial: Tutorial: How to calibrate a compass (and accelerometer) with Arduino | Underwater Arduino Data Loggers.

// minimal MPU-6050 tilt and roll
#include<Wire.h>
const int MPU_addr1 = 0x68;
float xa, ya, za, roll, pitch;

void setup() {

  Wire.begin();                                      //begin the wire communication
  Wire.beginTransmission(MPU_addr1);                 //begin, send the slave adress (in this case 68)
  Wire.write(0x6B);                                  //make the reset (place a 0 into the 6B register)
  Wire.write(0);
  Wire.endTransmission(true);                        //end the transmission
  Serial.begin(9600);
}

void loop() {

  Wire.beginTransmission(MPU_addr1);
  Wire.write(0x3B);
  Wire.endTransmission(false);
  Wire.requestFrom(MPU_addr1, 6, true); //get six bytes accelerometer data

  xa = Wire.read() << 8 | Wire.read();
  ya = Wire.read() << 8 | Wire.read();
  za = Wire.read() << 8 | Wire.read();

  roll = atan2(ya , za) * 180.0 / PI;
  pitch = atan2(-xa , sqrt(ya * ya + za * za)) * 180.0 / PI;

  Serial.print("roll = ");
  Serial.print(roll);
  Serial.print(", pitch = ");
  Serial.println(pitch);
  delay(200);
}

jremington:
The CORRECT tilt angle formulas are given in the DFROBOT web site I linked earlier.

Try the following.

//

xa = Wire.read() << 8 | Wire.read();
  ya = Wire.read() << 8 | Wire.read();
  za = Wire.read() << 8 | Wire.read();

}

Thank you very much, but could you please explain this part of the Code? What does it do? I don't understand.

I have tested the code and it still returns strange values that can not quite fit.
But I have not calibrated the sensors as in the tutorial yet. I will try this now and hope that the values of the angle then fit.

What does it do?

Reads the sensor data.

it still returns strange values that can not quite fit.But you don't think it would be helpful to post examples?

This project is way above your present skill level and you can expect endless frustration with this approach.

Start with the simple examples that come with the Arduino and learn the programming language, how to blink and LED, read a voltage, a pushbutton or a sensor. Then think about more advanced projects.

It's all out there

https://www.youtube.com/results?search_query=mpu6050+arduino

void read_gyro()
{

  read_mpu_6050_data();                                                //Read the raw acc and gyro data from the MPU-6050

  gyro_x -= gyro_x_cal;                                                //Subtract the offset calibration value from the raw gyro_x value
  gyro_y -= gyro_y_cal;                                                //Subtract the offset calibration value from the raw gyro_y value
  gyro_z -= gyro_z_cal;                                                //Subtract the offset calibration value from the raw gyro_z value

  //Gyro angle calculations
  //0.0000611 = 1 / (250Hz / 65.5)
  angle_pitch += gyro_x * 0.0000611;                                   //Calculate the traveled pitch angle and add this to the angle_pitch variable
  angle_roll += gyro_y * 0.0000611;                                    //Calculate the traveled roll angle and add this to the angle_roll variable

  //0.000001066 = 0.0000611 * (3.142(PI) / 180degr) The Arduino sin function is in radians
  angle_pitch += angle_roll * sin(gyro_z * 0.000001066);               //If the IMU has yawed transfer the roll angle to the pitch angel
  angle_roll -= angle_pitch * sin(gyro_z * 0.000001066);               //If the IMU has yawed transfer the pitch angle to the roll angel

  //Accelerometer angle calculations
  acc_total_vector = sqrt((acc_x * acc_x) + (acc_y * acc_y) + (acc_z * acc_z)); //Calculate the total accelerometer vector
  //57.296 = 1 / (3.142 / 180) The Arduino asin function is in radians
  angle_pitch_acc = asin((float)acc_y / acc_total_vector) * 57.296;    //Calculate the pitch angle
  angle_roll_acc = asin((float)acc_x / acc_total_vector) * -57.296;    //Calculate the roll angle

  //Place the MPU-6050 spirit level and note the values in the following two lines for calibration
  angle_pitch_acc -= 0.0;                                              //Accelerometer calibration value for pitch
  angle_roll_acc -= 0.0;                                               //Accelerometer calibration value for roll

  if (set_gyro_angles) {                                               //If the IMU is already started
    angle_pitch = angle_pitch * 0.9996 + angle_pitch_acc * 0.0004;     //Correct the drift of the gyro pitch angle with the accelerometer pitch angle
    angle_roll = angle_roll * 0.9996 + angle_roll_acc * 0.0004;        //Correct the drift of the gyro roll angle with the accelerometer roll angle
  }
  else {                                                               //At first start
    angle_pitch = angle_pitch_acc;                                     //Set the gyro pitch angle equal to the accelerometer pitch angle
    angle_roll = angle_roll_acc;                                       //Set the gyro roll angle equal to the accelerometer roll angle
    set_gyro_angles = true;                                            //Set the IMU started flag
  }

  //To dampen the pitch and roll angles a complementary filter is used
  angle_pitch_output = angle_pitch_output * 0.9 + angle_pitch * 0.1;   //Take 90% of the output pitch value and add 10% of the raw pitch value
  angle_roll_output = angle_roll_output * 0.9 + angle_roll * 0.1;      //Take 90% of the output roll value and add 10% of the raw roll value

}

void read_mpu_6050_data() {                                            //Subroutine for reading the raw gyro and accelerometer data

  Wire.beginTransmission(0x68);                                        //Start communicating with the MPU-6050
  Wire.write(0x3B);                                                    //Send the requested starting register
  Wire.endTransmission();                                              //End the transmission
  Wire.requestFrom(0x68, 14);                                          //Request 14 bytes from the MPU-6050
  while (Wire.available() < 14);                                       //Wait until all the bytes are received
  acc_x = Wire.read() << 8 | Wire.read();                              //Add the low and high byte to the acc_x variable
  acc_y = Wire.read() << 8 | Wire.read();                              //Add the low and high byte to the acc_y variable
  acc_z = Wire.read() << 8 | Wire.read();                              //Add the low and high byte to the acc_z variable
  temperature = Wire.read() << 8 | Wire.read();                        //Add the low and high byte to the temperature variable
  gyro_x = Wire.read() << 8 | Wire.read();                             //Add the low and high byte to the gyro_x variable
  gyro_y = Wire.read() << 8 | Wire.read();                             //Add the low and high byte to the gyro_y variable
  gyro_z = Wire.read() << 8 | Wire.read();                             //Add the low and high byte to the gyro_z variable

}

Hope that helps.

Forgot to add:

void calibrate_gyro(int cal_points)
{


  lcd.setCursor(0, 0);                                                 //Set the LCD cursor to position to position 0,0
  lcd.print("Calibrating gyro");                                       //Print text to screen
  lcd.setCursor(0, 1);                                                 //Set the LCD cursor to position to position 0,1
  for (int cal_int = 0; cal_int < cal_points ; cal_int ++) {                 //Run this code 2000 times
    if (cal_int % (cal_points / 16) == 0)lcd.print(".");                           //Print a dot on the LCD every 125 readings
    read_mpu_6050_data();                                              //Read the raw acc and gyro data from the MPU-6050
    gyro_x_cal += gyro_x;                                              //Add the gyro x-axis offset to the gyro_x_cal variable
    gyro_y_cal += gyro_y;                                              //Add the gyro y-axis offset to the gyro_y_cal variable
    gyro_z_cal += gyro_z;                                              //Add the gyro z-axis offset to the gyro_z_cal variable
    delay(3);                                                          //Delay 3us to simulate the 250Hz program loop
  }
  gyro_x_cal /= cal_points;                                                  //Divide the gyro_x_cal variable by 2000 to get the avarage offset
  gyro_y_cal /= cal_points;                                                  //Divide the gyro_y_cal variable by 2000 to get the avarage offset
  gyro_z_cal /= cal_points;                                                  //Divide the gyro_z_cal variable by 2000 to get the avarage offset

  Serial.print("gyro_x_cal:");
  Serial.println(gyro_x_cal);
  Serial.print("gyro_y_cal:");
  Serial.println(gyro_y_cal);
  Serial.print("gyro_z_cal:");
  Serial.println(gyro_z_cal);




}

ScrewLoose:
It's all out there

https://www.youtube.com/results?search_query=mpu6050+arduino

void read_gyro()

{

read_mpu_6050_data();                                                //Read the raw acc and gyro data from the MPU-6050

gyro_x -= gyro_x_cal;                                                //Subtract the offset calibration value from the raw gyro_x value
  gyro_y -= gyro_y_cal;                                                //Subtract the offset calibration value from the raw gyro_y value
  gyro_z -= gyro_z_cal;                                                //Subtract the offset calibration value from the raw gyro_z value

//Gyro angle calculations
  //0.0000611 = 1 / (250Hz / 65.5)
  angle_pitch += gyro_x * 0.0000611;                                  //Calculate the traveled pitch angle and add this to the angle_pitch variable
  angle_roll += gyro_y * 0.0000611;                                    //Calculate the traveled roll angle and add this to the angle_roll variable

//0.000001066 = 0.0000611 * (3.142(PI) / 180degr) The Arduino sin function is in radians
  angle_pitch += angle_roll * sin(gyro_z * 0.000001066);              //If the IMU has yawed transfer the roll angle to the pitch angel
  angle_roll -= angle_pitch * sin(gyro_z * 0.000001066);              //If the IMU has yawed transfer the pitch angle to the roll angel

//Accelerometer angle calculations
  acc_total_vector = sqrt((acc_x * acc_x) + (acc_y * acc_y) + (acc_z * acc_z)); //Calculate the total accelerometer vector
  //57.296 = 1 / (3.142 / 180) The Arduino asin function is in radians
  angle_pitch_acc = asin((float)acc_y / acc_total_vector) * 57.296;    //Calculate the pitch angle
  angle_roll_acc = asin((float)acc_x / acc_total_vector) * -57.296;    //Calculate the roll angle

//Place the MPU-6050 spirit level and note the values in the following two lines for calibration
  angle_pitch_acc -= 0.0;                                              //Accelerometer calibration value for pitch
  angle_roll_acc -= 0.0;                                              //Accelerometer calibration value for roll

if (set_gyro_angles) {                                              //If the IMU is already started
    angle_pitch = angle_pitch * 0.9996 + angle_pitch_acc * 0.0004;    //Correct the drift of the gyro pitch angle with the accelerometer pitch angle
    angle_roll = angle_roll * 0.9996 + angle_roll_acc * 0.0004;        //Correct the drift of the gyro roll angle with the accelerometer roll angle
  }
  else {                                                              //At first start
    angle_pitch = angle_pitch_acc;                                    //Set the gyro pitch angle equal to the accelerometer pitch angle
    angle_roll = angle_roll_acc;                                      //Set the gyro roll angle equal to the accelerometer roll angle
    set_gyro_angles = true;                                            //Set the IMU started flag
  }

//To dampen the pitch and roll angles a complementary filter is used
  angle_pitch_output = angle_pitch_output * 0.9 + angle_pitch * 0.1;  //Take 90% of the output pitch value and add 10% of the raw pitch value
  angle_roll_output = angle_roll_output * 0.9 + angle_roll * 0.1;      //Take 90% of the output roll value and add 10% of the raw roll value

}

void read_mpu_6050_data() {                                            //Subroutine for reading the raw gyro and accelerometer data

Wire.beginTransmission(0x68);                                        //Start communicating with the MPU-6050
  Wire.write(0x3B);                                                    //Send the requested starting register
  Wire.endTransmission();                                              //End the transmission
  Wire.requestFrom(0x68, 14);                                          //Request 14 bytes from the MPU-6050
  while (Wire.available() < 14);                                      //Wait until all the bytes are received
  acc_x = Wire.read() << 8 | Wire.read();                              //Add the low and high byte to the acc_x variable
  acc_y = Wire.read() << 8 | Wire.read();                              //Add the low and high byte to the acc_y variable
  acc_z = Wire.read() << 8 | Wire.read();                              //Add the low and high byte to the acc_z variable
  temperature = Wire.read() << 8 | Wire.read();                        //Add the low and high byte to the temperature variable
  gyro_x = Wire.read() << 8 | Wire.read();                            //Add the low and high byte to the gyro_x variable
  gyro_y = Wire.read() << 8 | Wire.read();                            //Add the low and high byte to the gyro_y variable
  gyro_z = Wire.read() << 8 | Wire.read();                            //Add the low and high byte to the gyro_z variable

}




Hope that helps.


Forgot to add:



void calibrate_gyro(int cal_points)
{

lcd.setCursor(0, 0);                                                //Set the LCD cursor to position to position 0,0
  lcd.print("Calibrating gyro");                                      //Print text to screen
  lcd.setCursor(0, 1);                                                //Set the LCD cursor to position to position 0,1
  for (int cal_int = 0; cal_int < cal_points ; cal_int ++) {                //Run this code 2000 times
    if (cal_int % (cal_points / 16) == 0)lcd.print(".");                          //Print a dot on the LCD every 125 readings
    read_mpu_6050_data();                                              //Read the raw acc and gyro data from the MPU-6050
    gyro_x_cal += gyro_x;                                              //Add the gyro x-axis offset to the gyro_x_cal variable
    gyro_y_cal += gyro_y;                                              //Add the gyro y-axis offset to the gyro_y_cal variable
    gyro_z_cal += gyro_z;                                              //Add the gyro z-axis offset to the gyro_z_cal variable
    delay(3);                                                          //Delay 3us to simulate the 250Hz program loop
  }
  gyro_x_cal /= cal_points;                                                  //Divide the gyro_x_cal variable by 2000 to get the avarage offset
  gyro_y_cal /= cal_points;                                                  //Divide the gyro_y_cal variable by 2000 to get the avarage offset
  gyro_z_cal /= cal_points;                                                  //Divide the gyro_z_cal variable by 2000 to get the avarage offset

Serial.print("gyro_x_cal:");
  Serial.println(gyro_x_cal);
  Serial.print("gyro_y_cal:");
  Serial.println(gyro_y_cal);
  Serial.print("gyro_z_cal:");
  Serial.println(gyro_z_cal);

}

Where is the void and the loop in your Code? Where do you have the Code from?

jremington:
Reads the sensor data.

it still returns strange values that can not quite fit.But you don't think it would be helpful to post examples?

roll = 45.75, pitch = -10.83
roll = 45.75, pitch = -10.79
roll = 45.82, pitch = -10.75
roll = 45.74, pitch = -10.77
roll = 45.74, pitch = -10.83
roll = 45.73, pitch = -10.76
roll = 47.48, pitch = -10.91
roll = 48.73, pitch = -10.46
roll = 51.73, pitch = -8.32
roll = 52.19, pitch = -7.71
roll = 0.00, pitch = 0.00
roll = 0.00, pitch = 0.00
roll = 0.00, pitch = 0.00
roll = 0.00, pitch = 0.00
roll = 0.00, pitch = 0.00
roll = 0.00, pitch = 0.00
roll = 0.00, pitch = 0.00
roll = 0.00, pitch = 0.00
roll = 0.00, pitch = 0.00
roll = 0.00, pitch = 0.00
roll = 0.00, pitch = 0.00
roll = 0.00, pitch = 0.00

At some point I just get 0 as output without doing anthing....

jremington:
This project is way above your present skill level and you can expect endless frustration with this approach.

Start with the simple examples that come with the Arduino and learn the programming language, how to blink and LED, read a voltage, a pushbutton or a sensor. Then think about more advanced projects.

I just want to realize this Code for a project and don't ant to start at the beginning. That's why I am here, to get help with my Code.