OK Mégsem
Fórum témák

» Több friss téma
Cikkek » 5.000 Ft-os Launchpad Robot
5.000 Ft-os Launchpad Robot
Szerző: Fizikus, idő: Jún 3, 2013, Olvasva: 21520, Oldal olvasási idő: kb. 2 perc
Lapozás: OK   9 / 10

A robotvezérlő program

A robotvezérlő program feladata egyszerű: beolvassa a vonalérzékelő szenzorok jeleit, melyek alapján meghatározza a robot válaszát és ennek megfelelően vezérli a motorokat. Az előző részben tárgyalt vonalkövető algoritmus Energia IDE nyelvre lekódolva:

 

  1. #define EN1 9 // Pin9 (P2.1) : Motor1 sebesseg (PWM)
  2. #define M1 10 // Pin10 (P2.2) : Motor1 irany (Bal Motor)
  3. #define EN2 12 // Pin12 (P2.4) : Motor2 sebesseg (PWM)
  4. #define M2 11 // Pin11 (P2.3) : Motor2 irany (Jobb Motor)
  5.  
  6. int BalSzenzor = A4; // Bal oldali szenzor az A4 analog labra kotve - Pin6 (P1.4)
  7. int JobbSzenzor = A7; // Jobb oldali szenzor az A7 analog labra kotve - Pin15 (P1.7)
  8. int balertek; // Bal szenzoron mert ertek
  9. int jobbertek; // Jobb szenzoron mert ertek
  10. int Hatarertek = 720; // szenzor hatarertek
  11. int MotorSebesseg = 30; // Kezdeti motorsebesseg megadasa
  12.  
  13. void setup()
  14. {
  15.   pinMode(EN1, OUTPUT); // Motor1 sebesseg (PWM)
  16.   pinMode(EN2, OUTPUT); // Motor2 sebesseg (PWM)
  17.   pinMode(M1, OUTPUT); // Motor1 irany (Bal Motor)
  18.   pinMode(M2, OUTPUT); // Motor2 irany (Jobb Motor)
  19.   MotorBal(MotorSebesseg); // Robot: Elore
  20.   MotorJobb(MotorSebesseg);
  21.   Serial.begin(9600); // 9600 baud-os soros kommunikacio beallitasa
  22. }
  23.  
  24. void loop() // vegtelen ciklus
  25. {
  26.   balertek = analogRead(BalSzenzor); // Bal szenzoron levo feszultseg merese (0 - 1023 kozotti ertek)
  27.   jobbertek = analogRead(JobbSzenzor); // Jobb szenzoron levo feszultseg merese (0 - 1023 kozotti ertek)
  28.  
  29. // Ha mindket szenzor a vilagos padlo felett van: Elore
  30. if (balertek>Hatarertek && jobbertek>Hatarertek)
  31.  {
  32.   MotorBal(MotorSebesseg); //Bal motor: elore
  33.   MotorJobb(MotorSebesseg); //Jobb motor: elore
  34.  }
  35. // Ha a Bal szenzor a vonal felett, a Jobb pedig a padlo felett van: Balra kanyarodas
  36. else if (balertek<Hatarertek && jobbertek>Hatarertek)
  37.  {
  38.   MotorBal(0); // Bal motor: stop
  39.   MotorJobb(MotorSebesseg); // Jobb motor: elore
  40.  }
  41. // Ha a Jobb szenzor a vonal felett, a Bal pedig a padlo felett van: Jobbra kanyarodas
  42. else if (balertek>Hatarertek && jobbertek<Hatarertek)
  43.  {
  44.   MotorBal(MotorSebesseg); // Bal motor: elore
  45.   MotorJobb(0); // Jobb motor: stop
  46.  }
  47. // Mindket szenzor a vonal felett van (keresztezodes): Elore
  48. else if (balertek<Hatarertek && jobbertek<Hatarertek)
  49.  {
  50.   MotorBal(MotorSebesseg); // Bal motor: elore
  51.   MotorJobb(MotorSebesseg); // Jobb motor: elore
  52.  }
  53.  
  54. // A mert szenzoradatok elkuldese a PC-re
  55. // (szenzor hatarertek meghatarozasakor es tesztelesnel hasznaltam)
  56. Serial.print("Bal Szenzor = " );
  57. Serial.print(balertek);
  58. Serial.print(" Jobb Szenzor = ");
  59. Serial.println(jobbertek);
  60.  
  61. delay(50); // Varakozas 50ms-ig
  62. }
  63. 　
  64. void MotorBal(int sebesseg)
  65. {
  66.   if (sebesseg>0)
  67.   {
  68.    digitalWrite(M1,HIGH);
  69.    analogWrite(EN1,sebesseg*255/100);
  70.   }
  71.  else
  72.   {
  73.    digitalWrite(M1,LOW);
  74.    analogWrite(EN1,abs(sebesseg)*255/100);
  75.   }
  76. }
  77. void MotorJobb(int sebesseg)
  78. {
  79.  if (sebesseg>0)
  80.   {
  81.    digitalWrite(M2,HIGH);
  82.    analogWrite(EN2,sebesseg*255/100);
  83.   }
  84. else
  85.   {
  86.    digitalWrite(M2,LOW);
  87.    analogWrite(EN2,abs(sebesseg)*255/100);
  88.   }
  89. }

 

