Arduino MPU6050 Accelerometer

Mar 16, 2021

•

11402 views

•

1 respects

Devices & Components

Arduino Nano

Inertial Measurement Unit (IMU) (6 deg of freedom)

Jumper wires (generic)

Software & Tools

Arduino IDE

Project description

Code

Arduino MPU6050 Accelerometer

arduino

1#include <Wire.h>
2
3const int MPU = 0x68;
4float AccX, AccY, AccZ;
5float   GyroX, GyroY, GyroZ;
6float accAngleX, accAngleY, gyroAngleX, gyroAngleY, gyroAngleZ;
7float   roll, pitch, yaw;
8float AccErrorX, AccErrorY, GyroErrorX, GyroErrorY, GyroErrorZ;
9float   elapsedTime, currentTime, previousTime;
10int c = 0;
11
12void setup() 
13{
14   Serial.begin(19200);
15  Wire.begin();                    
16  Wire.beginTransmission(MPU);    
17  Wire.write(0x6B);               
18  Wire.write(0x00);             
19   Wire.endTransmission(true);    
20  calculate_IMU_error();
21  delay(20);
22
23}
24
25void   loop() 
26{
27  Wire.beginTransmission(MPU);
28  Wire.write(0x3B); 
29  Wire.endTransmission(false);
30   Wire.requestFrom(MPU, 6, true);
31  AccX = (Wire.read() << 8 | Wire.read()) /   16384.0;
32  AccY = (Wire.read() << 8 | Wire.read()) / 16384.0;
33  AccZ = (Wire.read()   << 8 | Wire.read()) / 16384.0;
34  accAngleX = (atan(AccY / sqrt(pow(AccX, 2) +   pow(AccZ, 2))) * 180 / PI) - 0.58; 
35  accAngleY = (atan(-1 * AccX / sqrt(pow(AccY,   2) + pow(AccZ, 2))) * 180 / PI) + 1.58; 
36  previousTime = currentTime;       
37   currentTime = millis(); 
38  elapsedTime = (currentTime - previousTime) / 1000;   
39  Wire.beginTransmission(MPU);
40  Wire.write(0x43);
41  Wire.endTransmission(false);
42   Wire.requestFrom(MPU, 6, true); 
43  GyroX = (Wire.read() << 8 | Wire.read())   / 131.0; 
44  GyroY = (Wire.read() << 8 | Wire.read()) / 131.0;
45  GyroZ = (Wire.read()   << 8 | Wire.read()) / 131.0;
46  GyroX = GyroX + 0.56; 
47  GyroY = GyroY - 2;
48   GyroZ = GyroZ + 0.79; 
49  gyroAngleX = gyroAngleX + GyroX * elapsedTime;
50   gyroAngleY = gyroAngleY + GyroY * elapsedTime;
51  yaw =  yaw + GyroZ * elapsedTime;
52   roll = 0.96 * gyroAngleX + 0.04 * accAngleX;
53  pitch = 0.96 * gyroAngleY +   0.04 * accAngleY;
54  Serial.print(roll);
55  Serial.print("/");
56  Serial.print(pitch);
57   Serial.print("/");
58  Serial.println(yaw);
59}
60
61
62void calculate_IMU_error()   
63{
64  while (c < 200) {
65    Wire.beginTransmission(MPU);
66    Wire.write(0x3B);
67     Wire.endTransmission(false);
68    Wire.requestFrom(MPU, 6, true);
69    AccX   = (Wire.read() << 8 | Wire.read()) / 16384.0 ;
70    AccY = (Wire.read() << 8 |   Wire.read()) / 16384.0 ;
71    AccZ = (Wire.read() << 8 | Wire.read()) / 16384.0   ;
72    AccErrorX = AccErrorX + ((atan((AccY) / sqrt(pow((AccX), 2) + pow((AccZ),   2))) * 180 / PI));
73    AccErrorY = AccErrorY + ((atan(-1 * (AccX) / sqrt(pow((AccY),   2) + pow((AccZ), 2))) * 180 / PI));
74    c++;
75  }
76  AccErrorX = AccErrorX   / 200;
77  AccErrorY = AccErrorY / 200;
78  c = 0;
79  while (c < 200) {
80     Wire.beginTransmission(MPU);
81    Wire.write(0x43);
82    Wire.endTransmission(false);
83     Wire.requestFrom(MPU, 6, true);
84    GyroX = Wire.read() << 8 | Wire.read();
85     GyroY = Wire.read() << 8 | Wire.read();
86    GyroZ = Wire.read() << 8 | Wire.read();
87     GyroErrorX = GyroErrorX + (GyroX / 131.0);
88    GyroErrorY = GyroErrorY +   (GyroY / 131.0);
89    GyroErrorZ = GyroErrorZ + (GyroZ / 131.0);
90    c++;
91   }
92
93  GyroErrorX = GyroErrorX / 200;
94  GyroErrorY = GyroErrorY / 200;
95   GyroErrorZ = GyroErrorZ / 200;
96  Serial.print("AccErrorX: ");
97  Serial.println(AccErrorX);
98   Serial.print("AccErrorY: ");
99  Serial.println(AccErrorY);
100  Serial.print("GyroErrorX:   ");
101  Serial.println(GyroErrorX);
102  Serial.print("GyroErrorY: ");
103  Serial.println(GyroErrorY);
104   Serial.print("GyroErrorZ: ");
105  Serial.println(GyroErrorZ);
106}

