pierdziadek

lab7

Apr 13th, 2025
51
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 1.57 KB | None | 0 0
  1. #include <Wire.h>
  2. #include <SPI.h>
  3. #include "ADXL345.h"
  4. #include "LCD.h"
  5. #include "DEV_Config.h"
  6.  
  7. ADXL345 accelerometer;
  8.  
  9. void setup() {
  10. // Inicjalizacja komunikacji
  11. Serial.begin(9600);
  12. Wire.begin();
  13. SPI.begin();
  14.  
  15. // Inicjalizacja ADXL345
  16. accelerometer = ADXL345();
  17. accelerometer.begin();
  18. accelerometer.setRange(ADXL345_RANGE_4G);
  19.  
  20. // Inicjalizacja LCD
  21. System_Init();
  22. LCD_Init(SCAN_DIR_DFT);
  23. LCD_Clear(BLACK);
  24. LCD_Show();
  25. }
  26.  
  27. void loop() {
  28. Vector accRaw = accelerometer.readRaw();
  29. Vector accNorm = accelerometer.readNormalize();
  30.  
  31. float ax = accNorm.XAxis;
  32. float ay = accNorm.YAxis;
  33. float az = accNorm.ZAxis;
  34.  
  35. // Obliczanie orientacji
  36. float pitch = atan(ax / sqrt(ay * ay + az * az)) * (180.0 / PI);
  37. float roll = atan(ay / sqrt(ax * ax + az * az)) * (180.0 / PI);
  38. float yaw = atan(sqrt(ax * ax + ay * ay) / az) * (180.0 / PI);
  39.  
  40. // Konwersja na stringi
  41. char buffer[50];
  42. LCD_Clear(BLACK);
  43.  
  44. sprintf(buffer, "X: %.2f g", ax);
  45. LCD_DisplayString(0, 0, buffer, &Font12, BLACK, YELLOW);
  46.  
  47. sprintf(buffer, "Y: %.2f g", ay);
  48. LCD_DisplayString(0, 15, buffer, &Font12, BLACK, YELLOW);
  49.  
  50. sprintf(buffer, "Z: %.2f g", az);
  51. LCD_DisplayString(0, 30, buffer, &Font12, BLACK, YELLOW);
  52.  
  53. sprintf(buffer, "Pitch: %.1f", pitch);
  54. LCD_DisplayString(0, 50, buffer, &Font12, BLACK, WHITE);
  55.  
  56. sprintf(buffer, "Roll : %.1f", roll);
  57. LCD_DisplayString(0, 65, buffer, &Font12, BLACK, WHITE);
  58.  
  59. sprintf(buffer, "Yaw : %.1f", yaw);
  60. LCD_DisplayString(0, 80, buffer, &Font12, BLACK, WHITE);
  61.  
  62. delay(500);
  63. }
  64.  
Advertisement
Add Comment
Please, Sign In to add comment