[C] Problem z wykorzystaniem odczytu sensorów

[piotr3332]

Witam

Jestem poczatkujacym w programowaniu robotów typu line follower. Moj problem polega na wlasciwym uzyciu danych z sensorow. Postaram sie w jak najlepszy sposob wytlumaczyc dlaczego, razem z dostarczonym kodem. Otoz robot ktory w tej chwili programuje odczytuje dane z 8 sensorów ktorych wartosc jest przyblizana przy uzyciu ukladu RC charge/disharge gdzie wartosc dla danego sensoru jest podawana pomiedzy 4 a 8 poziomami.

Dla przykladu gdy sensor jest dokladnie na lini wartosc bedzie powiedzmy 6, gdy jest poza linia bedzie 15. Gdy jest pomiedzy to wartosc jest 9. W kodzie zrodlowym zalaczonym na dole petla for jest uzyta do obliczania wartosci dla danego sensoru.

Moje pytanie brzmi nastepujaco, w jaki najlepszy sposob moge uzyc te dane aby zaimplementowac algorytm PID dla mojego robota?

Problemu by nie bylo jezeli kazdy sensor dawalby output 1 kiedy wykrywalby linie. Tutaj jest wiecej kombinacji i na ta chwile nie wiem w jaki sposob moge uzyc tych danych.

Bardzo prosze o pomoc.

#include "mbed.h"

DigitalOut   led(LED1);                                  // status LED
DigitalOut   test(p27);                                  // test pin

DigitalInOut ir_rx0(p11);  // IR recieves
DigitalInOut ir_rx1(p12);  // IR recieves
DigitalInOut ir_rx2(p15);  // IR recieves
DigitalInOut ir_rx3(p16);  // IR recieves
DigitalInOut ir_rx4(p17);  // IR recieves
DigitalInOut ir_rx5(p18);  // IR recieves
DigitalInOut ir_rx6(p25);  // IR recieves
DigitalInOut ir_rx7(p26);  // IR recieves

PwmOut ir_tx(p24);                                       // IR transmitter

Serial pc(USBTX, USBRX);                                 // USB serial port

// Black  : 4.2x200us   = 840us
// White  : 2 x 100us   = 200us
// Silver : 1.2 x 100us = 120us


int main() {

 bool ir_set[8];
 int  ir_value[8];

 int  ir_buffer;
 int  i, j, k;

 ir_tx.period_us(100);       // a 20ms period
 ir_tx.pulsewidth_us(100);    // 

 led  = 1;   
 test = 0;

 while(true)
 {
   ir_rx0.output();
   ir_rx1.output();
   ir_rx2.output();
   ir_rx3.output(); 
   ir_rx4.output();
   ir_rx5.output();
   ir_rx6.output();
   ir_rx7.output(); 

   ir_rx0 = 1;
   ir_rx1 = 1;
   ir_rx2 = 1;
   ir_rx3 = 1;
   ir_rx4 = 1;
   ir_rx5 = 1;
   ir_rx6 = 1;
   ir_rx7 = 1; 

   for(i=0; i<8; i++)
   {
     ir_set[i]=true;
     ir_value[i]=0;
   }

   wait_us(50);

   ir_rx0.input(); 
   ir_rx1.input();  
   ir_rx2.input(); 
   ir_rx3.input(); 
   ir_rx4.input(); 
   ir_rx5.input();  
   ir_rx6.input(); 
   ir_rx7.input(); 

   test = 1;

   for(i=0; i<20; i++)
   {
     wait_us(50);

     if ((ir_buffer && 0x01) == 0)
       test = 0;

     if((ir_rx0==0) & ir_set[0])
     {
       ir_set[0]=false; ir_value[0]=i; 
     }  
     if((ir_rx1==0) & ir_set[1])
     {
       ir_set[1]=false; ir_value[1]=i; 
     }  
     if((ir_rx2==0) & ir_set[2])
     {
       ir_set[2]=false; ir_value[2]=i; 
     }  
     if((ir_rx3==0) & ir_set[3])
     {
       ir_set[3]=false; ir_value[3]=i; 
     }  
     if((ir_rx4==0) & ir_set[4])
     {
       ir_set[4]=false; ir_value[4]=i; 
     }  
     if((ir_rx5==0) & ir_set[5])
     {
       ir_set[5]=false; ir_value[5]=i; 
     }  
     if((ir_rx6==0) & ir_set[6])
     {
       ir_set[6]=false; ir_value[6]=i; 
     }  
     if((ir_rx7==0) & ir_set[7])
     {
       ir_set[7]=false; ir_value[7]=i; 
     }  

   }

   printf( "ADC conversion\n" );
   printf( "chan 0 = %d\n", ir_value[0] );  
   printf( "chan 1 = %d\n", ir_value[1] );     
   printf( "chan 2 = %d\n", ir_value[2] );  
   printf( "chan 3 = %d\n", ir_value[3] );          
   printf( "chan 4 = %d\n", ir_value[4] );  
   printf( "chan 5 = %d\n", ir_value[5] );     
   printf( "chan 6 = %d\n", ir_value[6] );  
   printf( "chan 7 = %d\n", ir_value[7] );      

   wait(1);

 }
}

[marek1707]

