#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050.h"
#include <LiquidCrystal_I2C.h>
#include <Kalman.h>
LiquidCrystal_I2C lcd(0x3F,16,2); // 若LCD 無法啟用，請將0X3F 改為0X27
Kalman kalmanX , kalmanY, kalmanZ;
MPU6050 accelgyro;
int16_t ax, ay, az, gx, gy, gz;
double accX, accY, accZ, gyroX, gyroY, gyroZ;
uint32_t timer;
double pitch, roll, yaw, AngleX, AngleY, AngleZ, Xrate, Yrate, Zrate;
void setup() {
Wire.begin();
lcd.init();
lcd.backlight();
lcd.clear();
Serial.begin(115200);
accelgyro.initialize();
accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
accX = ax; accY = ay; accZ = az; gyroX = gx; gyroY = gy; gyroZ = gz;
pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG;
roll = atan2(accY , accZ) * RAD_TO_DEG;
yaw = atan2(accX , accY) * RAD_TO_DEG;
kalmanY.setAngle(pitch);
kalmanX.setAngle(roll);
kalmanZ.setAngle(yaw);
}
void loop() {
accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
accX = ax;
accY = ay;
accZ = az;
gyroX = gx;
gyroY = gy;
gyroZ = gz;
double dt = (double)(micros() - timer) / 1000000; // Calculate delta time
timer = micros();
pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG;
roll = atan2(accY,accZ) * RAD_TO_DEG;
yaw = atan2(accX , accY) * RAD_TO_DEG;
Yrate = gyroY / 131.0; // Convert to deg/s
Xrate = gyroX / 131.0;
Zrate = gyroZ / 131.0;
AngleY = kalmanY.getAngle(pitch, Yrate, dt);
AngleX = kalmanX.getAngle(roll, Xrate, dt);
AngleZ = kalmanZ.getAngle(yaw, Zrate, dt);
disp();
}
void disp(){
if(AngleX>0){
lcd.setCursor(0,0);
lcd.print("X=");
lcd.setCursor(2,0);
lcd.print("+");
lcd.print(AngleX,1);
lcd.print(" ");
}else{
lcd.setCursor(0,0);
lcd.print("X=");
lcd.print(AngleX,1);
lcd.print(" ");
}
if(AngleY>0){
lcd.setCursor(8,0);
lcd.print("Y=");
lcd.setCursor(10,0);
lcd.print("+");
lcd.print(AngleY,1);
lcd.print(" ");
}else{
lcd.setCursor(8,0);
lcd.print("Y=");
lcd.print(AngleY,1);
lcd.print(" ");
}
if(AngleZ>0){
lcd.setCursor(0,1);
lcd.print("Z=");
lcd.setCursor(2,1);
lcd.print("+");
lcd.print(AngleZ,1);
lcd.print(" ");
}else{
lcd.setCursor(0,1);
lcd.print("Z=");
lcd.print(AngleZ,1);
lcd.print(" ");
}
}