#include <Wire.h>

const int MPU = 0x68;
float AccX, AccY, AccZ;
float   GyroX, GyroY, GyroZ;
float accAngleX, accAngleY, gyroAngleX, gyroAngleY, gyroAngleZ;
float   roll, pitch, yaw;
float AccErrorX, AccErrorY, GyroErrorX, GyroErrorY, GyroErrorZ;
float   elapsedTime, currentTime, previousTime;
int c = 0;

void setup() 
{
   Serial.begin(19200);
  Wire.begin();                    
  Wire.beginTransmission(MPU);    
  Wire.write(0x6B);               
  Wire.write(0x00);             
   Wire.endTransmission(true);    
  calculate_IMU_error();
  delay(20);

}

void   loop() 
{
  Wire.beginTransmission(MPU);
  Wire.write(0x3B); 
  Wire.endTransmission(false);
   Wire.requestFrom(MPU, 6, true);
  AccX = (Wire.read() << 8 | Wire.read()) /   16384.0;
  AccY = (Wire.read() << 8 | Wire.read()) / 16384.0;
  AccZ = (Wire.read()   << 8 | Wire.read()) / 16384.0;
  accAngleX = (atan(AccY / sqrt(pow(AccX, 2) +   pow(AccZ, 2))) * 180 / PI) - 0.58; 
  accAngleY = (atan(-1 * AccX / sqrt(pow(AccY,   2) + pow(AccZ, 2))) * 180 / PI) + 1.58; 
  previousTime = currentTime;       
   currentTime = millis(); 
  elapsedTime = (currentTime - previousTime) / 1000;   
  Wire.beginTransmission(MPU);
  Wire.write(0x43);
  Wire.endTransmission(false);
   Wire.requestFrom(MPU, 6, true); 
  GyroX = (Wire.read() << 8 | Wire.read())   / 131.0; 
  GyroY = (Wire.read() << 8 | Wire.read()) / 131.0;
  GyroZ = (Wire.read()   << 8 | Wire.read()) / 131.0;
  GyroX = GyroX + 0.56; 
  GyroY = GyroY - 2;
   GyroZ = GyroZ + 0.79; 
  gyroAngleX = gyroAngleX + GyroX * elapsedTime;
   gyroAngleY = gyroAngleY + GyroY * elapsedTime;
  yaw =  yaw + GyroZ * elapsedTime;
   roll = 0.96 * gyroAngleX + 0.04 * accAngleX;
  pitch = 0.96 * gyroAngleY +   0.04 * accAngleY;
  Serial.print(roll);
  Serial.print("/");
  Serial.print(pitch);
   Serial.print("/");
  Serial.println(yaw);
}


