Skocz do zawartości

Linefollower + robot śledzący


altjordan

Pomocna odpowiedź

To teraz moja kolej żebym się pochwalił swoim "dziełem". Konkretnie jest to algorytm linefollowera i drugiego robota podążającego za nim.

Roboty zbudowane są z dwóch zestawów klocków Lego Mindstorms NXT ze standardowym firmwarem

język programowania: NXC

http://bricxcc.sourceforge.net/nbc/

obsługa modułu Bluetooth - zmodyfikowana wersja (podana na stronie nie działa ze względu na zmianę składni polecenia TextOut) - http://daniele.benedettelli.com/BT_NXC.htm

inspiracja linefollowera - http://www.mindstorms.pl/cr_m_blackline.html

Taki algorytm pozwala zbadać różne typy komunikacji. Część algorytmu odpowiedzialna za podążanie za linią jest analogiczna z komunikacją domniemaną. Można sobie wyobrazić, że istnieje jeszcze jeden robot znaczący swoją trasę za pomocą linii. Odpowiada to przypadkowi, gdy robot sprawdza na przykład czy jedzie po terenie ze skoszoną trawą lub gdy odczytuje chemiczne ślady pozostawione przez inne roboty (odpowiednik feromonów, por. w Anies Hannawati Purnamadjaja, R. Andrew Russell: „Robotic Pheromones: Using Temperature Modulation in Tin Oxide Gas Sensor to Differentiate Swarm’s Behaviours”. Intelligent Robotics Research Centre, Monash University, Clayton, Australia (2006)). Część algorytmu pozwalająca drugiemu robotowi podążać za pierwszym to ilustracja komunikacji na podstawie stanu. W tym przypadku odczytywanym stanem jest tylko obecność pierwszego robota w zasięgu czujników robota podążającego. Trzeci użyty w algorytmie rodzaj komunikacji to odbywająca się przez bezprzewodowy moduł Bluetooth komunikacja formalna. Przekazywany jest konkretny komunikat o braku zakłóceń w komunikacji na podstawie stanu. Nie jest ona konieczna, ale zastąpienie jej innym typem mogłoby okazać się bardziej pracochłonne i nieefektywne.

linefol-jor-v5-mst.nxc

// line-follower + robot sledzacy
// projekt - Jordan 3.VI.2008r
// dla jednego czujnika z obsługą bluetootha (master) 
#include "BTlib.nxc" 
#define BT_CONN 1 // linia do ktorej jest podlaczony slave
#define MAILBOX 0

// tył tam gdzie czujnik
// silnik na porcie A - lewy
// silnik na porcie C - prawy

string in; 
bool kierunek; 
int ciemne = 480, predkosc = -40; 
unsigned long czas, timer; 
byte skok=400, cz1 = IN_1; /* cz1 - port do ktorego jest podpiety jest czujnik optyczny */

sub masterBTCheck(int conn){          /****************************/
  string cStr;                       /*     sprawdzenie czy      */
  cStr = NumToStr(conn);             /* mozliwa jest komunikacja */
  cStr = StrCat("na linii ",cStr);   /*      przez Bluetooth     */
  if (!BTCommCheck(conn)){           /****************************/
     TextOut(0,LCD_LINE2,"Polacz z robotem",true); 
     TextOut(0,LCD_LINE3,"podazajacym"); 
     TextOut(0,LCD_LINE4,cStr,false); 
     TextOut(0,LCD_LINE5,"w menu NXT",false); 
     until(ButtonPressed(BTNCENTER, false)); 
     Stop(true); 
  } 
} 

