Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- #include <Wire.h>
- #include <SPI.h>
- #include "ADXL345.h"
- #include "LCD.h"
- #include "DEV_Config.h"
- ADXL345 accelerometer;
- void setup() {
- // Inicjalizacja komunikacji
- Serial.begin(9600);
- Wire.begin();
- SPI.begin();
- // Inicjalizacja ADXL345
- accelerometer = ADXL345();
- accelerometer.begin();
- accelerometer.setRange(ADXL345_RANGE_4G);
- // Inicjalizacja LCD
- System_Init();
- LCD_Init(SCAN_DIR_DFT);
- LCD_Clear(BLACK);
- LCD_Show();
- }
- void loop() {
- Vector accRaw = accelerometer.readRaw();
- Vector accNorm = accelerometer.readNormalize();
- float ax = accNorm.XAxis;
- float ay = accNorm.YAxis;
- float az = accNorm.ZAxis;
- // Obliczanie orientacji
- float pitch = atan(ax / sqrt(ay * ay + az * az)) * (180.0 / PI);
- float roll = atan(ay / sqrt(ax * ax + az * az)) * (180.0 / PI);
- float yaw = atan(sqrt(ax * ax + ay * ay) / az) * (180.0 / PI);
- // Konwersja na stringi
- char buffer[50];
- LCD_Clear(BLACK);
- sprintf(buffer, "X: %.2f g", ax);
- LCD_DisplayString(0, 0, buffer, &Font12, BLACK, YELLOW);
- sprintf(buffer, "Y: %.2f g", ay);
- LCD_DisplayString(0, 15, buffer, &Font12, BLACK, YELLOW);
- sprintf(buffer, "Z: %.2f g", az);
- LCD_DisplayString(0, 30, buffer, &Font12, BLACK, YELLOW);
- sprintf(buffer, "Pitch: %.1f", pitch);
- LCD_DisplayString(0, 50, buffer, &Font12, BLACK, WHITE);
- sprintf(buffer, "Roll : %.1f", roll);
- LCD_DisplayString(0, 65, buffer, &Font12, BLACK, WHITE);
- sprintf(buffer, "Yaw : %.1f", yaw);
- LCD_DisplayString(0, 80, buffer, &Font12, BLACK, WHITE);
- delay(500);
- }
Advertisement
Add Comment
Please, Sign In to add comment