Położenie robota xy + mapa +filtr - aktualizacja

[Bazuka1988]

Witam, czy pomógłby mi ktoś pomóc zaimplementować to co wrzuciłem w załączniku w PDFie? Uzywam ESP i Arduino IDE. Dane wysyłam do Matlaba po bluetooth. Rysuje to co widać w załącznikach. Pojawia się błąd inkrementalny, z każdym przejechanym okrążeniem. Robot jeździ po zadanej ścieżce śledząc linię przy pomocy 2 czujników odbiciowych i skanuje otoczenie za pomocą 2 czujników ultradźwiękowych hc-sr04. Ustawione zostały na trasie "znaczniki" (w moim wypadku kartony), które mają służyć za punkt odniesienia i po wprowadzeniu filtru będą służyć do korygowania położenia. Położenie jest obliczane przy pomocy enkoderów kwadraturowych 1920imp/obrót. Robot jest trójkołowy. A może ktoś ma pomysł jak można jeszcze zmniejszyć ten błąd inkrementalny? Obecnie jak widać po screenach, trasa jest rysowana jak i odczyty z czujników. Rysuję to w matlabie.
Sterowaniem silników przy pomocy reg. PID i śledzeniem linii zajmuje się Arduino Nano, reszta jest na ESP.
Kod z Matlaba (sama pętla):

while (true)
  
x=fscanf(bt,'%f')
y=fscanf(bt,'%f')
Lx=fscanf(bt,'%f')
Ly=fscanf(bt,'%f')
Sx=fscanf(bt,'%f')
Sy=fscanf(bt,'%f')

xn=xn+x;
yn=yn+y;

plot(xn,yn,'black*',xn+Lx, yn+Ly,'red.', xn+Sx, yn+Sy,'green.');
hold on
  
end

Kod z Arduino Nano (sterowanie silnikami, reg.PID, czujniki odbiciowe):

#define R2_LINE_SENSOR A2
#define L2_LINE_SENSOR A1
#define RMOTOR 5
#define LMOTOR 6
#define IN1 7
#define IN2 8
#define IN3 9
#define IN4 10

int g=200, vstart=60, pochodna=0, poprzedni=0, calka=0, PID=0; //"g"-granica widzialnosci czarnej lini; "vstart"-predkosc stala
float blad=0, Kp=2, Ki=0.4, Kd=0.1; //nastawy regulatora PID

void setup() {
  
//Zasilanie +++
  pinMode (3, OUTPUT);//odbiciowy prawy
  digitalWrite(3, HIGH);
  pinMode (12, OUTPUT);//odbiciowy lewy
  digitalWrite(12, HIGH);
 
//Masa ---
  pinMode (4, OUTPUT);//odbiciowy prawy
  digitalWrite(4, LOW);
  pinMode (11, OUTPUT);//odbiciowy lewy
  digitalWrite(11, LOW);


//Czujniki odbiciowe
  pinMode(R2_LINE_SENSOR, INPUT);
  pinMode(L2_LINE_SENSOR, INPUT);
  
//Silniki
  pinMode (LMOTOR, OUTPUT);//lewy
  pinMode (IN1, OUTPUT);//przod
  pinMode (IN2, OUTPUT);//tyl
  pinMode (RMOTOR, OUTPUT);//prawy
  pinMode (IN3, OUTPUT);//przod
  pinMode (IN4, OUTPUT);//tyl
  analogWrite(LMOTOR, 0); //Ustawienie predkosci  
  digitalWrite(IN1, LOW); //Kierunek: do przodu
  digitalWrite(IN2, LOW); //Kierunek: do tylu
  analogWrite(RMOTOR, 0); //Ustawienie predkosci
  digitalWrite(IN3, LOW); //Kierunek: do przodu
  digitalWrite(IN4, LOW); //Kierunek: do tylu

    Serial.begin(9600);

    delay(300);
}

//********************************************
//*****************FUNKCJE********************
//********************************************

