Universitatea “Ştefan cel Mare” Suceava
Facultatea de Inginerie Electrică şi Ştiinţa Calculatoarelor
Program de studiu: Tehnici Avansate în Maşini şi Acţionări Electrice, anul I
Programarea robotului LEGO Mindstorms NXT
Îndrumător: Masterand:
Conf. univ. dr. ing POPA Cezar Dumitru TANASĂ Ana-Maria
1. Generalităţi
Figura 1. Robotul LEGO Mindstorms NXT
LEGO Mindstorms este o serie de componente LEGO care cuprinde unităţi
programabile, senzori, servomotoare şi alte părţi constructive.
Produsul LEGO Mindstorms NXT a fost lansat în iulie 2006 şi este succesorul primului
model, Robotics Invention System (RCX), care a stârnit interesul unui număr mare de
amatori. Succesul setului NXT este argumentat în special de uşurinţa cu care se poate
personaliza şi extinde platforma. Au fost dezvoltate numeroase concepte, multe dintre care
s-au evidenţiat în diverse concursuri de robotică din întreaga lume.
Având la dispoziţie 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 acţionat
şi controlat în scopul îndeplinirii unui task. Aceste elemente cuprind diferite tipuri de
articulaţii (cilindrice, sferice de translaţie), roţi dinţate (cilindrice şi conice), came, transmisii
prin curele, reductoare planetare etc.
Echipamentul NXT cuprinde:
Unitatea centrală de procesare
Este centrul operaţional al sistemului. Execută programele utilizatorului şi controlează comunicarea cu senzorii, servomotoarele, cu PC-ul sau cu alte unităţi NXT
3 servomotoare Motoare electrice de curent continuu alimentate la 9V sau 12V (pe perioade scurte).
Senzor cu ultrasunete
Determină distanţa de la senzor până la cel mai apropiat obiect folosind un sistem similar sonarului
Senzor de contact Este practic un întreruptor, având două stări: apăsat sau eliberat
Senzor optic Senzor fotosensibil capabil să distingă 4 culori sau variaţii în intensitatea luminii ambiente
Senzor acustic Măsoară intensitatea sonoră în decibeli (microfon)
Cabluri de interconectare
Servomotoarele şi senzorii se conectează la unitatea centrală de procesare prin cabluri cu câte 6 fire care suportă interfaţa analoga şi digitala
Cablu USB Robotul se poate conecta la PC prin USB sau prin Bluetooth
Suita software pentru PC
Conţine driverele necesare şi mediul de programare proprietar NXT-G
Sistemul de procesare al robotului cuprinde două microcontrolere, cel principal
execută programele utilizatorului, iar cel auxiliar este destinat controlului prin PWM al
servomotoarelor şi menţinerea vitezei în funcţie de sarcină.
Sub aceeaşi carcasă sunt găzduite şi interfeţele de comunicare cu senzorii, microcontrolerul
de comunicare Bluetooth, afişajul 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. Comunicaţia NXT - PC
Legătura fizică dintre PC şi robotul NXT se poate realiza prin cablu USB sau prin
conexiune Bluetooth 2.0. Avantajele comunicării prin cablul USB se restrâng la o viteză de
transfer superioară şi compatibilitate garantată cu PC-ul.
Avantajele conexiunii Bluetooth:
• Nu limitează mobilitatea robotului sau raza de acţiune
• Robotul NXT se poate conecta la maxim 10 alte dispozitive Bluetooth, fie că sunt tot
roboti NXT sau PC
• Comunicaţia este rezistentă la interferenţe radio datorită tehnicii spread spectrum
(salt de frecvenţă)
• Este omnidirecţională şi nu necesită o poziţionare î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 ilustraţi paşii 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
comunicării 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ă informaţie
se poate implementa programatic o metodă de iniţiere şi control a conexiunii Bluetooth
dintre PC şi robotul NXT.
3. Determinarea poziţiei obiectelor cu senzorul de ultrasunete
Robotului NXT i se pot ataşa 4 senzori la porturile de intrare (1, 2, 3, 4). Senzorii
standard cu care este dotat robotul NXT sunt capabili să măsoare distanţa, intensitatea
luminii şi intensitatea sonoră. Împreună cu posibilitatea de a determina rotaţia fiecărui
servomotor (cu ajutorul unui encoder încorporat), aceşti senzori alcătuiesc un sistem
senzorial foarte cuprinzător.
Senzorul cu ultrasunete cu care este echipat robotul LEGO Mindstorms NXT permite citirea
distanţei până la cel mai apropiat obiect care se încadrează în raza de detecţie a senzorului.
Pot fi detectate obiecte aflate la distanţe de la 1-2 cm la aproximativ 255 cm cu o precizie
ridicată. Cel mai uşor de detectat sunt obiectele mari care au forma regulată şi suprafaţa
dură. Obiectele din materiale moi, cu forme neregulate sau foarte subiri nu sunt bine
detectate rezultând în valori alternante ale distanţei până la obiect.
Deoarece senzorul cu ultrasunete are un unghi de acoperire mare (aprox. 120o) nu se
poate determina poziţia obiectelor din raza sa de măsurare sau numărul lor.
Pentru determinarea poziţiei obiectelor se poate construi un sistem din componenete LEGO
NXT care cuprinde un servomotor, unitatea centrală şi senzorul cu ultrasunete. Sistemul
funcţionează 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 ataşat rotorului servomotorului şi se roteşte stânga-dreapta pentru a
acoperi cât mai mult din spaţiul înconjurător. Pe măsură ce senzorul se roteşte, datele
colectate de la senzor sunt asociate cu poziţiile rotorului faţă de unghiul iniţial. Pentru un
control mai precis al rotaţiei se foloseşte un raport de transmisie între servomotor şi suportul
senzorului.
În principiu, astfel se poate determina atât poziţia unui obiect cât şi dimensiunile acestuia
(lăţimea sau înălţimea). Controlul servomotorului şi citirea senzorului cu ultrasunete se face
de pe PC prin Bluetooth folosind metoda şi algoritmul de citire a datelor achiziţionate de la
senzorii NXT.
Aplicaţia prezintă o interfaţă grafică facilă prin intermediul căreia se poate iniţia conectarea
sau deconectarea prin Bluetooth a PC-ului la NXT.
Aplicaţia realizează următoarele operaţii:
realizează conectarea prin Bluetooth a celor două sisteme: PC şi robot NXT;
setează modul de funcţionare al senzorului cu ultrasunete
efectuează la un anumit interval de timp citiri ale valorii furnizate de senzorul cu ultrasunete;
înregistrează fiecare valoare a distanţei citite şi o asociază cu valoarea rotaţiei motorului (unghiul) din momentul respectiv;
afişează în interfaţă grafică valoarea curentă a distanţei măsurate, a rotaţiei motorului şi a rotaţiei relative faţă de momentul pornirii procesului;
în funcţie de poziţia rotorului, se decide schimbarea sensului de rotaţie sau continuarea mişcării în sensul curent;
controlează viteza şi rotaţia motorului după fiecare măsurare a distanţei;
Deoarece măsurarea distanţei şi transmisia informaţiei prin Bluetooth cauzează o întârziere
considerabilă din momentul trimiterii comenzii până în momentul returnării rezultatului, nu
se poate măsura în timp ce senzorul cu ultrasunete se roteşte cu viteză constantă.
În consecinţă, măsurarea distanţei se face la intervale de timp precise, sincronizate cu
staţionarea motorului după parcurgerea unei anumite rotaţii. Astfel procesul de colectare a
datelor este sacadat şi lent, dar conferă o precizie superioară.
Analiza datelor obţinute se poate face în chiar timpul colectării lor datorită interfeţei grafice
care prezintă în mod vizual o hartă virtuală a conturului spaţiului înconjurător.
Figura 5. Interfaţa grafică a aplicaţiei
Intuitiv, zonele cu negru reprezintă “umbre” ale obiectelor situate în zona scanată (la
distante mici), iar roşu reprezintă spaţiul gol (distante mari).
În continuare este prezentat codul sursă pentru citirea şi afişarea datelor de la senzorul cu ultrasunete, scris în limbajul de programare C#:
using System; using System.Collections.Generic; using System.ComponentModel; using System.Drawing; using System.Data; using System.Text; using System.Windows.Forms; using 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); }
}
if (vals.Count > 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