Linia 1. kratka include otwórz nawias ostrokątny LiquidCrystal kropka h zamknij nawias ostrokątny. Linia 2. kratka include otwórz nawias ostrokątny Wire kropka h zamknij nawias ostrokątny. Linia 3. kratka include otwórz nawias ostrokątny MPU6050 kropka h zamknij nawias ostrokątny. Linia 5. LiquidCrystal lcd otwórz nawias okrągły 7 przecinek 6 przecinek 5 przecinek 4 przecinek 3 przecinek 2 zamknij nawias okrągły średnik. Linia 7. MPU6050 mpu średnik. Linia 9. unsigned long timer znak równości 0 średnik. Linia 10. float timeStep znak równości 0 kropka 01 średnik. Linia 12. float pitch znak równości 0 średnik. Linia 13. float roll znak równości 0 średnik. Linia 14. float yaw znak równości 0 średnik. Linia 16. void setup otwórz nawias okrągły zamknij nawias okrągły otwórz nawias klamrowy. Linia 17. Serial kropka begin otwórz nawias okrągły 115200 zamknij nawias okrągły średnik. Linia 18. lcd kropka begin otwórz nawias okrągły 16 przecinek 2 zamknij nawias okrągły średnik. Linia 20. while otwórz nawias okrągły wykrzyknik mpu kropka begin otwórz nawias okrągły MPU6050 podkreślnik SCALE podkreślnik 2000DPS przecinek MPU6050 podkreślnik RANGE podkreślnik 2G zamknij nawias okrągły zamknij nawias okrągły. Linia 21. otwórz nawias klamrowy. Linia 22. Serial kropka println otwórz nawias okrągły cudzysłów Błąd czujnika MPU6050 wykrzyknik Sprawdź połączenia wykrzyknik cudzysłów zamknij nawias okrągły średnik. Linia 23. lcd kropka setCursor otwórz nawias okrągły 0 przecinek 0 zamknij nawias okrągły średnik. Linia 24. lcd kropka print otwórz nawias okrągły cudzysłów Blad odczytu cudzysłów zamknij nawias okrągły średnik. Linia 25. lcd kropka setCursor otwórz nawias okrągły 0 przecinek 1 zamknij nawias okrągły średnik. Linia 26. lcd kropka print otwórz nawias okrągły cudzysłów czujnika MPU6050 cudzysłów zamknij nawias okrągły średnik. Linia 27. delay otwórz nawias okrągły 500 zamknij nawias okrągły średnik. Linia 28. zamknij nawias klamrowy. Linia 29. lcd kropka clear otwórz nawias okrągły zamknij nawias okrągły średnik. Linia 30. lcd kropka setCursor otwórz nawias okrągły 0 przecinek 0 zamknij nawias okrągły średnik. Linia 31. lcd kropka print otwórz nawias okrągły cudzysłów Polaczenie udane cudzysłów zamknij nawias okrągły średnik. Linia 32. lcd kropka setCursor otwórz nawias okrągły 4 przecinek 1 zamknij nawias okrągły średnik. Linia 33. lcd kropka print otwórz nawias okrągły cudzysłów Zyroskop cudzysłów zamknij nawias okrągły średnik. Linia 34. delay otwórz nawias okrągły 2000 zamknij nawias okrągły średnik. Linia 35. mpu kropka calibrateGyro otwórz nawias okrągły zamknij nawias okrągły średnik. Linia 37. mpu kropka setThreshold otwórz nawias okrągły 3 zamknij nawias okrągły średnik. Linia 38. zamknij nawias klamrowy. Linia 40. void loop otwórz nawias okrągły zamknij nawias okrągły otwórz nawias klamrowy. Linia 41. lcd kropka clear otwórz nawias okrągły zamknij nawias okrągły średnik. Linia 42. timer znak równości millis otwórz nawias okrągły zamknij nawias okrągły średnik. Linia 44. Vector norm znak równości mpu kropka readNormalizeGyro otwórz nawias okrągły zamknij nawias okrągły średnik. Linia 46. pitch znak równości pitch plus norm kropka YAxis asterysk timeStep średnik. Linia 47. roll znak równości roll plus norm kropka XAxis asterysk timeStep średnik. Linia 48. yaw znak równości yaw plus norm kropka ZAxis asterysk timeStep średnik. Linia 50. lcd kropka setCursor otwórz nawias okrągły 0 przecinek 0 zamknij nawias okrągły średnik. Linia 51. lcd kropka print otwórz nawias okrągły cudzysłów P cudzysłów zamknij nawias okrągły średnik. Linia 52. lcd kropka print otwórz nawias okrągły pitch zamknij nawias okrągły średnik. Linia 53. lcd kropka setCursor otwórz nawias okrągły 9 przecinek 0 zamknij nawias okrągły średnik. Linia 54. lcd kropka print otwórz nawias okrągły cudzysłów R cudzysłów zamknij nawias okrągły średnik. Linia 55. lcd kropka print otwórz nawias okrągły roll zamknij nawias okrągły średnik. Linia 56. lcd kropka setCursor otwórz nawias okrągły 0 przecinek 1 zamknij nawias okrągły średnik. Linia 57. lcd kropka print otwórz nawias okrągły cudzysłów Y cudzysłów zamknij nawias okrągły średnik. Linia 58. lcd kropka print otwórz nawias okrągły yaw zamknij nawias okrągły średnik. Linia 60. Serial kropka print otwórz nawias okrągły cudzysłów Pitch znak równości cudzysłów zamknij nawias okrągły średnik. Linia 61. Serial kropka print otwórz nawias okrągły pitch zamknij nawias okrągły średnik. Linia 62. Serial kropka print otwórz nawias okrągły cudzysłów Roll znak równości cudzysłów zamknij nawias okrągły średnik. Linia 63. Serial kropka print otwórz nawias okrągły roll zamknij nawias okrągły średnik. Linia 64. Serial kropka print otwórz nawias okrągły cudzysłów Yaw znak równości cudzysłów zamknij nawias okrągły średnik. Linia 65. Serial kropka println otwórz nawias okrągły yaw zamknij nawias okrągły średnik. Linia 67. delay otwórz nawias okrągły 500 zamknij nawias okrągły średnik. Linia 68. zamknij nawias klamrowy.