//---------------------Wyliczenie bledu
 int error () {
   if (analogRead(L2_LINE_SENSOR) < g && analogRead(R2_LINE_SENSOR) < g) { //nie widza lini
    blad=1;
  } 
    else if (analogRead(L2_LINE_SENSOR) > g && analogRead(R2_LINE_SENSOR) < g) { //lewy widzi linie
    blad=2; 
  } else if (analogRead(L2_LINE_SENSOR) < g && analogRead(R2_LINE_SENSOR) > g) { //prawy widzi linie
    blad=-2;
  }
  return blad;
  } 

//-----------------------------PID
void regPID () {
   pochodna=blad-poprzedni;
   poprzedni=blad-poprzedni;
   calka=constrain(calka+blad, -50, 50);
   PID=round(abs((Kp*blad)+(Kd*pochodna)+(Ki*calka)));

    if (blad == 1) {//jazda prosto
    leftMotor(vstart); 
    rightMotor(vstart);
    
  } else if (blad == 2) { //lewy widzi linie
    leftMotor(vstart-PID); 
    rightMotor(vstart+PID); 
    
  } else if (blad == -2) {  //prawy widzi linie
    leftMotor(vstart+PID); 
    rightMotor(vstart-PID);
     
  }
 }
 
//-------------------Sterowanie silnikami
void leftMotor(int V) {
  if (V>0) {
  //V=constrain(V, 0, 256);
  analogWrite(LMOTOR, V); //Ustawienie predkosci
  digitalWrite(IN1, HIGH);
  digitalWrite(IN2, LOW);

  } else {  //stop
    //V=abs(V);
    analogWrite(LMOTOR, 0); //Ustawienie predkosci
    digitalWrite(IN1, LOW);
    digitalWrite(IN2, LOW);
  }
}

void rightMotor(int V) {
  if (V>0) { 
  //V=constrain(V, 0, 256);
  analogWrite(RMOTOR, V); //Ustawienie predkosci
  digitalWrite(IN3, HIGH);
  digitalWrite(IN4, LOW);
 
  } else {   //stop
    //V=abs(V);
    analogWrite(RMOTOR, 0); //Ustawienie predkosci
    digitalWrite(IN3, LOW);
    digitalWrite(IN4, LOW);
  }
}

void stopMotors() {
  analogWrite(LMOTOR, 0); //Ustawienie predkosci
  analogWrite(RMOTOR, 0); //Ustawienie predkosci
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, LOW);
  digitalWrite(IN4, LOW);
  delay(1000);
}

//--------------PETLA GLOWNA--------------
void loop() {
  error();
  regPID();

}

Kod z ESP (enkodery, czujniki ultradźwiękowe, obliczanie pozycji i położenia przeszkód):

//biblioteki
#include <ESP32Encoder.h>//enkodery kwadraturowe https://github.com/madhephaestus/ESP32Encoder

//czujniki ultradzwiekowe
//srodkowy
#define SechoHSCR04 17
#define StrigHSCR04 2
//lewy
#define LechoHSCR04 5
#define LtrigHSCR04 0

//bluetooth
#include "BluetoothSerial.h"
#if !defined(CONFIG_BT_ENABLED) || !defined(CONFIG_BLUEDROID_ENABLED)
#error Bluetooth is not enabled! Please run `make menuconfig` to and enable it
#endif
BluetoothSerial SerialBT;
//---------------------------

float enkoderLewy=0, enkoderPrawy=0, x=0, y=0, teta=0, tetaKat=0, d=6.5, l=0, L=12.50, dystansL=0, dystansP=0, dystansS=0;  //"r"-promien kola; "L"-odleglosc pomiedzy kolami mierzona od osi kola
float Ldystans=0, LdystansX=0, LdystansY=0, Sdystans=0, SdystansX=0, SdystansY=0, Pdystans=0, PdystansX=0, PdystansY=0; //ultradzwiekowe

unsigned long aktualnyCzas = 0;
unsigned long zapamietanyCzas = 0;

