Arduino Polska Forum
Próbuje napisać kod strikte do łódki zanentowej - Wersja do druku

+- Arduino Polska Forum (https://forum.arduinopolska.pl)
+-- Dział: Korzystanie z Arduino (https://forum.arduinopolska.pl/dzial-korzystanie-z-arduino)
+--- Dział: Piaskownica (https://forum.arduinopolska.pl/dzial-piaskownica)
+--- Wątek: Próbuje napisać kod strikte do łódki zanentowej (/watek-pr%C3%B3buje-napisa%C4%87-kod-strikte-do-%C5%82%C3%B3dki-zanentowej)

Próbuje napisać kod strikte do łódki zanentowej - Dzimi87 - 09-07-2025

Mam już mnóstwo kodu i  już pływa, ale mam problem pływaniem w lini prostej.
Używam starego APM jako bazy ardurower średnio się sprawdza do tego celu.
Na razie używam samego GPS i kompasu, bez żyroskopu bo go wcale nie rozumiem..
Chciał bym się dowiedzieć czy kod wygląda dobrze dla łódki...

Kod:
#include "HMC5883L.h"
#include "ReadEprom.h"
#include <Wire.h>


void HMC5883L_Init() {
 
  Wire.begin(); 

  Wire.beginTransmission(HMC5883L_ADDRESS);
  Wire.write(0x00); // Rejestr konfiguracji A
  Wire.write(0x70); // 8 pomiarów na średnią, 15Hz(0x70) lub 30Hz(0x78)
  Wire.endTransmission();

  Wire.beginTransmission(HMC5883L_ADDRESS);
  Wire.write(0x01); // Rejestr konfiguracji B
  Wire.write(0x20); // Ustawienie wzmocnienia 40
  Wire.endTransmission();

  Wire.beginTransmission(HMC5883L_ADDRESS);
  Wire.write(0x02); // Rejestr trybu
  Wire.write(0x00); // Tryb ciągłego pomiaru
  Wire.endTransmission();
  Wire.requestFrom(HMC5883L_ADDRESS, 6);
}

// Zmienne globalne
float offsetX = 56.50, offsetY = -215.00, offsetZ = 0.0;
float declinationAngle = 5.0;
float rotationAngle = 90.0;
float heading = 0;
bool isUpsideDown = false;  // Domyślnie czujnik jest w normalnej orientacji
float x;
float y;

unsigned long os_czas_kom = 0;
bool AwariaKom = false;


void readRawData(int16_t &x, int16_t &y, int16_t &z) {
 
  // Ustaw wskaźnik na pierwszy rejestr danych (0x03)
  Wire.beginTransmission(HMC5883L_ADDRESS);
  Wire.write(0x03);
  Wire.endTransmission();
 
  Wire.requestFrom(HMC5883L_ADDRESS, 6);
  if (Wire.available() == 6) {
    x = (Wire.read() << 8) | Wire.read();
    z = (Wire.read() << 8) | Wire.read();
    y = (Wire.read() << 8) | Wire.read();

    os_czas_kom = millis();
    AwariaKom = false;
    }
}


unsigned long lastReadTime = 0; // Zmienna globalna

void Read_HMC5883L() {

  if (millis() - lastReadTime > 70) {  // Odczyt co 70 ms
 
  int16_t rawX, rawY, rawZ;
  readRawData(rawX, rawY, rawZ);

  if (isUpsideDown) { 
    rawY =  -rawY;
    x = rawX - offsetX;
    y = rawY + offsetY;
  }else{
  x = rawX - offsetX;
  y = rawY - offsetY;
  }


  heading = atan2(y, x) * 180.0 / M_PI;
  heading += declinationAngle;
  heading += rotationAngle;

  if (heading < 0) heading += 360.0;
  if (heading >= 360) heading -= 360.0;


    if (millis() - os_czas_kom > 3000){
    heading = 0;
    AwariaKom = true;
    }

    lastReadTime = millis();  // Aktualizacja czasu odczytu
    }
}

void calibrateXY() {
  int16_t rawX, rawY, rawZ;
  int16_t minX = 32767, minY = 32767, maxX = -32768, maxY = -32768;
  char userInput;

  // Zapytaj użytkownika o potwierdzenie rozpoczęcia kalibracji
  Serial.println(F("Czy chcesz rozpoczac kalibracje kompasu(T/N)"));
  Serial.println(F("Naciskajac 'T' masz 5s do obracania lodka"));
 
  // Czekaj na odpowiedź użytkownika
  while (true) {
    if (Serial.available() > 0) {
      userInput = Serial.read();  // Odczytanie odpowiedzi użytkownika
      if (userInput == 'T') {  // Jeżeli odpowiedź to 't' (tak)
        Serial.println(F("Rozpoczynam kalibracje..."));
        break;  // Kontynuuj kalibrację
      } else if (userInput == 'N') {  // Jeżeli odpowiedź to 'n' (nie)
        Serial.println(F("Kalibracja anulowana."));
        return;  // Zakończ funkcję, kalibracja nie zostanie wykonana
      }
    }
  }
  delay(5000);
  Serial.println(F("Obracaj lodka przez 10s"));
 
 
  // Zbieranie danych przez 10 sekund
  for (int i = 0; i < 5000; i++) {  // 5000 próbek w 10 sekund
    readRawData(rawX, rawY, rawZ);

    // Szukaj minimum i maksimum dla obu osi X i Y
    if (rawX < minX) minX = rawX;
    if (rawX > maxX) maxX = rawX;
    if (rawY < minY) minY = rawY;
    if (rawY > maxY) maxY = rawY;

    delay(2);  // opóźnienia dla większej liczby próbek
  }

  // Obliczanie ofsetów na podstawie minimalnych i maksymalnych wartości
  offsetX = (maxX + minX) / 2.0;
  offsetY = (maxY + minY) / 2.0;

  // Zapisz ofsety do pamięci eprom i wyswietl
  SaveOfsetCompas();
  delay(10);
  Serial.print(F("Ofset X: "));
  Serial.println(offsetX);
  Serial.print(F("Ofset Y: "));
  Serial.println(offsetY);

  // Kalibracja zakończona
  Serial.println(F("Kalibracja zakonczona"));
}


// Funkcja do zmiany deklinacji przez UART z potwierdzeniem
void changeDeclinationAngle() {
  // Wyświetlanie obecnej wartości deklinacji
  Serial.print(F("Aktualna deklinacja: "));
  Serial.println(declinationAngle);
 
  Serial.println(F("Czy chcesz zmienic wartosc deklinacji? (T/N)"));
  Serial.println(F("www.magnetic-declination.com"));
 
  char userInput;
  while (true) {
    if (Serial.available() > 0) {
      userInput = Serial.read();  // Odczytanie odpowiedzi użytkownika
      if (userInput == 'T') {  // Jeżeli odpowiedź to 't' (tak)
        Serial.println(F("Podaj nowa wartosc deklinacji:"));
        break;  // Kontynuuj zmianę deklinacji
      } else if (userInput == 'N') {  // Jeżeli odpowiedź to 'n' (nie)
        Serial.println(F("Zmiana deklinacji anulowana."));
        return;  // Zakończ funkcję, nie zmieniaj deklinacji
      }
    }
  }
 
 
  // Oczekiwanie na wprowadzenie nowej wartości deklinacji
  while (true) {
    if (Serial.available() > 0) {
      String input = Serial.readStringUntil('\n');  // Odczytanie wejścia użytkownika
      input.trim();  // Usuń zbędne białe znaki

      // Sprawdź, czy wprowadzenie jest liczbą
      float newDeclination = input.toFloat();
      if (newDeclination != 0.0 || input == "0") {
        declinationAngle = newDeclination;  // Ustaw nową deklinację
        SaveDeclinationCompas();
        delay(10);
        Serial.print(F("Nowa deklinacja: "));
        Serial.println(declinationAngle);
        break;
      } else {
        Serial.println(F(""));
      }
    }
  }
}

void changeRotationAngle() {
  // Wyświetlanie obecnej wartości kąta obrotu
  Serial.println(F(""));
  Serial.print(F("Aktualny kat obrotu: "));
  Serial.print(rotationAngle);
  Serial.println(F("°"));
  Serial.println(F(""));
 
  Serial.println(F("Wybierz opcje zmiany kata obrotu:"));
  Serial.println(F("1.Ustaw kat 0°"));
  Serial.println(F("2.Ustaw kat 90° (wew APM)"));
  Serial.println(F("3.Ustaw kat 180°"));
  Serial.println(F("4.Ustaw kat 270°"));
  Serial.println(F("5.Wprowadz dowolny kat (1-359°)"));
  Serial.println(F("0.Powrot"));

  while (true) {
    if (Serial.available() > 0) {
      char userInput = Serial.read();  // Odczytanie wyboru użytkownika

      // Usuń znaki końca linii z bufora
      if (userInput == '\n' || userInput == '\r') {
        continue;
      }
     
      switch (userInput) {
        case '1':
          rotationAngle = 0;
          SaveRotationCompas();
          Serial.println(F("Kat obrotu ustawiony na 0°."));
          return;  // Wyjście z funkcji
        case '2':
          rotationAngle = 90;
          SaveRotationCompas();
          Serial.println(F("Kat obrotu ustawiony na 90°."));
          return;  // Wyjście z funkcji
        case '3':
          rotationAngle = 180;
          SaveRotationCompas();
          Serial.println(F("Kat obrotu ustawiony na 180°."));
          return;  // Wyjście z funkcji
        case '4':
          rotationAngle = 270;
          SaveRotationCompas();
          Serial.println(F("Kat obrotu ustawiony na 270°."));
          return;  // Wyjście z funkcji
        case '5':

        Serial.println(F(""));
        Serial.println(F("Podaj nową wartosc kata obrotu (1-359°)"));
       
while (true) {
    if (Serial.available() > 0) {
      String input = Serial.readStringUntil('\n');  // Odczytanie wejścia użytkownika
      input.trim();  // Usuń zbędne białe znaki

      // Sprawdź, czy wprowadzenie jest liczbą
      float newRotationAngle = input.toFloat();
      // Sprawdzamy, czy kąt mieści się w zakresie 1-359
      if (newRotationAngle >= 1.0 && newRotationAngle <= 359.0) {
        rotationAngle = newRotationAngle;  // Ustaw nowy kąt obrotu
        SaveRotationCompas();
        delay(10);
        Serial.print(F("Nowy kat obrotu: "));
        Serial.print(rotationAngle);
        Serial.println(F("°"));
        return;  // Zakończ funkcję
      }
    }
  }
        return;
        case '0':
          return;  // Wyjście z funkcji

      }
    }
  }
}

void changeOrientation() {
  Serial.print(F("Aktualna pozycja: "));
  if (isUpsideDown) { 
  Serial.println(F("do gory nogami"));
  }else{
  Serial.println(F("normalna"));
  }
  Serial.println(F("Wybierz orientacje kompasu:"));
  Serial.println(F("1.Normalna orientacja(wew APM)"));
  Serial.println(F("2.Czujnik wlutowany do gory nogami"));
  Serial.println(F("0.Powrot"));

  while (true) {
    if (Serial.available() > 0) {
      char userInput = Serial.read();  // Odczytanie wyboru użytkownika

      // Usuń znaki końca linii z bufora
      if (userInput == '\n' || userInput == '\r') {
        continue;
      }
     
      switch (userInput) {
        case '1':
          isUpsideDown = false;  // Normalna orientacja
          SaveUpSideCompas();
          Serial.println(F("Czujnik ustawiony w normalnej orientacji."));
          return;
        case '2':
          isUpsideDown = true;  // Odwrócona orientacja
          SaveUpSideCompas();
          Serial.println(F("Czujnik ustawiony do gory nogami."));
          return;
        case '0':
          return;  // Wyjście z funkcji
      }
    }
  }
}

 
 
 
i
Kod:
#ifndef HMC5883L_H
#define HMC5883L_H
#include <Arduino.h>

// Adres czujnika HMC5883L
#define HMC5883L_ADDRESS 0x1E

void HMC5883L_Init();    //INICJALIZACJA

extern float heading;    //DANE KOMPAU
extern bool  AwariaKom;  //AWARIA KOMPASU

extern float offsetX;
extern float offsetY;
extern float offsetZ;

extern float declinationAngle;
extern float rotationAngle;
extern bool  isUpsideDown;

// FUNKCJE
void calibrateXY();
void Read_HMC5883L();
void changeDeclinationAngle();
void changeRotationAngle();
void changeOrientation();

#endif

Może by ktoś chętny co nie co poprawił lub podpowiedział....Sprawdził i utwierdził by mnie czy to działa prawidłowo.      Bo niby wskazania są ale nie wiem czy wystarczająco dokładne.