Line follower Linefollower + robot śledzący

[altjordan]

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

[Sabre]

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.

[altjordan]

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

[Master]

Ps. wrzuć zdjęcie jakieś do załącznika aby w menu głównym wyświetliło wkońcu zdjęcie 😋

[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

[Daniel93]

Aby zdjęcie sie wyświetlało należy je wysłać w załaczniku.

[altjordan]

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

[Ditto090]

Może byłoby bardziej płynnie jak byś zamontował toto http://www.nowyelektronik.pl/searchsklep.php?mode=opis&nr=61606

[paradox91]

Jestem ciekawy jakiego kompilatora używasz o gdzie znalazłeś informacje dotyczące programowania tego typu robotów w C? Byłbym bardzo wdzięczny za odpowiedź gdyż niedawno mnie to zainteresowało.

[Mihau]

Jest środowiska RobotC, niestety płatne(okres próbny - 30dni).

Jest też NXC, ale z nim nie pracowałem 😉.

[ Dodano: 12 Gru 09 06:33 ]

Tu jest chyba NXC użyte.

[GruX]

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;)

[arktos6]

Czy te klocki Lego Mindstorms 2.0 kuples jednoczesnie? i gdzie jeśli mogę wiedzieć bo zamierzam kupic NXT 2.0 😅

[Mihau]

IMO nxt 1 wystarcza.