//Enkodery kwadraturowe
ESP32Encoder encoder;//lewy
ESP32Encoder encoder2;//prawy

void setup() {
  
l=d*PI; //obliczenie obwodu kola
  
//------------------------------------------------------------Enkodery
  ESP32Encoder::useInternalWeakPullResistors=UP;

  encoder.attachHalfQuad(35, 34);
  encoder2.attachHalfQuad(25, 26);
    
  encoder.clearCount();
  encoder2.clearCount();
//--------------------------------------------------------------------

//---------------------------------------------Czujniki ultradzwiekowe
  //srodek
  pinMode(SechoHSCR04, INPUT);
  pinMode(StrigHSCR04, OUTPUT);
  //lewy
  pinMode(LechoHSCR04, INPUT);
  pinMode(LtrigHSCR04, OUTPUT);
//--------------------------
  Serial.begin(115200);//predkosc transmisji
  //bluetooth
  SerialBT.begin("ESP32Pawel");//nazwa widziana po BT
}

void loop() {

    aktualnyCzas = millis();
  if (aktualnyCzas - zapamietanyCzas >= 200UL) {
    zapamietanyCzas = aktualnyCzas;
    pozycja();
  }
}

//--------------------------------------------Pozycja
void pozycja() {
  //droga przejechana przez kazde z kol
  enkoderLewy=((int32_t)encoder.getCount());
  dystansL=((enkoderLewy/1920)*l);
  enkoderPrawy=((int32_t)encoder2.getCount());
  dystansP=((enkoderPrawy/1920)*l);
  //kata o jaki obrocil sie robot
  tetaKat=tetaKat+((dystansP-dystansL)/L);
  //pozycja
  dystansS=(dystansP+dystansL)/2;
  x=dystansS*(cos(tetaKat));
  y=dystansS*(sin(tetaKat));
  SerialBT.println(x);
  SerialBT.println(y);

//---------------Pomiar odleglosci czujnika lewego
    float czasL=0;
    digitalWrite(LtrigHSCR04, LOW);
    delayMicroseconds(2);
    digitalWrite(LtrigHSCR04, HIGH);
    delayMicroseconds(10);
    digitalWrite(LtrigHSCR04, LOW);
    czasL = pulseIn(LechoHSCR04, HIGH);
    Ldystans = ((czasL / 58)-4);
    LdystansX=(Ldystans)*(cos(tetaKat+1.57));
    LdystansY=(Ldystans)*(sin(tetaKat+1.57));
    SerialBT.println(LdystansX);
    SerialBT.println(LdystansY);

//---------------Pomiar odleglosci czujnika srodkowego
    float czasS=0;
    digitalWrite(StrigHSCR04, LOW);
    delayMicroseconds(2);
    digitalWrite(StrigHSCR04, HIGH);
    delayMicroseconds(10);
    digitalWrite(StrigHSCR04, LOW);
    czasS = pulseIn(SechoHSCR04, HIGH);
    Sdystans = ((czasS / 58)-1);
    SdystansX=Sdystans*(cos(tetaKat));
    SdystansY=Sdystans*(sin(tetaKat));
    SerialBT.println(SdystansX);
    SerialBT.println(SdystansY);

  encoder.clearCount();
  encoder2.clearCount();
  enkoderLewy=0;
  enkoderPrawy=0;
}

Do tego wszystkiego potrzebuję dorzucić rozszerzony filtr Kalmana, a konkretniej to co mam w pdfie poniżej. Jeżeli dobrze rozumiem to (odnośnie zawartości pdf):
1 Równanie stanu
s_t = s_t-1 + Δs_t + ε_t
gdzie: s_t – położenie wzdłuż ścieżki w chwili t; Δs_t – odległość przebyta przez robota między
chwilami t-1 i t; ε_t – zakłócenie losowe (zakłada się, że ma rozkład normalny o zerowej wartości
oczekiwanej i wariancji w_t).
Dla robota z napędem różnicowym zachodzi Δs_t = 1/2(Δs_r,t+Δs_l,t)
gdzie: Δ_r,t – droga przebyta przez prawe koło miedzy chwilami t-1 i t; Δ_l,t – droga przebyta
przez lewe koło miedzy chwilami t-1 i t. Drogi kół można obliczyć na podstawie wskazań
enkoderów i promieni kół.
-tutaj mam obliczanie położenia, przejechanej drogi - co aktualnie mam na esp