/***********************
*    PROGRAM GLOWNY    *
***********************/
task main() { 

//     INICJALIZACJA   //
masterBTCheck(BT_CONN);  // czy jest komunikacja Bluetooth
SetSensor(cz1, SENSOR_LIGHT); 
SetSensorType(cz1,SENSOR_TYPE_LIGHT_ACTIVE); 
string tekst,out; 
int czas; 
// NORMALNA PRACA LIDERA //

byte cz2=IN_4;    // czujnik swiatla dzialajacy jako lampka kontrolna - nieobowiazkowa

 while(true) { 
   ClearScreen(); 
   SetSensorType(cz2,SENSOR_TYPE_LIGHT_ACTIVE); 
   //    BTSendMessage(BT_CONN,MAILBOX,out); //wyslanie wiadomosci

   /****** normalna jazda do przodu ******/
   tekst = StrToNum(Sensor(cz1)); 
   TextOut(10, LCD_LINE3,"cz1="+tekst,false);             /* A-lewy, C-prawy */

   in = BTReceiveMessage(BT_CONN, MAILBOX, TRUE); 
   until (in=="ok") { 
         Off(OUT_AC); 
         in = BTReceiveMessage(BT_CONN, MAILBOX, TRUE); 
   } 

   OnFwd(OUT_AC,predkosc); 
   Wait(50); 
   /****** szukanie linni *****/
      czas=0; 
      SetSensorType(cz2,SENSOR_TYPE_LIGHT_INACTIVE); 
      while (Sensor(cz1) > ciemne) { 
           if ((kierunek==false) && (Sensor(cz1) > ciemne)) { 
              SetSensorType(cz2,SENSOR_TYPE_LIGHT_ACTIVE); 
              Wait(1);                                        /***************************/
              SetSensorType(cz2,SENSOR_TYPE_LIGHT_INACTIVE);  /*  obraca robota w prawo  */
              OnRev(OUT_A,predkosc);                          /*      i szuka linii      */
              OnFwd(OUT_C,predkosc);                          /***************************/
              timer=0; 
              until ( (Sensor(cz1) < ciemne) || (timer >= czas) ) {timer++; /* czyli czekaj */ }; 
              if (Sensor(cz1) > ciemne) { kierunek = true; }; 
              czas=czas+skok; 
           } 
           if ((kierunek==true) && (Sensor(cz1) > ciemne)) { 
              SetSensorType(cz2,SENSOR_TYPE_LIGHT_ACTIVE); 
              Wait(1);                                       /***************************/
              SetSensorType(cz2,SENSOR_TYPE_LIGHT_INACTIVE); /*  obraca robota w lewo   */
              OnRev(OUT_C,predkosc);                         /*      i szuka linii      */
              OnFwd(OUT_A,predkosc);                         /***************************/
              timer=0; 
              until ( (Sensor(cz1) < ciemne) || (timer >= czas) ) {timer++; /* czyli czekaj */ }; 
              if (Sensor(cz1) > ciemne) { kierunek = false; }; 
              czas=czas+skok; 
           } 
      }  // skoniec while (Sensor(cz1) > ciemne) 

   /******* linia znaleziona, jedzie dalej ***/

 } // koniec glownej petli while(true) 

}

linefol-jor-v5-slv.nxc

// line-follower + robot sledzacy
// projekt - Jordan 3.VI.2008r
// dla jednego czujnika z obsługą bluetootha (slave) 
#include "BTlib.nxc" 
#define BT_CONN 0 // linia do ktorej jest podlaczony slave
#define MAILBOX 0

// tył tam gdzie czujnik
// silnik na porcie A - lewy
// silnik na porcie C - prawy

bool kierunek; 
int odl_sledzenia=40; // max odleglosc na ktora moze oddalic sie master
unsigned long czas, timer; 
byte skok=10, cz1 = IN_1; /*  cz1 - port do ktorego jest podpiety jest */
                         /*     ultradzwiekowy czujnik odleglosci     */

int predkosc() {           // dynamiczne przystosowanie predkosci do odleglosci
  int odl = SensorUS(cz1); 
  if (odl > odl_sledzenia) return (-45); // <- predkosc przy rozgladaniu sie
  else { if (odl < 8) return (0);        // <- dla odleglosci < 6 zatrzymaj robota
         else return (odl * (-3)/2);       // <- normalna predkosc sledzenia
  } 
} 

