09.電子水平儀(GY-521)

#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("        ");

}

}