Założenie w_t = α(|Δs_r,t| + |Δs_l,t|);
gdzie: α – nieznana stała; jej wartość dobierze się metoda prób i błędów.
-wariancja w_t to nic innego jak przejechana droga w danej chwili przez oba koła pomnożona przez stałą α. Czy to będzie wykorzystane do równania stanu jako ε_t ? ε_t – zakłócenie losowe (zakłada się, że ma rozkład normalny o zerowej wartości oczekiwanej i wariancji w_t).

2 Równanie wyjścia z_t = h(s_t) + δ(s_t); gdzie:
z_t =[z_1,t
       z_2,t],

z_1,t – pomiar z pierwszego czujnika niepodległościowego w chwili t, z_2,t – pomiar z drugiego czujnika
niepodległościowego w chwili t,
-tutaj zapis pomiaru z lewego  i prawego czujnika w tym samym położeniu robota
h_(st)=[h_1(st)
          h_2(st)]
h_1(st) – średni pomiar z pierwszego czujnika odległosciowego, gdy robot jest w położeniu s_t;
h_2(st) – średni pomiar z drugiego czujnika odległosciowego, gdy robot jest w położeniu s_t.
δ(s_t)=[δ₁(s_t)
           δ₁(s_t)]
δ₁(s_t) – zakłócenie pomiaru z pierwszego czujnika odległosciowego, gdy robot jest w położeniu s_t;
δ₂(s_t) – zakłócenie pomiaru z drugiego czujnika odległosciowego, gdy robot jest w położeniu s_t.

Powyższe średnie otrzymuje się na etapie budowania mapy. Na całej ścieżce należy wybrać
od kilku do kilkunastu punktów referencyjnych. Robot przejeżdża ścieżkę kilkukrotnie, a podczas
każdego przejazdu rejestruje się wskazania czujników w punktach referencyjnych. Potem
te wskazania ze wszystkich przejazdów uśrednia się w każdym punkcie referencyjnym z osobna
(w Matlabie służy do tego funkcja mean). W ten sposób powstanie tablica wartości, w której
jedna kolumna będą położenia robota, a druga i trzecia – średnie odczyty obu czujników.
- rozumiem przez to że mam wykonać pomiary punktów ref. (moich znaczników), w danym położeniu robota i zrobić kilka punktów dla każdego z punktu ref. tak aby otrzymać tą średnią z czujnika lewego i prawego
Gdy podczas pracy filtru Kalmana potrzebna będzie wartość średnia w punkcie, który nie był
referencyjny, brakującą wartość średnią oszacuje się poprzez interpolację liniową.
Założenie:
1. δ₁(s_t) ma rozkład normalny o zerowej wartości oczekiwanej i wariancji ∨₁₁(s_t);
2. δ₂(s_t) ma rozkład normalny o zerowej wartości oczekiwanej i wariancji ∨₂₂(s_t).
Powyższe wariancje otrzymuje się na etapie budowania mapy, dla każdego punktu referencyjnego
z osobna licząc wariancje pomiarów dla wszystkich przejazdów (w Matlabie służy do
tego funkcja var). Podobnie jak średnie, obie wariancje się tablicuje (najlepiej użyć tej samej
tablicy, co dla średnich, czyli w ten sposób wypełni się jej czwarta i piata kolumnie).
W algorytmie rozszerzonego filtru Kalmana, obie wariancje posłużą do budowania macierzy
kowariancji zakłóceń pomiarowych postaci
V(s_t)=[∨₁₁(s_t)    0
            0             ∨₂₂(s_t)]