A főprogram legelején, a loop() végtelen cikuls folyamatosan beolvassa az analogRead() utasítással a jobb és bal szenzor jeleit a balertek és jobbertek nevű változókba. Az if() és else if() utasítások azt vizsgálják, hogy melyik szenzor van a vonal vagy a padló felett (melyik értéke van a 720-as határérték alatt és felett), és ettől függően a korábban tárgyalt vonalkövető algoritmusnak megfelelően úgy vezéreli a Launchpad panel a motorokat, hogy a követendő vonal mindig a két szenzor között legyen.

A robot irányításának a leegyszerűsítése érdekében létrehoztam a MotorBal és MotorJobb motorvezérlő függvényeket. A függvényeknek 100 és -100 közé eső számot kell megadni paraméternek (előre 100% - hátra 100%). A motorvezérlő függvényeken belül a motorok forgásirányát a digitalWrite() utasítás, a motorok sebességét pedig az analogWrite() utasítás állítja.

 

A program feltöltése

Most, hogy a robot már elkészült, már csak a Launchpad panel programozása maradt hátra. Győződjünk meg arról, hogy az MSP2553-as IC van a panelba helyezve (a dobozból kivéve, alapból ez az IC van a panelben) és hogy az Energia programban is az MSP2553-as IC van kiválasztva a "Tools/Board" menüben. Miután bemásoltuk a mellékelt kódot az Energia-ba, fordítsuk le a Compile gombra kattintva. A lefordított program az Upload gombbal tölthető fel a Launchpad panelre. A program feltöltése után végre el is készült a robot.

A programban használt Serial.print() és Serial.println() utasítások a hardveres soros portot használják, és csak az MSP430G2553-es mikrovezérlővel működnek, mert az UART kommunikációt a Launchpad kártyához kapott mikrovezérlők közül csak az MSP430G2553 támogatja hardveresen. (MSP430G2553 esetén HW soros port konfigurálás kell, és ahogy a kártya feliratozása is mutatja, az átkötéseket elforgatott állásban kell felhelyezni ).

 

A végére egy kis video:

robotvideo.jpg 

(a videóhoz kattints a képre) 

 

Továbbfejlesztés

Az alváz és a Launchpad-re csatlakozó motorvezérlő panel egy stabil kiindulási alapot biztosít a későbbi esetleges továbbfejlesztésekhez. A vonalérzékelő szenzorok számát és elrendezését változtatva bonyolultabb és sokkal hatékonyabb vonalkövető algoritmusok is alkalmazhatóak.

 

Ezzel a cikk végére is értem. Remélem tetszett, és sokan kedvet kapnak tőle a Launchpad panel használatához és a robotépítéshez.

Üdvözlettel: Fizikus


A cikk még nem ért véget, lapozz!
Következő: »»   9 / 10
Értékeléshez bejelentkezés szükséges!
Bejelentkezés

Belépés
Hirdetés
XDT.hu
Az oldalon sütiket használunk a helyes működéshez. Bővebb információt az adatvédelmi szabályzatban olvashatsz. Megértettem