void calculate_IMU_error()   
{
  while (c < 200) {
    Wire.beginTransmission(MPU);
    Wire.write(0x3B);
     Wire.endTransmission(false);
    Wire.requestFrom(MPU, 6, true);
    AccX   = (Wire.read() << 8 | Wire.read()) / 16384.0 ;
    AccY = (Wire.read() << 8 |   Wire.read()) / 16384.0 ;
    AccZ = (Wire.read() << 8 | Wire.read()) / 16384.0   ;
    AccErrorX = AccErrorX + ((atan((AccY) / sqrt(pow((AccX), 2) + pow((AccZ),   2))) * 180 / PI));
    AccErrorY = AccErrorY + ((atan(-1 * (AccX) / sqrt(pow((AccY),   2) + pow((AccZ), 2))) * 180 / PI));
    c++;
  }
  AccErrorX = AccErrorX   / 200;
  AccErrorY = AccErrorY / 200;
  c = 0;
  while (c < 200) {
     Wire.beginTransmission(MPU);
    Wire.write(0x43);
    Wire.endTransmission(false);
     Wire.requestFrom(MPU, 6, true);
    GyroX = Wire.read() << 8 | Wire.read();
     GyroY = Wire.read() << 8 | Wire.read();
    GyroZ = Wire.read() << 8 | Wire.read();
     GyroErrorX = GyroErrorX + (GyroX / 131.0);
    GyroErrorY = GyroErrorY +   (GyroY / 131.0);
    GyroErrorZ = GyroErrorZ + (GyroZ / 131.0);
    c++;
   }

  GyroErrorX = GyroErrorX / 200;
  GyroErrorY = GyroErrorY / 200;
   GyroErrorZ = GyroErrorZ / 200;
  Serial.print("AccErrorX: ");
  Serial.println(AccErrorX);
   Serial.print("AccErrorY: ");
  Serial.println(AccErrorY);
  Serial.print("GyroErrorX:   ");
  Serial.println(GyroErrorX);
  Serial.print("GyroErrorY: ");
  Serial.println(GyroErrorY);
   Serial.print("GyroErrorZ: ");
  Serial.println(GyroErrorZ);
}

Arduino MPU6050 Accelerometer

arduino