- tutaj zabłądziłem, nie ogarniam na ten moment jak mam się do tego zabrać
W dalszej części jest już filtr Kalmana rozszerzony. Myślę że póki co niech uda mi się tę część zrobić to już będzie jakiś start.

lokalizacja.pdf

Edytowano Styczeń 3, 2021 przez Bazuka1988

[Treker (Damian Szymański)]

@Bazuka1988 większość osób, które zobaczy taki temat nawet nie pobierze PDF-a. Może chociaż napiszesz w skrócie o co tu chodzi, aby zainteresować więcej osób tym problemem? 😉

[Bazuka1988]

@Treker czy coś jeszcze mógłbym tu dodać co może być przydatne? Dziękuję🙂

[Treker (Damian Szymański)]

@Bazuka1988 teraz wygląda już dużo lepiej 😉

[Polecacz 101]

Produkcja i montaż PCB - wybierz sprawdzone PCBWay!
   • Darmowe płytki dla studentów i projektów non-profit
   • Tylko 5$ za 10 prototypów PCB w 24 godziny
   • Usługa projektowania PCB na zlecenie
   • Montaż PCB od 30$ + bezpłatna dostawa i szablony
   • Darmowe narzędzie do podglądu plików Gerber
Zobacz również » Film z fabryki PCBWay

[ethanak]

Takie głupie pytania:

1. dlaczego używasz float a nie double? Chyba czas jakiegoś tam parszywego dzielenia nie jest aż tak ważny, skoro używasz pulseIn i świadomie tracisz kilkadziesiąt milisekund na pomiar odległości.
2. czy jesteś pewien, że nawet zakładając wystarczającą dokładność tego floata przekazujesz wystarczająco dokładną wartość do matlaba? Dlaczego w ogóle liczysz to na ESP a nie wrzucisz do matlaba surowych danych z enkoderów (int64), zamiast tego przetwarzasz je najpierw na jakieś floaty, a potem jeszcze zmniejszasz dokładność przesyłając te floaty w postaci dość mocno przybliżonej (przez print)?

O dokładności czujników ultradźwiękowych nie chcę dyskutować, a tym bardziej o pomiarze czasu przez pulseIn.

 

[Bazuka1988]

@ethanak 
Ad1. Czy uzywajac double az tak to wplynie na poprawe pomiaru, po co mi az tyle miejsc po przecinku? Czego mógłbym użyć zamiast pulseIn? Wszędzie gdzie szukałem to są praktycznie identyczne przykłady. Spotkałem się jeszcze z biblioteką gdzie jest brana temperatura otoczenia pod uwagę. Wogole to myslalem tylko tu nad dodaniem jakiejs mediany z 3 pomiarów w czasie jazdy z jednej pozycji, ale raczej mi to nie wypali, chyba ze robot bedzie sie zatrzymywał, ale zalozenie jest ze ma to robic w ruchu.
Ad2. Nie, nie jestem, sprawdzę. Czy potrzebny mi int64 ? Odczyt następuje kilka razy na sekunde, robot jedzie wolno i pełny obrot 1920imp/obrot zajmuje kilka sekund. Z tymi konwersjami typów masz racje... poprawię to. Dane wysyłam przez print bo tak chyba najprościej, próbowałem to jakoś zrobić przez np jakąś ramkę, ale mój poziom umiejętności programowania jest słaby. Potem znów pojawia się problem żeby to odbierać przez matlaba. Jakiej innej funkcji mógłbym użyć zamiast printfBT?

Edit:
Troche poszperałem i wychodzi na to ze przy enkoderach podczas przesylania np do matlaba bezposrednio tak jak zaproponowales bedzie efektywniejsze int64.

Odnośnie czujnika ultradźwiękowego, znalazłem taką biblioteke https://github.com/enjoyneering/HCSR04
Co myślisz?

Edytowano Styczeń 3, 2021 przez Bazuka1988

[ethanak]

2 minuty temu, Bazuka1988 napisał:

Czy uzywajac double az tak to wplynie na poprawe pomiaru, po co mi az tyle miejsc po przecinku