#include <LiquidCrystal.h>
#include <Wire.h>
#include <MPU6050.h>

LiquidCrystal lcd (7, 6, 5, 4, 3, 2);

MPU6050 mpu;

unsigned long timer = 0;
float timeStep = 0.01;

float pitch = 0;
float roll = 0;
float yaw = 0;

void setup() {
  Serial.begin(115200);
  lcd.begin (16, 2);

  while(!mpu.begin(MPU6050_SCALE_2000DPS, MPU6050_RANGE_2G))
  {
    Serial.println("Błąd czujnika MPU6050! Sprawdź połączenia!");
    lcd.setCursor (0, 0);
    lcd.print("  Blad odczytu  ");
    lcd.setCursor (0, 1);
    lcd.print("czujnika MPU6050");
    delay(500);
  }
  lcd.clear ();
  lcd.setCursor (0, 0);
  lcd.print ("Polaczenie udane");
  lcd.setCursor (4, 1);
  lcd.print ("Zyroskop");
  delay (2000);
  mpu.calibrateGyro();

  mpu.setThreshold(3);
}

void loop () {
  lcd.clear ();
  timer = millis();

  Vector norm = mpu.readNormalizeGyro();

  pitch = pitch + norm.YAxis * timeStep;
  roll = roll + norm.XAxis * timeStep;
  yaw = yaw + norm.ZAxis * timeStep;

  lcd.setCursor (0, 0);
  lcd.print ("P");
  lcd.print(pitch);
  lcd.setCursor (9, 0);
  lcd.print ("R");
  lcd.print(roll);
  lcd.setCursor (0, 1);
  lcd.print ("Y");
  lcd.print(yaw);
  
  Serial.print(" Pitch = ");
  Serial.print(pitch);
  Serial.print(" Roll = ");
  Serial.print(roll);  
  Serial.print(" Yaw = ");
  Serial.println(yaw);

  delay(500);
}