Posted by author-avatar
Arduino Sensor Examples
📟
Arduino Sensor Examples
1 Examples • C/C++
arduino mpu6050
📄 File: arduino_mpu6050.ino
6142 characters
1class=class="string">"comment">/*
2 Arduino and MPU6050 Accelerometer and Gyroscope Sensor Tutorial
3 by Dejan, https://howtomechatronics.com
4*/
5class="preprocessor">#include <Wire.h>
6const int MPU = 0x68; class=class="string">"comment">// MPU6050 I2C address
7float AccX, AccY, AccZ;
8float GyroX, GyroY, GyroZ;
9float accAngleX, accAngleY, gyroAngleX, gyroAngleY, gyroAngleZ;
10float roll, pitch, yaw;
11float AccErrorX, AccErrorY, GyroErrorX, GyroErrorY, GyroErrorZ;
12float elapsedTime, currentTime, previousTime;
13int c = 0;
14void setup() {
15 Serial.begin(19200);
16 Wire.begin(); class=class="string">"comment">// Initialize comunication
17 Wire.beginTransmission(MPU); class=class="string">"comment">// Start communication with MPU6050 // MPU=0x68
18 Wire.write(0x6B); class=class="string">"comment">// Talk to the register 6B
19 Wire.write(0x00); class=class="string">"comment">// Make reset - place a 0 into the 6B register
20 Wire.endTransmission(true); class=class="string">"comment">//end the transmission
21 class=class="string">"comment">/*
22 // Configure Accelerometer Sensitivity - Full Scale Range(default +/- 2g)
23 Wire.beginTransmission(MPU);
24 Wire.write(0x1C); //Talk to the ACCEL_CONFIG register (1C hex)
25 Wire.write(0x10); //Set the register bits as 00010000 (+/- 8g full scale range)
26 Wire.endTransmission(true);
27 // Configure Gyro Sensitivity - Full Scale Range(default +/- 250deg/s)
28 Wire.beginTransmission(MPU);
29 Wire.write(0x1B); // Talk to the GYRO_CONFIG register (1B hex)
30 Wire.write(0x10); // Set the register bits as 00010000 (1000deg/s full scale)
31 Wire.endTransmission(true);
32 delay(20);
33 */
34 class=class="string">"comment">// Call this function if you need to get the IMU error values for your module
35 calculate_IMU_error();
36 delay(20);
37}
38void loop() {
39 class=class="string">"comment">// === Read acceleromter data === //
40 Wire.beginTransmission(MPU);
41 Wire.write(0x3B); class=class="string">"comment">// Start with register 0x3B(ACCEL_XOUT_H)
42 Wire.endTransmission(false);
43 Wire.requestFrom(MPU, 6, true); class=class="string">"comment">// Read 6 registers total, each axis value is stored in 2 registers
44 class=class="string">"comment">//For a range of +-2g, we need to divide the raw values by 16384, according to the datasheet
45 AccX = (Wire.read() << 8 | Wire.read()) / 16384.0; class=class="string">"comment">// X-axis value
46 AccY = (Wire.read() << 8 | Wire.read()) / 16384.0; class=class="string">"comment">// Y-axis value
47 AccZ = (Wire.read() << 8 | Wire.read()) / 16384.0; class=class="string">"comment">// Z-axis value
48 class=class="string">"comment">// Calculating Roll and Pitch from the accelerometer data
49 accAngleX = (atan(AccY / sqrt(pow(AccX, 2) + pow(AccZ, 2))) * 180 / PI) - 0.58; class=class="string">"comment">// AccErrorX ~(0.58) See the calculate_IMU_error()custom function for more details
50 accAngleY = (atan(-1 * AccX / sqrt(pow(AccY, 2) + pow(AccZ, 2))) * 180 / PI) + 1.58; class=class="string">"comment">// AccErrorY ~(-1.58)
51 class=class="string">"comment">// === Read gyroscope data === //
52 previousTime = currentTime; class=class="string">"comment">// Previous time is stored before the actual time read
53 currentTime = millis(); class=class="string">"comment">// Current time actual time read
54 elapsedTime = (currentTime - previousTime) / 1000; class=class="string">"comment">// Divide by 1000 to get seconds
55 Wire.beginTransmission(MPU);
56 Wire.write(0x43); class=class="string">"comment">// Gyro data first register address 0x43
57 Wire.endTransmission(false);
58 Wire.requestFrom(MPU, 6, true); class=class="string">"comment">// Read 4 registers total, each axis value is stored in 2 registers
59 GyroX = (Wire.read() << 8 | Wire.read()) / 131.0; class=class="string">"comment">// For a 250deg/s range we have to divide first the raw value by 131.0, according to the datasheet
60 GyroY = (Wire.read() << 8 | Wire.read()) / 131.0;
61 GyroZ = (Wire.read() << 8 | Wire.read()) / 131.0;
62 class=class="string">"comment">// Correct the outputs with the calculated error values
63 GyroX = GyroX + 0.56; class=class="string">"comment">// GyroErrorX ~(-0.56)
64 GyroY = GyroY - 2; class=class="string">"comment">// GyroErrorY ~(2)
65 GyroZ = GyroZ + 0.79; class=class="string">"comment">// GyroErrorZ ~ (-0.8)
66 class=class="string">"comment">// Currently the raw values are in degrees per seconds, deg/s, so we need to multiply by sendonds(s) to get the angle in degrees
67 gyroAngleX = gyroAngleX + GyroX * elapsedTime; class=class="string">"comment">// deg/s * s = deg
68 gyroAngleY = gyroAngleY + GyroY * elapsedTime;
69 yaw = yaw + GyroZ * elapsedTime;
70 class=class="string">"comment">// Complementary filter - combine acceleromter and gyro angle values
71 roll = 0.96 * gyroAngleX + 0.04 * accAngleX;
72 pitch = 0.96 * gyroAngleY + 0.04 * accAngleY;
73
74 class=class="string">"comment">// Print the values on the serial monitor
75 Serial.print(roll);
76 Serial.print(class="string">"/");
77 Serial.print(pitch);
78 Serial.print(class="string">"/");
79 Serial.println(yaw);
80}
81void calculate_IMU_error() {
82 class=class="string">"comment">// We can call this funtion in the setup section to calculate the accelerometer and gyro data error. From here we will get the error values used in the above equations printed on the Serial Monitor.
83 class=class="string">"comment">// Note that we should place the IMU flat in order to get the proper values, so that we then can the correct values
84 class=class="string">"comment">// Read accelerometer values 200 times
85 while (c < 200) {
86 Wire.beginTransmission(MPU);
87 Wire.write(0x3B);
88 Wire.endTransmission(false);
89 Wire.requestFrom(MPU, 6, true);
90 AccX = (Wire.read() << 8 | Wire.read()) / 16384.0 ;
91 AccY = (Wire.read() << 8 | Wire.read()) / 16384.0 ;
92 AccZ = (Wire.read() << 8 | Wire.read()) / 16384.0 ;
93 class=class="string">"comment">// Sum all readings
94 AccErrorX = AccErrorX + ((atan((AccY) / sqrt(pow((AccX), 2) + pow((AccZ), 2))) * 180 / PI));
95 AccErrorY = AccErrorY + ((atan(-1 * (AccX) / sqrt(pow((AccY), 2) + pow((AccZ), 2))) * 180 / PI));
96 c++;
97 }
98 class=class="string">"comment">//Divide the sum by 200 to get the error value
99 AccErrorX = AccErrorX / 200;
100 AccErrorY = AccErrorY / 200;
101 c = 0;
102 class=class="string">"comment">// Read gyro values 200 times
103 while (c < 200) {
104 Wire.beginTransmission(MPU);
105 Wire.write(0x43);
106 Wire.endTransmission(false);
107 Wire.requestFrom(MPU, 6, true);
108 GyroX = Wire.read() << 8 | Wire.read();
109 GyroY = Wire.read() << 8 | Wire.read();
110 GyroZ = Wire.read() << 8 | Wire.read();
111 class=class="string">"comment">// Sum all readings
112 GyroErrorX = GyroErrorX + (GyroX / 131.0);
113 GyroErrorY = GyroErrorY + (GyroY / 131.0);
114 GyroErrorZ = GyroErrorZ + (GyroZ / 131.0);
115 c++;
116 }
117 class=class="string">"comment">//Divide the sum by 200 to get the error value
118 GyroErrorX = GyroErrorX / 200;
119 GyroErrorY = GyroErrorY / 200;
120 GyroErrorZ = GyroErrorZ / 200;
121 class=class="string">"comment">// Print the error values on the Serial Monitor
122 Serial.print(class="string">"AccErrorX: ");
123 Serial.println(AccErrorX);
124 Serial.print(class="string">"AccErrorY: ");
125 Serial.println(AccErrorY);
126 Serial.print(class="string">"GyroErrorX: ");
127 Serial.println(GyroErrorX);
128 Serial.print(class="string">"GyroErrorY: ");
129 Serial.println(GyroErrorY);
130 Serial.print(class="string">"GyroErrorZ: ");
131 Serial.println(GyroErrorZ);
132}
Collection of Arduino code examples including sensor reading and array manipulation functions.
VL53L0X GitHub
Adafruit_Sensor GitHub
Generated using C/C++ to HTML Code Generator
Back to list
Older
author-avatar

About Ekostra Electronics

Related Posts