Co znaczy "miejsc po przecinku"? Jakich miejsc? Jaką dokładność ma float, a jaka double? Ile stracisz np. przy n-tym dodawaniu (a tak liczysz kąt)? Poczytaj sobie w ogóle o arytmetyce float.

4 minuty temu, Bazuka1988 napisał:

Czy potrzebny mi int64

Takiego dostajesz z enkodera, możesz sobie zamienić choćby na int8 jeśli Ci wystarczy (a uwierz że czasem wystarczy, tylko nie w tym przypadku).

5 minut temu, Bazuka1988 napisał:

Dane wysyłam przez print bo tak chyba najprościej

Ale rozumiesz różnicę między zamianą na string (bo do tego sprowadza się print) liczby typu int (gdzie po stronie odbierającej odtwarzasz dokładnie to samo co weszło do nadajnika) a liczby typu float (gdzie wysyłasz tylko dwa miejsca po przecinku bo print tak ma,  a poza tym z wyjątkiem bardzo specyficznych wartości liczby float nie da się przedstawić w postaci ułamka dziesiętnego, ani tym bardziej przekształcić zapisu dziesiętnego na binarny float)?

A tak przy okazji: programując w ESP32 spokojnie możesz używać delay(), a w niektórych przypadkach jest to nawet wymagane. To nie mały AVR-ek że trzeba kombinować z millis() żeby uzyskać namiastkę multitaskingu (którego przecież i tak nie masz).

 

[Bazuka1988]

@ethanak

Co do float i arytmetyki masz rację. Nie wiedziałem że ten błąd może tak szybko narastać.
Co do int64 i enkoderów znów masz rację.
Muszę się do edukować jak to przesłać w najprostszej postaci żeby nie używać tego printf'a... Da się to wogóle po BT zrobić bez specjalnych kombinacji i umiejętności?
Chodzi Ci o millis() przy opóźnieniu funkcji pozycja() u mnie w kodzie?
Próbowałem też używać 2 rdzeni na esp za pomocą https://randomnerdtutorials.com/esp32-dual-core-arduino-ide/ ale średnio wyszło. Próbowałem tego rozwiązania jak miałem wszystko na Esp, czyli + silniki, czujniki odbiciowe, pid.

Odnośnie czujnika ultradźwiękowego, znalazłem taką bibliotekę https://github.com/enjoyneering/HCSR04
Co myślisz?

Edytowano Styczeń 3, 2021 przez Bazuka1988

[ethanak]

1 minutę temu, Bazuka1988 napisał:

Muszę się do edukować jak to przesłać w najprostszej postaci żeby nie używać tego printf'a

E tam, nie o to chodzi żeby nie uzywać, tylko żeby używać z głową 🙂 Tak jak pisałem: przesyłaj inty, to nie będziesz miał problemów.

2 minuty temu, Bazuka1988 napisał:

Próbowałem też używać 2 rdzeni na esp za pomocą https://randomnerdtutorials.com/esp32-dual-core-arduino-ide/ ale średnio wyszło

Zostaw na razie dwa rdzenie w spokoju, niech rdzeń  0 zajmuje się swoimi rdzeniowymi sprawami. Powinieneś poznać przynajmniej podstawy multitaskingu freeRTOS-a, podawałem komus ostatnio link do takiego artykuliku: https://savjee.be/2020/01/multitasking-esp32-arduino-freertos/

4 minuty temu, Bazuka1988 napisał:

Chodzi Ci o millis() przy opóźnieniu funkcji pozycja() u mnie w kodzie?

No właśnie tu aż się prosi o delay.

Dobra, bierz się za freeRTOS-a i multitasking, bo bez tego czarno widzę programowanie ESP32 (a przynajmniej czegoś bardziej skomplikowanego niż miganie ledą). Spróbuj na początek napisać program który miga trzema ledami niezależnie z różną częstotliwością - jak Ci wyjdzie to zrozumiesz że to w sumie nie takie straszne i będziesz mógł wrócić do robota 🙂