No niestety, informacja z Twoich czujników jest zbyt "zaawansowana" dla prostego regulatora PID. Jak rozumiem, każdy kanał w sposób analogowy pokazuje ile energii na niego pada po odbiciu od podłoża więc z kilku czujników dostajesz jakby "przekrój" tego co widzą - trochę jak z prymitywnej, kilkupikselowej kamerki CCD 🙂 Teraz to Ty musisz się zastanowić jaki algorytm wymyślić, by dopasować to do wejścia PID, który z kolei potrzebuje jednej liczby - odchyłki od trasy. Powinieneś więc wymyślić jak w uzyskanym z czujników przekroju trasy znaleźć położenie środka linii. W najprostszym przypadku byłby to czujnik o najmniejszym odczycie - prawdopodobnie znajduje się w okolicach środka linii bo w całym swoim polu widzenia ma ciemność. Przy małej rozdzielczości pomiaru możesz mieć jednak kilka czujników dających podobny odczyt więc możesz znaleźć taki, który np. po swojej lewej i po prawej ma czujnik, który daje co najmniej taki odczyt jak on sam lub większy. Możesz też przybliżyć wektor danych z czujników jakąś krzywą, np wielomianem niskiego stopnia lub czymś co z definicji bardziej przypomina oczekiwany kształt i analitycznie wyznaczyć minimum tej funkcji. Operacje na krzywych będą o tyle fajne, że wynikiem (znalezionym miejscem minimum) nie musi być konkretny czujnik tylko np. coś między nimi. Wtedy wyznaczony środek trasy może np. leżeć na pozycji 1,37 zamiast na 1 lub 2 a to jest dużo lepsze wejście do PIDa. Tak więc metod jest dużo ale nie martwię się - jeśli zrobiłeś tak niecodzienne czujniki (tylko program trochę taki "przaśny", można było analizę czujników zrobić w przerwaniu i z większą rozdzielczością) to i z aproksymacją punktów pomiarowych funkcją poradzisz sobie na pewno 🙂 Na szybko możesz też przeszukiwać wektor wyników od lewej i od prawej strony w poszukiwaniu pierwszego czujnika dającego mniejszy wynik niż jakiś próg "szarości". To będą krawędzie linii więc środek będzie po środku 🙂 np. jeśli z lewej krawędź będzie na czujniku 3 a z prawej na 5 to środek wychodzi na 4 (eureka..). Teraz wystarczy tylko zamienić numer czujnika (lub w inny sposób znalezione położenie środka linii) na liczbę wejściową do PIDa ale to już zależy od tego jak został napisany on sam i jak interpretujesz jego wyniki. Tu wystarczy zwykłe przysunięcie i skalowanie czyli y=ax+b. Najbardziej naturalnie byłoby, gdyby do PID wchodziły liczby "symetryczne", np. z zakresu -4..+4 (dla 9 czujników) albo jakoś przeskalowane nieliniowo po obu stronach by dodatkowo przyśpieszyć reakcje na duże odchylenia ale o wagach czujników itp szczegółach już Koledzy od LFów dokładnie Ci opowiedzą 🙂

EDIT: literówki i parę drobiazgów.

[piotr3332]

Dziekuje za odpowiedz. Zdecydowalem sie na dwie metody implementacji algorytmu line follower. Pierwszy to przy uzyciu PID. Gdzie blad dla danego sensora liczony jest gdy tylko wartosc dla sensora bedzie mniejsza od jego maksymalnej wartosci odczytu czyli w momenci gdy zacznie widziec biala linie. Np. powiedzmy ze dla sensora x wartosc maksymalna czyli ta kiedy biala linia jest poza widocznoscia to 15. Gdy jednak odczyt pokazuje 14 czyli po czesci zauwazona jest linia wtedy przypisywany jest blad dla tego sensora.

Moja implementacja jak narazie daje satysfakcjonujace wyniki jednak mysle ze jest opcja by zrobic to lepiej.

[MirekCz]

1. Nadaj sensorom wagi. (np. masz sensory od 0 do 7 i wagi -120,-70,-40,-25,-10,10,25,40,70,120) -> najłatwiej to zrobić jako tablica

2. Dla ułatwienia przetwórz wartości sensora na coś bardziej ludzkiego, czyli jak np. 5 to linia a 15 to brak linii to przelicz to na skale 0..100 (czyli wynik = (10-(wynik-5))*10 ) - chodzi tylko o łatwiejsze czytanie niżej podanego kodu, że np. 0 to brak linii, a 100 to linia jest kompletnie pod sensorem

3. Teraz niech Twój algorytm poszuka czujnika, który ma największą wartość (czyli linia jest najbardziej pod nim).

Powiedzmy, że po przeliczeniu czujniki zwracają wartość 0,10,50,90,30,10,0,0 (100-widoczna linia, 0 - nie widoczna)

Więc największą wartość ma czujnik 3 -> 90.

Bierzemy czujniki na lewo/prawo od niego i patrzymy na ich wartość i wagę:

2 -> 50, waga -40

3 -> 90, waga -25

4 -> 30, waga -10

Teraz wystarczy prosta matematyka - liniowo szukamy wartości pomiędzy czujnikami.

x = ( a*wagaa + b*wagab + c*wagac ) / ( a+b+c )

x = ( 50 * -40 + 90 * -25 + 30 * -10 ) / ( 50+90+30 )

x = ( -2000 -2150 -300 ) / ( 170)

x = -26

4. x to wartość zwracana z naszych czujników, którą podajemy do PID (wartość normalnie będzie pomiędzy -120 a 120 z obecnie dobranymi wagami). Przesuwając robota nad linią wartość x będzie płynnie się zmieniała. Dodatkowo dzięki zastosowaniu wag możesz zrobić tak jak w obecnym przykładzie, że czujniki blisko środka wprowadzają tylko nieznaczną zmianę kierunku (zakładamy, że jedziemy praktycznie po linii i potrzebna tylko mała korekta), a czujniki skrajne (lub wręcz przypadek, gdy żaden czujniki nie widzi linii) powodują zdecydowaną zmianę kierunku.

[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