Robottechnika projektfeladat Kovács Dániel (G6HEBK) Pethő Ádám (GMQYM2)
A feladat leírása 2x2x2-es Rubik kocka megkeverése több lépésben. Célkitűzések A Rubik kockát lehessen forgatni Mindhárom tengelye körül megoldott legyen a forgatás A műveletek módosítása kellően egyszerű legyen Kivitelezés A robot a 45mm oldalhosszúságú Rubik kocka felső sorainál fogja meg a kockát, hogy az a későbbiekben forgatható legyen. Ezután a robotkar az asztal széle fölé viszi, majd lassú mozgással a kocka alsó sorait az asztal pereméhez nyomja. Ezzel meggátolja, hogy a robot utolsó, J6 tengelyének forgatásával az alsó és felső sorok is együtt mozogjanak. A kocka másik két tengelye körüli forgatás is megvalósítható, ehhez azonban le kell tenni az asztallapra a kockát, és felvenni azt egy másik irányból megközelítve. Ilyen típusú parancs kiadásakor a robot az asztal sarka fölé megy és leteszi az alkatrészt. Ezután a robot addig helyezgeti a kockát, amíg annak a művelet során kiválasztott lapja felül nem lesz. A forgatás parancs eztán újból kiadható. A műveletek befejezésével a robot visszateszi a kockát a kezdeti helyére, majd visszatér a HOME pozíciójába. Tárolt értékek Regiszterek R[i] = 0 R[LENGTH] = 0 Az i változó értéke adja meg, hogy egy forgatási művelet hányszor menjen végbe. i=1 esetén a robot 90 -ot, i=2 esetében 2x90 -ot forgat a kockán. A LENGTH változó a kockán elvégzett forgatások számát adja vissza. Pozíció regiszterek PR[HOME] PR[PICK0] PR[PICK] PR[TURN X0] PR[TURN X] 1
PR[TURN Y0] PR[TURN Y] PR[TURN Z0] PR[TURN Z] PR[ROTATE0] PR[ROTATE] PR[TMP] A HOME regiszter a robot kezdeti pozíciója. A PICK és PICK0 regiszterek a kocka helyét jelölik. Minden 0-ra (nullára) végződő regiszter azt a helyet jelöli, ahova a robotnak mennie kell, mielőtt a tényleges pozícióba jutna. A TURN X, TURN Y, TURN Z és ezek nullára végződő megfelelőik a kocka egészének forgatásában segítenek. Előbbi a kockát az asztal X irányából, második Y irányából, legutóbbi pedig felülről határolja be. Ez a három megfogási irány elegendő ahhoz, hogy a kockát bármelyik lapjával felülre forgassuk. A ROTATE regiszter a kocka tengelye körüli forgatás helyét jelöli. Ez a pozíció az asztal pereménél van. A TMP-nek nincs kitüntetett pozíciója, csak a segédváltozó szerepét tölti be. Értékét a program változtathatja. Sztringek SR[STEPS] = SR[TOP] = T SR[FRONT] = F SR[RIGHT] = R SR[CURRENT] = A STEPS változó tárolja, hogy a program futása során a Rubik kocka mely tengelyei körül lett megforgatva. Ezáltal a program végeztével kiolvasható a regiszterből az a lépéssorozat, amelyet fordítva lejátszva kirakható a kocka forgatás előtti állapota. A TOP, FRONT és RIGHT a kocka oldalait jelöli, ezek értékei kerülnek a STEPS-be a program futása során. A CURRENT értéke mindig a kockának a felül lévő oldala. 2
Programok MAIN A MAIN a főprogram, amelyet a robotnak le kell futtatnia ahhoz, hogy a megvalósuljon. Mindenekelőtt a program inicializálja magát, azaz törli az előző program szemetét ; a STEPS változó értékét törli, illetve a CURRENT, azaz a kocka aktuálisan felfele néző oldalának kijelöli a TOP oldalt. Majd ezek után beáll a HOME pozícióba. Az inicializálás után a robot megfogja a kockát. Hogy a Roboguide szoftverben is látható legyen a megfogás, meghívja a PICKUP1 szimulációs programot. Ezek után felemeli az asztallapról és nekilát a forgatáshoz. Minden forgatási parancs előtt be kell állítani az i regiszternek azt az értéket, ahányszor meg szeretnénk forgatni a kockát. Maga a forgatás a ROTATE program meghívásával adható ki. Amennyiben a kocka másik tengelyei körül is szeretnénk forgatni, úgy kell forgatni a kocka egészét, hogy az adott tengely függőleges legyen. Ezekért a SELECT_TOP, SELECT_FRONT és SELECT_RIGHT programok felelnek, amelyek úgy forgatják a kockát, hogy annak rendre a felső, elülső és jobb oldalai felfelé nézzenek. Az alábbi program a következő műveleteket végzi el: Kétszer forgatja a kocka felső (TOP) oldallapját, egyszer az elülsőt (FRONT), majd kétszer a jobb oldalit (RIGHT), végezetül kiválasztja a felső oldalt (TOP). 1.! Inicializálás 2. SR[1:STEPS] = SR[25] 3. SR[5:CURRENT] = SR[2:TOP] 4. J PR[1:HOME] 100% FINE 5. 6.! Kocka felvétele 7. J PR[2:PICK0] 50% FINE 8. L PR[3:PICK] 50mm/sec FINE 9.! Megfogás 10. WAIT.50(sec) 11. CALL PICKUP1 12. WAIT.50(sec) 13.! Felemel 14. L PR[2:PICK0] 50mm/sec FINE 15. 16.! Műveletek a kockával 17. 18.! Forgat kétszer 19. R[1:i] = 2 20. CALL ROTATE 21.! A kocka Front oldalára vált és egyszer megforgat 22. CALL SELECT_FRONT 23. R[1:i] = 1 24. CALL ROTATE 25.! A kocka Right oldalára vált és kétszer megforgat 26. CALL SELECT_RIGHT 27. R[1:i] = 2 3
28. CALL ROTATE 29.! A kiinduló Top oldalt teszi aktívvá 30. CALL SELECT_TOP 31. 32.! Kockát visszaviszi a kezdőhelyére 33. J PR[2:PICK0] 50% FINE 34. L PR[3:PICK] 50mm/sec FINE 35.! Kocka elengedése 36. WAIT.50(sec) 37. CALL DROPDOWN1 38. WAIT.50(sec) 39.! Visszatérés a Home pozícióba 40. L PR[2:PICK0] 50mm/sec FINE 41. J PR[1:HOME] 100% FINE 42. 43.! Műveletek számának mentése regiszterbe 44. R[2:LENGTH] = STRLEN SR[1:STEPS] A műveletek elvégzése után a program visszatér a kocka kezdőhelyére, elengedi azt a DROPDOWN1 szimulációs program meghívásával, majd visszatér a robot a HOME pozíciójába. Végezetül megszámolja az elvégzett műveletek és elmenti azt a LENGTH regiszterbe. ROTATE Ez a program felel a Rubik kocka oldalainak elforgatásáért. A robot viszonylag lassan, 50mm/sec sebességgel hozzáérinti a kocka alsó sorát az asztal széléhez. A TMP pozíció regiszterbe a program eltárolja a ROTATE értékét. Erre azért van szükség, hogy a forgatáshoz ne kelljen felülírni a ROTATE eredeti koordinátáit. (T/F/R). Amint az Az LBL, JMP és IF utasításokkal létrehozunk egy For ciklust, amely mindaddig forgatja a kockát 90 -kal az óramutató járásával ellentétes irányba, amíg az i regiszter értéke nagyobb, mint 0. Minden ciklus végén a lépéssorozathoz hozzáadódik a kocka aktív oldallapjának kódja egyenlőtlenség igaz lesz, a program LBL[2]-re ugrik és ezzel kilép a ciklusból. 1.! A kockát az asztal széléhez érinti 2. J PR[10:ROTATE0] 100% FINE 3. L PR[11:ROTATE] 50mm/sec FINE 4. 5.! Shiftelés 6. PR[12:TMP] = PR[11:ROTATE] 7. 8.! Ciklus eleje 4
9. LBL[1] 10. IF R[1:i] <= 0, JMP LBL[2] 11.! Lépésszám dekrementálása 12. R[1:i] = R[1:i] - 1 13. 14.! Forgatás 15. WAIT.50(sec) 16.! Z tengely körüli forgatás +90 fokkal 17. PR[12,6:TMP] = PR[12,6:TMP] + 90 18. L PR[12:TMP] 30mm/sec FINE 19. WAIT.50(sec) 20.! Művelet hozzáadása a lépésekhez 21. SR[1:STEPS] = SR[1:STEPS] + SR[5:CURRENT] 22.! Visszaugrik a ciklus elejére 23. JMP LBL[1] 24. 25.! Forgatás befejezése, vissza ROTATE0 pozícióba 26. LBL[2] 27. L PR[10:ROTATE0] 30mm/sec FINE SELECT_TOP, SELECT_FRONT, SELECT_RIGHT E három program nem lesz külön részletezve, hisz mindegyik hasonló elven működik. Céljuk, hogy a kocka egészét úgy forgassák, hogy egy adott oldala felülre kerüljön, hogy a ROTATE program más tengelyek körül tudja megforgatni a munkadarabot. Szemet szúrhat, hogy a kocka 6 oldalából csak 3- hoz tartozik program. A BOTTOM, BACK és LEFT oldalak elhagyása esetünkben lehetséges, mivel egy 2x2x2-es Rubik kockánál ezek irrelevánsak. A program először összeveti az aktuálisan felül lévő oldalt a kívánttal. Amennyiben a kettő megegyezik, nincs több teendő. Ellenkező esetben a mutató arra a címkére ugrik, amelyik megvalósítja azt a forgatás-sorozatot, amellyel a kiválasztott oldal felülre kerül. SELECT_TOP 1. IF SR[5:CURRENT] = SR[2:TOP], JMP LBL[1] 2. IF SR[5:CURRENT] = SR[3:FRONT], JMP LBL[2] 3. IF SR[5:CURRENT] = SR[4:RIGHT], JMP LBL[3] 4. 5.! Top -> Top 6. LBL[1] 7. END 8.! Front -> Top 9. LBL[2] 10. CALL TURN_X 11. CALL TURN_X 12. CALL TURN_X 13. SR[5:CURRENT] = SR[2:TOP] 14. END 15.! Right -> Top 16. LBL[3] 17. CALL TURN_Y 18. SR[5:CURRENT] = SR[2:TOP] 19. END 5
SELECT_FRONT 1. IF SR[5:CURRENT] = SR[2:TOP], JMP LBL[1] 2. IF SR[5:CURRENT] = SR[3:FRONT], JMP LBL[2] 3. IF SR[5:CURRENT] = SR[4:RIGHT], JMP LBL[3] 4. 5.! Top -> Front 6. LBL[1] 7. CALL TURN_X 8. SR[5:CURRENT] = SR[3:FRONT] 9. END 10.! Front -> Front 11. LBL[2] 12. END 13.! Right -> Front 14. LBL[3] 15. CALL TURN_Y 16. CALL TURN_X 17. SR[5:CURRENT] = SR[3:FRONT] 18. END SELECT_RIGHT 1. IF SR[5:CURRENT] = SR[2:TOP], JMP LBL[1] 2. IF SR[5:CURRENT] = SR[3:FRONT], JMP LBL[2] 3. IF SR[5:CURRENT] = SR[4:RIGHT], JMP LBL[3] 4. 5.! Top -> Right 6. LBL[1] 7. CALL TURN_Y 8. SR[5:CURRENT] = SR[4:RIGHT] 9. END 10.! Front -> Right 11. LBL[2] 12. CALL TURN_X 13. CALL TURN_X 14. CALL TURN_X 15. CALL TURN_Y 16. SR[5:CURRENT] = SR[4:RIGHT] 17. END 18.! Right -> Right 19. LBL[3] 20. END 6
TURN_X, TURN_Y Lényegében e két program döntögeti a kockát. A TURN_X az asztal X tengelye körül, a TURN_Y értelemszerűen az Y tengelye körül. A programok megfelelő sorrendjével és kellő számú ismétlésével a kocka bármely oldalát felső helyzetbe lehet forgatni, eltérő lépésszámmal. TURN_X A program az asztal sarkához viszi a kockát, mert ott mindkét (X és Y) irányból könnyen hozzáférhető. Ezután leteszi a DROPDOWN2 szimulációs program meghívásával. A külön indexelésre azért van szükség, mert a DROPDOWN1 program által a kiinduló pozíciójába ugrott volna a kocka. Természetesen ez csak offline környezetben van így megoldva, a valóságban a két elengedési és megfogási mód közt nincs különbség. Miután a satus megfogó nyitott állapotba került, a robot oldalirányból (PR[TURN X]) ismét felemeli a kockát, majd felső helyzetbe megy (PR[TURN Z]). Ezeknél a mozgásoknál fontos a lineáris mozgás, mert jointos esetén könnyen megváltozhatnak a Rubik kocka geometriai jellemzői. 1.! Mozgás az asztal sarka fölé és leteszi a kockát 2. J PR[8:TURN Z0] 100% FINE 3. L PR[9:TURN Z] 50mm/sec FINE 4.! Kocka elengedése 5. WAIT.50(sec) 6. CALL DROPDOWN2 7. WAIT.50(sec) 8. L PR[8:TURN Z0] 50mm/sec FINE 9. 10.! Felvétel oldalról és eldöntés az X tengely körül 11. L PR[4:TURN X0] 50mm/sec FINE 12. L PR[5:TURN X] 50mm/sec FINE 13.! Kocka megfogása 14. WAIT.50(sec) 15. CALL PICKUP2 16. WAIT.50(sec) 17. L PR[4:TURN X0] 50mm/sec FINE 18. 19.! Mozgás a sarok fölé, a kezdeti pozícióba 20. L PR[8:TURN Z0] 50mm/sec FINE TURN_Y A módszer hasonló az előzőhöz, azonban mégis van egy lényeges különbség. Az előző programoknál feltűnhetett, hogy néhány oldalkiválasztás egyszerűbben, vagy másképp is megvalósítható lett volna. Amennyiben a rövidebb megoldásokat választottuk volna, bekövetkezett volna az a probléma a sokadik forgatás után, hogy a program hamisan ítéli meg, melyik oldal van felül. Például, ha a felső lap után a jobbat választjuk, majd a jobb után ismét a felsőt, nem biztos, hogy a kocka kezdeti és végső állapota ugyanaz lett volna. Természetesen ez a probléma kiküszöbölhető komplexebb algoritmusokkal. 7
Ebből a problémából adódóan hosszabb a TURN_Y program a TURN_X-nél. Miután a robot elvégezte a kocka forgatását az asztal Y tengelye körül, el kell engednie a kockát. Erre azért van szükség, mert az effektor 90 -kal el van fordulva az eredeti pozíciójához képest, viszont a kockát nem szeretnénk tovább forgatni, mert abban az esetben fellépne a fentebb vázolt probléma. A forgatásnál memória-takarékosság céljából ismét használatba vettük a TMP pozíció regisztert. 1.! Mozgás az asztal sarka fölé és leteszi a kockát 2. J PR[8:TURN Z0] 100% FINE 3. L PR[9:TURN Z] 50mm/sec FINE 4.! Kocka elengedése 5. WAIT.50(sec) 6. CALL DROPDOWN2 7. WAIT.50(sec) 8. L PR[8:TURN Z0] 50mm/sec FINE 9. 10.! Felvétel oldalról és eldöntés az Y tengely körül 11. L PR[6:TURN Y0] 50mm/sec FINE 12. L PR[7:TURN Y] 50mm/sec FINE 13.! Kocka megfogása 14. WAIT.50(sec) 15. CALL PICKUP2 16. WAIT.50(sec) 17. L PR[6:TURN Y0] 50mm/sec FINE 18. 19.! A döntés irányából adódóan a megfogás 90 fokkal el van forgatva 20. PR[12:TMP] = PR[8:TURN Z0] 21. PR[12,6:TMP] = PR[12,6:TMP] + 90 22. 23.! Kocka letétele az asztal sarkára 24. L PR[12:TMP] 50mm/sec FINE 25.! A Z magasság az asztal lapjára essen 26. PR[12,3:TMP] = PR[9,3:TURN Z] 27. L PR[12:TMP] 50mm/sec FINE 28.! Kocka elengedése 29. WAIT.50(sec) 30. CALL DROPDOWN2 31. WAIT.50(sec) 32. PR[12,3:TMP] = PR[8,3:TURN Z0] 33. L PR[12:TMP] 50mm/sec FINE 34. 35.! Helyes megfogás 36. L PR[8:TURN Z0] 50mm/sec FINE 37. L PR[9:TURN Z] 50mm/sec FINE 38. WAIT.50(sec) 39. CALL PICKUP2 40. WAIT.50(sec) 41. L PR[8:TURN Z0] 50mm/sec FINE 8
Értékelés A programmal teljesítettük az eredeti célokat. A Rubik kockát lehessen forgatni Mindhárom tengelye körül megoldott legyen a forgatás A műveletek módosítása kellően egyszerű legyen A feladat továbbfejleszthető az alábbiakkal: A program visszafelé is hajtsa végre a műveleteket, kirakva ezzel az eredeti állapotot A bemutatott problémától független legyen a forgatás 3x3x3-as Rubik kocka 9