sub slaveBTCheck(int conn){ 
  string cStr; 
  cStr = NumToStr(conn); 
  cStr = StrCat("na linni ",cStr); 
  if (!BTCommCheck(conn)){ 
     TextOut(0,LCD_LINE2,"Polacz z robotem",true); 
     TextOut(0,LCD_LINE3,"prowadzacym"); 
     TextOut(0,LCD_LINE4,cStr,false); 
     TextOut(0,LCD_LINE5,"w menu NXT",false); 
     until(ButtonPressed(BTNCENTER, false));   // czeka na nacisniecie srodkowego przycisku
     Stop(true);                               // konczy dzialanie programu
  } 
} 

/***********************
*    PROGRAM GLOWNY    *
***********************/

task main() { 

//     INICJALIZACJA   //
slaveBTCheck(0); // czy jest komunikacja Bluetooth
SetSensorLowspeed(cz1); 
string tekst,out; 
int czuj; 
byte cz2=IN_4;    // czujnik swiatla dzialajacy jako lampka kontrolna - nieobowiazkowa
// NORMALNA PRACA PODOAZAJACEGO //

 while(true) { 
    ClearScreen(); 
   SetSensorType(cz2,SENSOR_TYPE_LIGHT_ACTIVE); 
   if (SensorUS(cz1) < odl_sledzenia) BTSendMessage(BT_CONN,MAILBOX,"ok"); //wyslanie wiadomosci

   //    PODAZANIE ZAROBOTEM
   /* ---------------------- */
   tekst = StrToNum(SensorUS(cz1)); 
   TextOut(10, LCD_LINE3,"cz1="+tekst,false);       /* A-lewy, C-prawy */
   OnFwd(OUT_AC,predkosc()); 
   /****** szukanie linni *****/

      czas=0; 
      while (SensorUS(cz1) > odl_sledzenia) { 
           BTSendMessage(BT_CONN,MAILBOX,"nie_ok"); 

           if ((kierunek==false) && (SensorUS(cz1) > odl_sledzenia)) { 
              SetSensorType(cz2,SENSOR_TYPE_LIGHT_ACTIVE);    /* mrygniecie dioda */
              Wait(1); 
              SetSensorType(cz2,SENSOR_TYPE_LIGHT_INACTIVE);           /***************************/
              OnRev(OUT_C,predkosc());                                 /*  obraca robota w prawo  */
              OnFwd(OUT_A,predkosc());                                 /*      i szuka mastera    */
              timer=0;                                                 /***************************/
              until ( (SensorUS(cz1) < odl_sledzenia) || (timer >= czas) ) {timer++; }; 
              if (SensorUS(cz1) > odl_sledzenia) { kierunek = true; }; 
              czas=czas+skok; 
           } 

           if ((kierunek==true) && (SensorUS(cz1) > odl_sledzenia)) { 
              SetSensorType(cz2,SENSOR_TYPE_LIGHT_ACTIVE);  /* mrygniecie dioda */
              Wait(1); 
              SetSensorType(cz2,SENSOR_TYPE_LIGHT_INACTIVE);          /***************************/
              OnRev(OUT_A,predkosc());                                /*  obraca robota w lewo   */
              OnFwd(OUT_C,predkosc());                                /*      i szuka mastera    */
              timer=0;                                                /***************************/
              until ( (SensorUS(cz1) < odl_sledzenia) || (timer >= czas) ) {timer++; }; 
              if (SensorUS(cz1) > odl_sledzenia) { kierunek = false; }; 
              czas=czas+skok; 
           } 

      }  // skoniec while (SensorUS(cz1) > odl_sledzenia) 

   /******* linia znaleziona, jedzie dalej ***/
 } // koniec petli while(true) 

}

Dorzucam też wstępne algorytmy. W programie nie jest uwzględnione znalezienie końca trasy.

Algorytm lidera:

Algorytm robota śledzącego:

Nie działa to jeszcze tak jak trzeba, przydałoby się poprawić niektóre stałe, żeby zachowywał się bardziej płynnie. Efekt jest następujący:

http://pl.youtube.com/watch?v=8kV26Pf8Uf8

Kod razem z poprawioną wersja modułu BTLib zamieszczam w załączniku

Lego.zip

SS850923.thumb.jpg.37337a9e4b165a2691d8dae8c66aac5a.jpg

Link do komentarza
Share on other sites

Podoba Ci się ten projekt? Zostaw pozytywny komentarz i daj znać autorowi, że zbudował coś fajnego!

Masz uwagi? Napisz kulturalnie co warto zmienić. Doceń pracę autora nad konstrukcją oraz opisem.

Całkiem ciekawy algorytm ma robot jadący jako 2. Widać, że jest szybszy, robot z przodu mu przeszkadza swoją obecnością cały czas.

Doczytałem dopiero po jakimś czasie, że ten 2 robot jest robotem śledzącym. Na filmiku wyglądało to troszkę inaczej.

Link do komentarza
Share on other sites

Wygląda to dziwnie przez to, że ultradźwiękowy czujnik odległości z drugiego robota jest bardzo niedokładny przy odczytywaniu odległości z nierównych powierzchni (takich jak tył pierwszego robota). Specjalnie użyłem jeszcze papierowego 'zderzaka' żeby mu trochę ułatwić zadanie. Ale jak pisałem już wyżej, algorytm wymaga poprawienia kilku stałych, takich jak prędkość i zmiana okresu wahnięć przy szukaniu linii lub drugiego robota

Link do komentarza
Share on other sites

Zarejestruj się lub zaloguj, aby ukryć tę reklamę.
Zarejestruj się lub zaloguj, aby ukryć tę reklamę.

jlcpcb.jpg

jlcpcb.jpg

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

Panowie Admini - przecież zrobione 🙂

Ciągle prowadzę badania nad tym algorytmem (jest to część mojej pracy magisterskiej). Staram się lepiej dobrać prędkości robotów, żeby szybciej pokonywały wyznaczoną trasę.

Pozdrawiam i czekam na komentarze

** Dodano: 2008-06-05, 15:48 **

Zamierzam zmodyfikować program tak, żeby roboty mogły wymieniać się rolami, to znaczy żeby oba miały możliwość śledzenia linii lub drugiego robota

Link do komentarza
Share on other sites

Dla Mindstorms serii RiS było NQC, dla NXT powstało NXC, jest to język oparty na C, bardzo przyjemny;) istnieje jeszcze java[napewno dla RiS, nie wiem jak jest dla NXT], no i dla NXT klocki oczywiście i rozpowszechniany przez RoboNET RobotC;) Ode mnie taka mała uwaga, żebyś zawsze zaczynał szukanie lini od strony z której ją ostatnio znalazłeś, znacznie przyspiesza przejazd robota po linii;) Aż się przypominają czasy liceum i 3 lata zabawy z tym ustrojstwem;)

Link do komentarza
Share on other sites

Dołącz do dyskusji, napisz odpowiedź!

Jeśli masz już konto to zaloguj się teraz, aby opublikować wiadomość jako Ty. Możesz też napisać teraz i zarejestrować się później.
Uwaga: wgrywanie zdjęć i załączników dostępne jest po zalogowaniu!

Gość
Dołącz do dyskusji! Kliknij i zacznij pisać...

×   Wklejony jako tekst z formatowaniem.   Przywróć formatowanie

  Dozwolonych jest tylko 75 emoji.

×   Twój link będzie automatycznie osadzony.   Wyświetlać jako link

×   Twoja poprzednia zawartość została przywrócona.   Wyczyść edytor

×   Nie możesz wkleić zdjęć bezpośrednio. Prześlij lub wstaw obrazy z adresu URL.

×
×
  • Utwórz nowe...

Ważne informacje

Ta strona używa ciasteczek (cookies), dzięki którym może działać lepiej. Więcej na ten temat znajdziesz w Polityce Prywatności.