Hello, i use MPU6500 to read values from Gyro and Acc so i can combine them to angles. The purpose of it to create PID in the future, but for now i have problems reading those values. I use MPU6050_light library. When i try to apply Complimentary Filter to it and for example i turn the MPU for 90 degrees, for some reason it shows me only 20. I tried a couple of ways to reduce noises and etc but i give up. If somebody can help me to understand what it can be?
#include <Wire.h>
#include <MPU6050_light.h>
MPU6050 mpu(Wire);
unsigned long timer;
float dt;
float roll, pitch; // Filtered angles
float alpha = 0.96; // Complementary filter coefficient (0..1)
void setup() {
Serial.begin(115200);
Wire.begin();
if (mpu.begin() != 0) {
Serial.println("MPU6050 initialization failed");
while (1);
}
Serial.println("Calibrating, please keep the MPU6050 still...");
delay(2000);
mpu.calcOffsets();
Serial.println("Calibration complete!");
timer = millis();
}
void loop() {
mpu.update();
dt = (millis() - timer) / 1000.0; // Time difference in seconds
timer = millis();
// Angles from accelerometer (in degrees)
float rollAcc = atan2(mpu.getAccY(), mpu.getAccZ()) * 180 / PI;
float pitchAcc = atan2(-mpu.getAccX(), sqrt(mpu.getAccY()*mpu.getAccY() + mpu.getAccZ()*mpu.getAccZ())) * 180 / PI;
// Integrate gyroscope data (degrees per second * dt = degrees)
roll += mpu.getGyroX() * dt;
pitch += mpu.getGyroY() * dt;
// Apply complementary filter
roll = alpha * roll + (1 - alpha) * rollAcc;
pitch = alpha * pitch + (1 - alpha) * pitchAcc;
Serial.print("Roll: ");
Serial.print(roll, 2);
Serial.print(" | Pitch: ");
Serial.println(pitch, 2);
delay(10);
}
Update. For example using Arduino Nano Connect RP2040 gives me quite good numbers and i don't have any problem with angles. Using inbuilt Gyro and Acc inside of RP2040