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
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
Kod:
// 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 //
/****** 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
Kod:
// 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 //
} // 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
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
_________________ Mam boskie ciało. Szkoda chodzi o Buddę
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
_________________ Mam boskie ciało. Szkoda chodzi o Buddę
Nie możesz pisać nowych tematów Nie możesz odpowiadać w tematach Nie możesz zmieniać swoich postów Nie możesz usuwać swoich postów Nie możesz głosować w ankietach Nie możesz załączać plików na tym forum Możesz ściągać załączniki na tym forum
FAQ
Szukaj
Użytkownicy
Regulamin
Grupy
Rejestracja
Zaloguj
[url]
