+ All Categories
Home > Documents > Programarea Robotului LEGO Mindstorms NXT

Programarea Robotului LEGO Mindstorms NXT

Date post: 09-Aug-2015
Category:
Upload: ana-maria-tanasa
View: 112 times
Download: 4 times
Share this document with a friend
15
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
Transcript
Page 1: Programarea Robotului LEGO Mindstorms NXT

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

Page 2: Programarea Robotului LEGO Mindstorms NXT

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

Page 3: Programarea Robotului LEGO Mindstorms NXT

ş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ă.

Page 4: Programarea Robotului LEGO Mindstorms NXT

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ţă)

Page 5: Programarea Robotului LEGO Mindstorms NXT

• 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

Page 6: Programarea Robotului LEGO Mindstorms NXT

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.

Page 7: Programarea Robotului LEGO Mindstorms NXT

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;

Page 8: Programarea Robotului LEGO Mindstorms NXT

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

Page 9: Programarea Robotului LEGO Mindstorms NXT

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

Page 10: Programarea Robotului LEGO Mindstorms NXT

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

}

Page 11: Programarea Robotului LEGO Mindstorms NXT

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;

Page 12: Programarea Robotului LEGO Mindstorms NXT

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


Recommended