1#include <Wire.h>
2
3const int MPU = 0x68;
4float AccX, AccY, AccZ;
5float
6  GyroX, GyroY, GyroZ;
7float accAngleX, accAngleY, gyroAngleX, gyroAngleY, gyroAngleZ;
8float
9  roll, pitch, yaw;
10float AccErrorX, AccErrorY, GyroErrorX, GyroErrorY, GyroErrorZ;
11float
12  elapsedTime, currentTime, previousTime;
13int c = 0;
14
15void setup() 
16{
17
18  Serial.begin(19200);
19  Wire.begin();                    
20  Wire.beginTransmission(MPU);
21   
22  Wire.write(0x6B);               
23  Wire.write(0x00);             
24
25  Wire.endTransmission(true);    
26  calculate_IMU_error();
27  delay(20);
28
29}
30
31void
32  loop() 
33{
34  Wire.beginTransmission(MPU);
35  Wire.write(0x3B); 
36  Wire.endTransmission(false);
37
38  Wire.requestFrom(MPU, 6, true);
39  AccX = (Wire.read() << 8 | Wire.read()) /
40  16384.0;
41  AccY = (Wire.read() << 8 | Wire.read()) / 16384.0;
42  AccZ = (Wire.read()
43  << 8 | Wire.read()) / 16384.0;
44  accAngleX = (atan(AccY / sqrt(pow(AccX, 2) +
45  pow(AccZ, 2))) * 180 / PI) - 0.58; 
46  accAngleY = (atan(-1 * AccX / sqrt(pow(AccY,
47  2) + pow(AccZ, 2))) * 180 / PI) + 1.58; 
48  previousTime = currentTime;       
49
50  currentTime = millis(); 
51  elapsedTime = (currentTime - previousTime) / 1000;
52  
53  Wire.beginTransmission(MPU);
54  Wire.write(0x43);
55  Wire.endTransmission(false);
56
57  Wire.requestFrom(MPU, 6, true); 
58  GyroX = (Wire.read() << 8 | Wire.read())
59  / 131.0; 
60  GyroY = (Wire.read() << 8 | Wire.read()) / 131.0;
61  GyroZ = (Wire.read()
62  << 8 | Wire.read()) / 131.0;
63  GyroX = GyroX + 0.56; 
64  GyroY = GyroY - 2;
65
66  GyroZ = GyroZ + 0.79; 
67  gyroAngleX = gyroAngleX + GyroX * elapsedTime;
68
69  gyroAngleY = gyroAngleY + GyroY * elapsedTime;
70  yaw =  yaw + GyroZ * elapsedTime;
71
72  roll = 0.96 * gyroAngleX + 0.04 * accAngleX;
73  pitch = 0.96 * gyroAngleY +
74  0.04 * accAngleY;
75  Serial.print(roll);
76  Serial.print("/");
77  Serial.print(pitch);
78
79  Serial.print("/");
80  Serial.println(yaw);
81}
82
83
84void calculate_IMU_error()
85  
86{
87  while (c < 200) {
88    Wire.beginTransmission(MPU);
89    Wire.write(0x3B);
90
91    Wire.endTransmission(false);
92    Wire.requestFrom(MPU, 6, true);
93    AccX
94  = (Wire.read() << 8 | Wire.read()) / 16384.0 ;
95    AccY = (Wire.read() << 8 |
96  Wire.read()) / 16384.0 ;
97    AccZ = (Wire.read() << 8 | Wire.read()) / 16384.0
98  ;
99    AccErrorX = AccErrorX + ((atan((AccY) / sqrt(pow((AccX), 2) + pow((AccZ),
100  2))) * 180 / PI));
101    AccErrorY = AccErrorY + ((atan(-1 * (AccX) / sqrt(pow((AccY),
102  2) + pow((AccZ), 2))) * 180 / PI));
103    c++;
104  }
105  AccErrorX = AccErrorX
106  / 200;
107  AccErrorY = AccErrorY / 200;
108  c = 0;
109  while (c < 200) {
110
111    Wire.beginTransmission(MPU);
112    Wire.write(0x43);
113    Wire.endTransmission(false);
114
115    Wire.requestFrom(MPU, 6, true);
116    GyroX = Wire.read() << 8 | Wire.read();
117
118    GyroY = Wire.read() << 8 | Wire.read();
119    GyroZ = Wire.read() << 8 | Wire.read();
120
121    GyroErrorX = GyroErrorX + (GyroX / 131.0);
122    GyroErrorY = GyroErrorY +
123  (GyroY / 131.0);
124    GyroErrorZ = GyroErrorZ + (GyroZ / 131.0);
125    c++;
126
127  }
128
129  GyroErrorX = GyroErrorX / 200;
130  GyroErrorY = GyroErrorY / 200;
131
132  GyroErrorZ = GyroErrorZ / 200;
133  Serial.print("AccErrorX: ");
134  Serial.println(AccErrorX);
135
136  Serial.print("AccErrorY: ");
137  Serial.println(AccErrorY);
138  Serial.print("GyroErrorX:
139  ");
140  Serial.println(GyroErrorX);
141  Serial.print("GyroErrorY: ");
142  Serial.println(GyroErrorY);
143
144  Serial.print("GyroErrorZ: ");
145  Serial.println(GyroErrorZ);
146}

#include <Wire.h>

const int MPU = 0x68;
float AccX, AccY, AccZ;
float
  GyroX, GyroY, GyroZ;
float accAngleX, accAngleY, gyroAngleX, gyroAngleY, gyroAngleZ;
float
  roll, pitch, yaw;
float AccErrorX, AccErrorY, GyroErrorX, GyroErrorY, GyroErrorZ;
float
  elapsedTime, currentTime, previousTime;
int c = 0;

void setup() 
{

  Serial.begin(19200);
  Wire.begin();                    
  Wire.beginTransmission(MPU);
   
  Wire.write(0x6B);               
  Wire.write(0x00);             

  Wire.endTransmission(true);    
  calculate_IMU_error();
  delay(20);

}

void
  loop() 
{
  Wire.beginTransmission(MPU);
  Wire.write(0x3B); 
  Wire.endTransmission(false);

  Wire.requestFrom(MPU, 6, true);
  AccX = (Wire.read() << 8 | Wire.read()) /
  16384.0;
  AccY = (Wire.read() << 8 | Wire.read()) / 16384.0;
  AccZ = (Wire.read()
  << 8 | Wire.read()) / 16384.0;
  accAngleX = (atan(AccY / sqrt(pow(AccX, 2) +
  pow(AccZ, 2))) * 180 / PI) - 0.58; 
  accAngleY = (atan(-1 * AccX / sqrt(pow(AccY,
  2) + pow(AccZ, 2))) * 180 / PI) + 1.58; 
  previousTime = currentTime;       

  currentTime = millis(); 
  elapsedTime = (currentTime - previousTime) / 1000;
  
  Wire.beginTransmission(MPU);
  Wire.write(0x43);
  Wire.endTransmission(false);

  Wire.requestFrom(MPU, 6, true); 
  GyroX = (Wire.read() << 8 | Wire.read())
  / 131.0; 
  GyroY = (Wire.read() << 8 | Wire.read()) / 131.0;
  GyroZ = (Wire.read()
  << 8 | Wire.read()) / 131.0;
  GyroX = GyroX + 0.56; 
  GyroY = GyroY - 2;

  GyroZ = GyroZ + 0.79; 
  gyroAngleX = gyroAngleX + GyroX * elapsedTime;

  gyroAngleY = gyroAngleY + GyroY * elapsedTime;
  yaw =  yaw + GyroZ * elapsedTime;

  roll = 0.96 * gyroAngleX + 0.04 * accAngleX;
  pitch = 0.96 * gyroAngleY +
  0.04 * accAngleY;
  Serial.print(roll);
  Serial.print("/");
  Serial.print(pitch);

  Serial.print("/");
  Serial.println(yaw);
}


void calculate_IMU_error()
  
{
  while (c < 200) {
    Wire.beginTransmission(MPU);
    Wire.write(0x3B);

    Wire.endTransmission(false);
    Wire.requestFrom(MPU, 6, true);
    AccX
  = (Wire.read() << 8 | Wire.read()) / 16384.0 ;
    AccY = (Wire.read() << 8 |
  Wire.read()) / 16384.0 ;
    AccZ = (Wire.read() << 8 | Wire.read()) / 16384.0
  ;
    AccErrorX = AccErrorX + ((atan((AccY) / sqrt(pow((AccX), 2) + pow((AccZ),
  2))) * 180 / PI));
    AccErrorY = AccErrorY + ((atan(-1 * (AccX) / sqrt(pow((AccY),
  2) + pow((AccZ), 2))) * 180 / PI));
    c++;
  }
  AccErrorX = AccErrorX
  / 200;
  AccErrorY = AccErrorY / 200;
  c = 0;
  while (c < 200) {

    Wire.beginTransmission(MPU);
    Wire.write(0x43);
    Wire.endTransmission(false);

    Wire.requestFrom(MPU, 6, true);
    GyroX = Wire.read() << 8 | Wire.read();

    GyroY = Wire.read() << 8 | Wire.read();
    GyroZ = Wire.read() << 8 | Wire.read();

    GyroErrorX = GyroErrorX + (GyroX / 131.0);
    GyroErrorY = GyroErrorY +
  (GyroY / 131.0);
    GyroErrorZ = GyroErrorZ + (GyroZ / 131.0);
    c++;

  }

  GyroErrorX = GyroErrorX / 200;
  GyroErrorY = GyroErrorY / 200;

  GyroErrorZ = GyroErrorZ / 200;
  Serial.print("AccErrorX: ");
  Serial.println(AccErrorX);

  Serial.print("AccErrorY: ");
  Serial.println(AccErrorY);
  Serial.print("GyroErrorX:
  ");
  Serial.println(GyroErrorX);
  Serial.print("GyroErrorY: ");
  Serial.println(GyroErrorY);

  Serial.print("GyroErrorZ: ");
  Serial.println(GyroErrorZ);
}

Downloadable files

Arduino MPU6050 Accelerometer

The circuit diagram

Arduino MPU6050 Accelerometer

Comments

Only logged in users can leave comments

MinukaThesathYapa

0 Followers

•

0 Projects

Intro

Trademarks & CopyrightsWhistleblowingDigital Services ActTerms of ServicePrivacy PolicySecurityCookie Settings

Report content