[Bazuka1988]

@ethanak żebym miał więcej czasu, a go coraz mniej. Odnośnie tego printf'a to może to poprawi sprawę, ale to na szybko, zobaczę jaki będzie efekt - Serial.print(x, 4), i jeżeli chodzi Ci o wysyłanie int to znaczy ze mam np liczbę 123.4567890 podzielić np na kawałki i wysyłać w 3 printfach w tej sytuacji Serial.print(123); potem Serial.print(".") i Serial.print(4567890) o to chodzi? Nie wiem czy dobrze myślę.
Użyje delay() zamiast tego millis().
Podsunąłeś mi trochę pomysłów i pokazałeś gdzie są błędy, bardzo dziękuję. Sprawdzę jeszcze tą bibliotekę od hc-sr04, może też poprawi sprawę. Ale i tak wciąż zostaje kwestia tego filtru Kalmana.
Edit:
Sprawdziłem co odbiera matlab i faktycznie...a ja nie zwróciłem na to uwagi... zapisuje do float, ale wysylane sa tylko liczby do .2 po przecinku.
 

Edytowano Styczeń 3, 2021 przez Bazuka1988

[ethanak]

5 minut temu, Bazuka1988 napisał:

Nie wiem czy dobrze myślę.

Nie. Po prostu nigdy nie przesyłaj floatów, w ESP ile się  da operuj na intach, a dopiero matlab niech z tego robi floaty. Przecież możesz wysłać po prostu die odległości (czyli int w milisekundach) i dwie pozycje enkodera (lub różnicę między bieżącą a poprzednią - też inty) a niech matlab sobie z tego sinusy i cosinusy liczy.

A jeśli już musisz wysłać floata (double)... zrób to tak, aby dokładnie taki sam float pojawił się po stronie odbiornika. Binarny przesył nic nie da, bo wewnętrzny format floata może być zupełnie inny po obu stronach. Najpewniejszym sposobem byłoby tu wysłanie na zasadzie znak-mantysa-wykładnik, ale jeśli operujesz liczbami ze znanego zakresu możesz to trochę uprości: co prawda będzie to również przybliżenie - ale dużo dokładniejsze.

Pomnóż po stronie nadajnika liczbę przez jakąś potęgę dwójki (tu musisz sobie oszacować jaką). Na przykład masz wysłać wartość kąta w zakresie od -100 do 100 (nieważne czego, stopni, radianów, rumbów czy co tam sobie wymyślisz). Pomijając znak masz 7 bitów na część całkowitą, zostają 24 bity na część ułamkową. Mnożysz liczbę przez 2^24, robisz z tego inta i takiego wysyłasz. A w matlabie po prostu dzielisz liczbę przez to samo 2^24 i masz praktycznie tego samego floata (no, prawie).

Sposobów jest oczywiście więcej, ale nie będę tu wszystkich wymieniać.

Co do hc-sr04: niestety, nie znalazłem gotowej biblioteki do ESP32/Arduino działającej na RMT, spróbuj która z istniejących będzie najlepsza (NewPing?)

Do do filtrów - niech się wypowiedzą ci, co się znają lepiej ode mnie.

Aha - i nie myl printfa z printem, bo to dwa różne światy 🙂 W ESP32 działa oczywiście printf bez żadnych ograniczeń, czyli możesz spokojnie użyć:

Serial.printf("%d %d %.6e\n", int1, int2, float1); 

 

[Bazuka1988]

7 minut temu, ethanak napisał:

Aha - i nie myl printfa z printem, bo to dwa różne światy 🙂 W ESP32 działa oczywiście printf bez żadnych ograniczeń, czyli możesz spokojnie użyć:

Serial.printf("%d %d %.6e\n", int1, int2, float1); 

 

