Universitatea tefan cel Mare Suceava Facultatea de Inginerie Electric i tiina Calculatoarelor Program de studiu: Tehnici Avansate n Maini

i Acionri Electrice, anul I

Programarea robotului LEGO Mindstorms NXT

ndrumtor: Conf. univ. dr. ing POPA Cezar Dumitru

Masterand: TANAS Ana-Maria

1. Generaliti

Figura 1. Robotul LEGO Mindstorms NXT LEGO Mindstorms este o serie de componente LEGO care cuprinde uniti programabile, senzori, servomotoare i alte pri constructive. Produsul LEGO Mindstorms NXT a fost lansat n iulie 2006 i este succesorul primului model, Robotics Invention System (RCX), care a strnit interesul unui numr mare de amatori. Succesul setului NXT este argumentat n special de uurina cu care se poate personaliza i extinde platforma. Au fost dezvoltate numeroase concepte, multe dintre care s-au evideniat n diverse concursuri de robotic din ntreaga lume. Avnd la dispoziie elemente mecanice cu care se pot construi structuri i mecanisme, elemente electronice cum ar fi senzori, servomotoare i microcontrolere, rezult c setul NXT permite construirea i programarea unui robot mobil reconfigurabil deosebit de versatil. Elementele mecanice ale sistemului respect binecunoscutul principiu LEGO al interconexiunii dintre elemente modularizate. Utilizarea acestor elemente are ca scop realizarea subansamblului mecanic al sistemului mecatronic: mecanismul care va fi acionat

i controlat n scopul ndeplinirii unui task. Aceste elemente cuprind diferite tipuri de articulaii (cilindrice, sferice de translaie), roi dinate (cilindrice i conice), came, transmisii prin curele, reductoare planetare etc. Echipamentul NXT cuprinde: Unitatea central Este centrul operaional al sistemului. Execut programele de procesare utilizatorului i controleaz comunicarea cu senzorii, servomotoarele, cu PC-ul sau cu alte uniti NXT 3 servomotoare Motoare electrice de curent continuu alimentate la 9V sau 12V (pe perioade scurte). cu Determin distana de la senzor pn la cel mai apropiat obiect folosind un sistem similar sonarului Este practic un ntreruptor, avnd dou stri: apsat sau eliberat Senzor fotosensibil capabil s disting 4 culori sau variaii n intensitatea luminii ambiente Msoar intensitatea sonor n decibeli (microfon) de Servomotoarele i senzorii se conecteaz la unitatea central de procesare prin cabluri cu cte 6 fire care suport interfaa analoga i digitala Robotul se poate conecta la PC prin USB sau prin Bluetooth

Senzor ultrasunete

Senzor de contact Senzor optic

Senzor acustic Cabluri interconectare

Cablu USB

Suita software Conine driverele necesare i mediul de programare proprietar NXT-G pentru PC

Sistemul de procesare al robotului cuprinde dou microcontrolere, cel principal execut programele utilizatorului, iar cel auxiliar este destinat controlului prin PWM al servomotoarelor i meninerea vitezei n funcie de sarcin.

Sub aceeai carcas sunt gzduite i interfeele de comunicare cu senzorii, microcontrolerul de comunicare Bluetooth, afiajul cu cristale lichide, butoanele de navigare prin meniu, un difuzor i porturile de conectare cu senzorii i servomotoarele.

Figura 2. Diagrama sistemului de procesare NXT

2. Comunicaia NXT - PC
Legtura fizic dintre PC i robotul NXT se poate realiza prin cablu USB sau prin conexiune Bluetooth 2.0. Avantajele comunicrii prin cablul USB se restrng la o vitez de transfer superioar i compatibilitate garantat cu PC-ul. Avantajele conexiunii Bluetooth: Nu limiteaz mobilitatea robotului sau raza de aciune Robotul NXT se poate conecta la maxim 10 alte dispozitive Bluetooth, fie c sunt tot roboti NXT sau PC Comunicaia este rezistent la interferene radio datorit tehnicii spread spectrum (salt de frecven)

 Este omnidirecional i nu necesit o poziionare n linie cu celelalte dispozitive pentru conectare Ca atare, pentru dezvoltarea programelor care asigur comunicarea dintre robotul NXT i PC, s-a ales conexiunea Bluetooth. Pentru comunicarea PC-ului cu robotul NXT trebuie configurat protocolul de comunicare prin port serial. Aceasta se poate face din software de pe PC i realizeaz o alocare a conexiunii Bluetooth cu un anumit dispozitiv la un anumit port serial al PC-ului (exemplu: COM1, COM2, etc.). n figura 3 sunt ilustrai paii tipici de configurare a conexiunii Bluetooth dintre PC i un robot NXT:

Figura 3. Configurarea conexiunii Bluetooth dintre PC i NXT Configurarea conexiunii Bluetooth duce la stabilirea unui port serial al calculatorului dedicat comunicrii dintre PC i robotul NXT (exemplu: COM6). Numele portului rezervat pentru

conexiunea Bluetooth cu robotul NXT poate s difere de la PC la PC. Cu aceast informaie se poate implementa programatic o metod de iniiere i control a conexiunii Bluetooth dintre PC i robotul NXT.

3. Determinarea poziiei obiectelor cu senzorul de ultrasunete

Robotului NXT i se pot ataa 4 senzori la porturile de intrare (1, 2, 3, 4). Senzorii standard cu care este dotat robotul NXT sunt capabili s msoare distana, intensitatea luminii i intensitatea sonor. mpreun cu posibilitatea de a determina rotaia fiecrui servomotor (cu ajutorul unui encoder ncorporat), aceti senzori alctuiesc un sistem senzorial foarte cuprinztor. Senzorul cu ultrasunete cu care este echipat robotul LEGO Mindstorms NXT permite citirea distanei pn la cel mai apropiat obiect care se ncadreaz n raza de detecie a senzorului. Pot fi detectate obiecte aflate la distane de la 1-2 cm la aproximativ 255 cm cu o precizie ridicat. Cel mai uor de detectat sunt obiectele mari care au forma regulat i suprafaa dur. Obiectele din materiale moi, cu forme neregulate sau foarte subiri nu sunt bine detectate rezultnd n valori alternante ale distanei pn la obiect. Deoarece senzorul cu ultrasunete are un unghi de acoperire mare (aprox. 120o) nu se poate determina poziia obiectelor din raza sa de msurare sau numrul lor. Pentru determinarea poziiei obiectelor se poate construi un sistem din componenete LEGO NXT care cuprinde un servomotor, unitatea central i senzorul cu ultrasunete. Sistemul funcioneaz ca radarul de pe vasele maritime i scaneaz mprejurimile pentru construirea unui contur virtual al obiectelor din jur.

Figura 4. Citirea datelor de la senzori Senzorul este ataat rotorului servomotorului i se rotete stnga-dreapta pentru a acoperi ct mai mult din spaiul nconjurtor. Pe msur ce senzorul se rotete, datele colectate de la senzor sunt asociate cu poziiile rotorului fa de unghiul iniial. Pentru un control mai precis al rotaiei se folosete un raport de transmisie ntre servomotor i suportul senzorului. n principiu, astfel se poate determina att poziia unui obiect ct i dimensiunile acestuia (limea sau nlimea). Controlul servomotorului i citirea senzorului cu ultrasunete se face de pe PC prin Bluetooth folosind metoda i algoritmul de citire a datelor achiziionate de la senzorii NXT. Aplicaia prezint o interfa grafic facil prin intermediul creia se poate iniia conectarea sau deconectarea prin Bluetooth a PC-ului la NXT. Aplicaia realizeaz urmtoarele operaii:
realizeaz conectarea prin Bluetooth a celor dou sisteme: PC i robot NXT; seteaz modul de funcionare al senzorului cu ultrasunete efectueaz la un anumit interval de timp citiri ale valorii furnizate de senzorul cu

ultrasunete;
nregistreaz fiecare valoare a distanei citite i o asociaz cu valoarea rotaiei

motorului (unghiul) din momentul respectiv;

afieaz n interfa grafic valoarea curent a distanei msurate, a rotaiei

motorului i a rotaiei relative fa de momentul pornirii procesului;

n funcie de poziia rotorului, se decide schimbarea sensului de rotaie sau

continuarea micrii n sensul curent;

controleaz viteza i rotaia motorului dup fiecare msurare a distanei;

Deoarece msurarea distanei i transmisia informaiei prin Bluetooth cauzeaz o ntrziere considerabil din momentul trimiterii comenzii pn n momentul returnrii rezultatului, nu se poate msura n timp ce senzorul cu ultrasunete se rotete cu vitez constant. n consecin, msurarea distanei se face la intervale de timp precise, sincronizate cu staionarea motorului dup parcurgerea unei anumite rotaii. Astfel procesul de colectare a datelor este sacadat i lent, dar confer o precizie superioar. Analiza datelor obinute se poate face n chiar timpul colectrii lor datorit interfeei grafice care prezint n mod vizual o hart virtual a conturului spaiului nconjurtor.

Figura 5. Interfaa grafic a aplicaiei

Intuitiv, zonele cu negru reprezint umbre ale obiectelor situate n zona scanat (la distante mici), iar rou reprezint spaiul gol (distante mari). n continuare este prezentat codul surs pentru citirea i afiarea datelor de la senzorul cu ultrasunete, scris n limbajul de programare C#:
using using using using using using using using System; System.Collections.Generic; System.ComponentModel; System.Drawing; System.Data; System.Text; System.Windows.Forms; System.Collections;