o.O
Nie wiedziałem.......ehhh to ułatwia sprawę. Narazie sprawdzam jak wyglądają poszczególne zmienne i ustawiam wszędzie ten sam typ i jak to jest wysyłane. Później robot zrobi trasę i zobaczę czy jest jakaś poprawa, a już po samym kącie jak wspominałeś widzę że powinna być. Po około 15sec jazdy moja teta juz urosła do 30 tys +

12 minut temu, ethanak napisał:

Nie. Po prostu nigdy nie przesyłaj floatów, w ESP ile się  da operuj na intach, a dopiero matlab niech z tego robi floaty. Przecież możesz wysłać po prostu die odległości (czyli int w milisekundach) i dwie pozycje enkodera (lub różnicę między bieżącą a poprzednią - też inty) a niech matlab sobie z tego sinusy i cosinusy liczy.

To jest świetny pomysł!

[Bazuka1988]

@ethanak za wiele to nie dało, ale nie powiem...łatwiej mi będzie teraz operować danymi i esp ma więcej czasu na enkodery i czujniki.  Takie dane odbieram  w matlabie. Wysyłam same impulsy z enkoderów i czas z czujników ultradźwiękowych.

impL =142
impP =136
hcsr04L =4052
hcsr04S =5666
impL =140
impP =139
hcsr04L =4102
hcsr04S =5574
impL = 143

Matlab kod:
 

figure(1);
axis([-400 400 -400 400]);
axis auto
hold on
hcsr04L=0, hcsr04Ldystans=0, Lx=0, Ly=0, hcsr04S=0, hcsr04Sdystans=0, Sx=0, Sy=0; %czujniki ultradzwiekowe
impL=0, impP=0;%enkodery kwadraturowe
d=6.50, dystansL=0, dystansP=0, dystansS=0, tetaKat=0, L=12.50; %d-srednica kola; dystansL,P-droga przejechana przez kazde z kol; dystansS-droga przejechana przez robota; tetaKat-kat skretu robota; L-odleglosc mniedzy osiami kol;
x=0, xn=0, y=0, yn=0;%polozenie robota
l=d*pi;%obwod kola

while (true)

impL=fscanf(bt,'%f')%odczyt impulsow z lewego kola
impP=fscanf(bt,'%f')%odczyt impulsow z prawego kola
hcsr04L=fscanf(bt,'%f')%odczyt czasu z pinu LechoHSCR04
hcsr04S=fscanf(bt,'%f')%odczyt czasu z pinu PechoHSCR04
%droga przejechana przez kazde z kol
dystansL=((impL/1920)*l);
dystansP=((impP/1920)*l);
%kata o jaki obrocil sie robot
tetaKat=tetaKat+((dystansP-dystansL)/L);
%pozycja
dystansS=(dystansP+dystansL)/2;
x=dystansS*(cos(tetaKat));
y=dystansS*(sin(tetaKat));
%hcsr04-LEWY
hcsr04Ldystans = ((hcsr04L/58.2)-4);
Lx=(hcsr04Ldystans)*(cos(tetaKat+1.57));
Ly=(hcsr04Ldystans)*(sin(tetaKat+1.57));
%hcsr04-SRODEK
hcsr04Sdystans = ((hcsr04S/58.2)-1);
Sx=(hcsr04Sdystans)*(cos(tetaKat));
Sy=(hcsr04Sdystans)*(sin(tetaKat));
xn=xn+x;
yn=yn+y;
plot(xn,yn,'black*',xn+Lx, yn+Ly,'red.', xn+Sx, yn+Sy,'green.');
hold on
end

 

[Bazuka1988]

@GAndaLF <<<< Dzień dobry. Czy mógłbyś mi pomóc z filtrem Kalmana?(temat postu)

[ethanak]

13 godzin temu, Bazuka1988 napisał:

za wiele to nie dało

A czy ja gdzieś napisałem, że daję ci Idealne Pacaneum na Kaca? Wyeliminowałeś na razie jedną z wielu przyczyn, dalej to już nie moja działka.

Powiem tylko, że do pomiaru kąta stosuję żyroskop. Jeśli go połączysz z enkoderami możesz dostać całkiem ciekawe wyniki...