using System.Drawing.Drawing2D; namespace PC2NXT { public partial class SonarMap : UserControl { private Brush brush1 = new SolidBrush(Color.Red); private Brush brush2 = new SolidBrush(Color.Black); private Brush brush3 = new SolidBrush(Color.AliceBlue); public int RotationRange = 0; public SonarMap() { InitializeComponent(); } private void SonarMap_Load(object sender, EventArgs e) { } public void Init(int RotationRange) { this.RotationRange = RotationRange; this.Clear(); label1.Text = ""; label2.Text = ""; } private ArrayList _FoundObjects; public ArrayList FoundObjects { get { return _FoundObjects; } set { _FoundObjects = value; } } public void Clear() { panel1.CreateGraphics().Clear(panel1.BackColor); } public void Draw(SomeObject obj) { Draw(obj, panel1.CreateGraphics()); } public void Draw(SomeObject obj, Graphics graphics) { int W = panel1.Width; int H = panel1.Height - 10;

int w = (int)(W / RotationRange); int h = (int)(H * obj.Distance / 255); int x = (int)(w * obj.Rotation); graphics.FillRectangle(brush1, x, 10 + H - h, 4, h); graphics.FillRectangle(brush2, x, 10, 4, H - h); graphics.FillRectangle(brush3, 0, 0, W, 10); graphics.FillPolygon(brush1, new Point[] {new Point(x - 3, 0), new Point(x, 8), new Point(x + 4, 0)}); } private void DrawAll(Graphics graphics) { if (this.FoundObjects != null) for (int i = 0; i < this.FoundObjects.Count; i++) Draw((SomeObject)this.FoundObjects[i], graphics);

} public void DrawAll(ArrayList PartiallyFoundObjects) { this.FoundObjects = PartiallyFoundObjects; DrawAll(panel1.CreateGraphics()); } private void panel1_Paint(object sender, PaintEventArgs e) { DrawAll(e.Graphics); } private void SonarMap_Resize(object sender, EventArgs e) { panel1.Width = this.Width; } private int? SelectionStart = null; private void panel1_MouseDown(object sender, MouseEventArgs e) { if (e.Button == MouseButtons.Right) { if (SelectionStart != null) { SelectionStart = null; panel1.Cursor = Cursors.Default; } else { SelectionStart = e.X; panel1.Cursor = Cursors.VSplit; } } } private void panel1_MouseMove(object sender, MouseEventArgs e) { if (SelectionStart != null) { label1.Text = "Distanta minima: " + GetMinDistanceBetween((int)SelectionStart, e.X); label1.Text = "intre " + SelectionStart + " i " + e.X;// + GetWidthBetween((int)SelectionStart, e.X); } else { label1.Text = "Distanta: " + GetDistanceAround(e.X); label2.Text = ""; } }

private int GetDistanceAround(int x) if (FoundObjects == null) return 0; int r = XToRotation(x); ArrayList vals = new ArrayList(); for (int i = 0; i < FoundObjects.Count; i++) { int R = ((SomeObject)FoundObjects[i]).Rotation; if (R == r) { return ((SomeObject)FoundObjects[i]).Distance; } else if (Math.Abs(R - r) < 3) { vals.Add(((SomeObject)FoundObjects[i]).Distance); } } i f ( v a l s . C o u n

t > 0) { int sum = 0; for (int i = 0; i < vals.Count; i++) { sum += (int)vals[i]; } return (int)(sum / vals.Count); } return 0;

private int GetMinDistanceBetween(int StartX, int x) { if (FoundObjects == null) return 0; int r1 = XToRotation(StartX); int r2 = XToRotation(x); if (r1 > r2){ int a = r1; r1 = r2; r2 = a; } ArrayList vals = new ArrayList(); for (int i = 0; i < FoundObjects.Count; i++) { int R = ((SomeObject)FoundObjects[i]).Rotation; if (r1 <= R && R <= r2) { vals.Add(((SomeObject)FoundObjects[i]).Distance); } } if (vals.Count > 0){ int min = (int)vals[0]; for (int i = 1; i < vals.Count; i++){ if ((int)vals[i] < min) min = (int)vals[i]; } return min; } return 0; } private int XToRotation(int x) { return (int)(x / (panel1.Width / RotationRange)); } }

4.Bibliografie
1. ***, LEGO Mindstorms NXT Hardware Developer Kit. 2.***, LEGO Mindstorms NXT Bluetooth Developer Kit 3. ***, LEGO Mindstorms NXT Direct commands 4.***, Mindstorms Education NXT User Guide