Fjernstyring af Lego-robot med WiiMote og Tahoe-II

Transkript

1 Fjernstyring af Lego-robot med WiiMote og Tahoe-II WEM1 projektrapport Lasse Haugsted Rasmussen Jeppe Langhoff Sørensen Martin Slotsdal Madsen Peter Vestergaard Nielsen

2 Indledning Vi har til WEM1 projektet stillet os selv den opgave at kunne styre en Lego Mindstorm robot vha. en WiiMote. Det har været vores plan at forbinde WiiMoten med et Tahoe-II-kort via. bluetooth, bearbejde de data der bliver modtaget fra WiiMoten, og på baggrund af disse, styre robotten via bluetooth. Vi vil i denne rapport beskrive projektforløbet, hvad der er gået godt og hvad der er gået mindre godt. Ydermere vil vi gå lidt i dybden med de specifikke tekniske løsninger vi har valgt. Side 2 af 15

3 Deployment the saga Vores udgangspunkt: Styr en robot med en WiiMote. Hvilke enheder er nødvendige, og hvordan skal de kobles sammen? En visuel beskrivelse af følgende kapitel kan findes i bilag 1. WiiMote til Tahoe-II Vores første forsøg og oprindelig tanke på en løsning, var at bruge Tahoe-II kortet som logisk bindeled og fortolker til en robot. Tahoe-II-kortet skulle ved hjælp af bluetooth modtage WiiMotens signaler, fortolke dem og benytte disse til at styre en legorobot. Legorobotten ville vi styre simpelt med et par DC-motorer med output fra Tahoe-II-kortet. Men under vores research fandt vi, at robotten vi benyttede, en Lego NXT, understøttede bluetooth. Det betød at vi kunne gøre det hele trådløst og det blev vores nye mål. Hurtigt kunne vi styre robotten fra computer via bluetooth ved hjælp af et.net tredjepartsbibliotek kaldet NXT Sharp. Brugen af bluetooth fra Tahoe-II-kortet var et langt større problem. Bluetoothmodulet vi benytter, understøtter blot SPP, altså simpel seriel trådløs kommunikation, og ikke HID-interfacet som WiiMotes benytter sig af. Heldigvis benytter Lego NXT sig også af SPP og kommunikation mellem Tahoe-II og robot anså vi defor stadig for en mulighed. Det viste sig dog efter mange timers test og forsøg i hyperterminaler, at parring af Lego NXT og Tahoe-II-kortet ikke var mulig og derved blev initialisering af forbindelsen også et problem. Når vi åbnede serielporten på Tahoe-II kortet og sendte data, som vi havde testet var det samme, som det vi før havde styret robotten med fra computeren, reagerede robotten ikke. Efter mange forsøg i samme genre opgav vi denne løsningsmodel. Side 3 af 15

4 WiiMote til PDA For at holde os inde for fagets rammer besluttede vi at benytte en PDA som bindeled mellem WiiMote og Lego NXT. PDA en skulle have en vis understøttelse af HID-bluetooth-enheder og var derfor et naturligt valg. PDA en kunne dog heller ikke forstå vores besværlige WiiMote, da den kun understøtter standard HIDenheder. Til gengæld fik vi forbindelse til Lego NXT, hvorfor vi valgte at arbejde videre med PDA en. Men vi manglede stadig at løse hovedopgaven i vores projekt, - at styre en robot med en WiiMote og projektet benyttede sig ikke længere af Tahoe-II-kortet, et krav fra opgavebeskrivelsen. Vi valgte derfor at sætte en PC ind som WiiMote-modtager, og lade PC en videresende status fra WiiMoten til PDA, som så skulle fortolke inputs til styring af robotten. WiiMote til PC til PDA Nu var den oprindelige idé gennemført, med et par ekstra aktører, men uden den påkrævede brug af Tahoe-II-kortet. Da vi tidligere havde brugt Tahoe-kortets accelerometer, kom vi frem til, at det ville komplementere WiiMoten godt som en alternativ styringsmekanisme til robotten, nemlig intuitiv bevægelsesteknologi ved brug af accelerometre. Den bedste løsning i forhold til opgaven, og faget generelt, ville så være at forbinde Tahoe-II-kortet til PDA via SPP. Men igen viste der sig at være problemer med bluetooth-kommunikationen, da PDA en allokerede samme COM port til både NXT en og serieladapteren. Side 4 af 15

5 Vi måtte derfor igen indsætte PC en til dataopsamling, og herfra videresende de opsamlede data til PDA en, der igen fortolker data til robotstyring. Denne opsætning endte med at blive vores løsning, en løsning der inkluderer alle de dele vi ønskede, dog med nogle uønskede, men nødvendige, bindeled. Side 5 af 15

6 WiiMote kommunikation I det følgende beskrives der hvordan WiiMotens signaler bliver opsamlet af en PC og sendt videre til en PDA, der på baggrund af disse input styrer robotten. Dette delsystem er illustreret nedenfor. WiiMoten kommunikerer over bluetooth og skal paires med PC en som et HID device. De opretter dog ikke automatisk forbindelse som et almindeligt HID device ville gøre, hvilket derfor skal gøres hver gang programmet skal køre. For at kunne få fat i WiiMotens accelerometerværdier og knaptrykstatus har vi benyttet os af et tredjeparts.net bibliotek, skrevet i C#, kaldet WiiMoteLib. Dette bibliotek modellerer en WiiMote og den tilhørende Nunchuck. Når man opretter en WiiMote i koden, har denne en WiiMoteState der indeholder alle aktuelle informationer om den givne WiiMote. Man kan bl.a. se de aktuelle accelerometerværdier, knaptryk, batteriniveau, LED status og meget mere. Der kan ydermere abonneres på ændringer i et WiiMote-changed event. Da PC en i løbet af projektet er blevet sat ind som et slags mellemled, har vi prøvet at abstrahere fra, at den er en del af vores system, ved blot at lade den modtage værdierne fra WiiMoten, samle disse i et byte array og sende dem videre til PDA en. Vi havde til at begynde med valgt at sende to forskellige typer af byte arrays, et indeholdende knaptrykstatus og et indeholdende accelerometerværdier, men blev enige om, for at abstrahere mest muligt fra PC ens tilstedeværelse i systemet, kun at sende ét byte array indeholdende alle værdier, der er opbygget på følgende måde: Pakkelængde X-Værdi Y-Værdi Z-Værdi Knapværdier Byte 0-1 Byte 2-5 Byte 6-9 Byte Byte Applikationen på PC en abonnerer på et WiiMoteChanged event, der bliver affyret hver gang der er en ændring i WiiMotens eller Nunchuckens state, hvilket i realiteten er stort set hele tiden. Vi havde som udgangspunkt designet programmet på den måde, at der når eventet indtræf, blev oprettet et byte array Side 6 af 15

7 og dette blev så fyldt op med værdier fra WiiMoten og Nunchucken og sendt af sted via bluetooth til PDA en. Problemet med denne løsning er dog at WiiMoteChanged eventet indtræffer så ofte, at koden i eventhandleren kan risikere ikke at blive eksekveret, inden det næste event indtræffer. Vi ændrede derfor på vores design, og lavede i stedet en tråd, der hele tiden står og fylder et byte array med de aktuelle og relevante WiiMote og Nunchuck værdier og herefter sender dem til PDA en, uanset om der er ændringer i WiiMotens eller Nunchuckens state. Applikationen på PDA en har via bluetooth en virtuel COM port, hvorigennem den er forbundet til PC en. Denne COM port har et DataReceived event, som vi abonnerer på. Hver gang dette event indtræffer, indlæses data fra serielporten, og de enkelte positions- og knapværdier sættes. Der kører sideløbende en tråd på applikationen, der på baggrund af disse værdier styrer robotten. Side 7 af 15

8 Tahoe-II På Tahoe-II kortet gør vi brug af alle knapper, samt det indbyggede accelerometer. Accelerometeret er moduleret i klassen MMA7455, hvorfra det er muligt at læse X, Y og Z koordinater. Accelerometer værdierne ligger cirka mellem -80 og +80. De præcise værdier afhænger dog af hvilken akse der måles på. Tahoe-II kortet har 9 knapper der alle er aktivt lave. Vi har valgt at sende alt data fra Tahoe kortet til en PC via bluetooth. Tahoe kortet har dog ikke indbygget bluetooth, vi har derfor benyttet en bluetooth-adapter på kortets COM port 1. Når programmet kører på Tahoe-kortet åbnes der for mulighed for bluetooth-kommunikation via den tilkoblede bluetooth-adapter. Dette gør det muligt for andre bluetooth-enheder at detektere og oprette forbindelse til kortet. Derefter begynder kortet at læse accelerometer- og knapværdier. Disse målinger ligges i en pakke som vist nedenfor. Denne pakke sendes herefter ud på bluetooth-porten og kan læses af tilkoblede enheder. Alt data læses og sendes 20 gange i sekundet. Accelerometers Buttons (1-9) X Y Z Byte 0 Byte 1 Byte 3 Byte 4-12 PC PC en fungerer udelukkende som mellemled i det samlede system og gør intet andet end at oprette forbindelse, først til Tahoe kortets bluetooth adapter og dernæst til PDA en. Når forbindelserne er oprettet læser PC en løbende data udsendt fra Tahoe kortets bluetooth adapter og videresender det direkte til PDA en uden nogen form for databehandling. Vi har valgt at gøre det på denne måde for så vidt muligt at abstrahere fra at PC en overhovedet findes i det samlede system. Tahoe-II bluetooth PC bluetooth PDA Side 8 af 15

9 Lego Mindstorm NXT Kommunikation med Lego Mindstorm NXT Brick en foregår gennem bluetooth. Lego har stillet kommunikationsprotokollen til rådighed for udviklere, som ønsker at skrive tredjeparts kode til styring af robotten. For at kunne forbinde til NXT en skal den først parres med en computer, herefter kan der kommunikeres med robotten gennem en seriel port som tildeles af bluetooth stakken. Pakken som sendes til NXT en er defineret herunder Pakkelængde Kommando type Kommando byte Data Byte 0 Byte 1 Byte 2 Byte 3 Byte 4-N Kommandotypen bestemmer hvilken slags pakke der sendes og om der forventes et svar fra NXT en. I de tilfælde hvor man ønsker at påvirke robotten (eksempelvis ved at styre en motor) forventes der intet svar, imens en forespørgsel på sensor-værdier vil kræve et svar. Kommandobyten beskriver hvordan pakken skal behandles, om det er output-data til motorer eller en input-data-forespørgsel fra en sensor. Data indeholder det specifikke pakke-data, fx. hvilken motor der skal reguleres eller hvilken sensor der skal læses fra. I tilfælde af at det er en motor, der skal reguleres, er der en række parametre såsom hastighed, omdrejningstype og bremseeffekt, der sendes med. Svarpakken, som returneres fra NXT en i tilfælde af, at der forventes et svar, er defineret på følgende måde: Pakkelængde Type (0x02) Kommando byte Succes (0x00) / Fejl (0x??) Data Byte 0 Byte 1 Byte 2 Byte 3 Byte 4 Byte 5-N Byte 2 skal være 0x02 for at pakken indeholder et svar. Kommandobyten indeholder den type kommando som pakken indeholder svar på. Byte 4 indeholder information om hvorvidt kommandoen blev succesfuldt eksekveret eller i tilfælde af fejl, hvilken fejl der er opstået. Data kan, alt efter hvilken kommando der forventes svar på, lægges i en struct som indeholder al data for den givne kommando og herefter bruges. Side 9 af 15

10 NXT Wrapper Vi fandt et API, som havde wrappet protokollen i en række af structs og enums som lettede kommunikationen med NXT en. Her var de forskellige kommando-typer wrappet i en enum, alle kommandobytes var ligeledes wrappet i en enum og de forskellige svar-pakker var wrappet i structs. Dette API havde en helt masse funktionalitet som vi ikke havde behov for såsom programmering af kommandoer hvorfor vi valgte at anvende de nødvendige structs, enums og funktioner til at lave en modeleret NXTbrick, der kunne fungere på.net Compact Frameworket og indeholdte alle kommandoer der var nødvendige for at kunne kommunikere med robotten via bluetooth. Robot Wrapper De generelle robot-kommandoer blev isoleret i en Robotklasse, som er den modelerede udgave af vores specifikke robot med funktioner til bevægelse såsom MoveForward, StopMoving osv. Den har desuden en række sensorer hvor værdierne kan hentes direkte, fremfor at skulle sende et request til NXT en med port-navn for den ønskede sensor osv. Dette giver en abstraktion fra Bluetooth-kommunikationen, som simplificerer styring samt monitorering af robotten. Side 10 af 15

11 Windows Mobile Applikation på PDA For at kunne styre og monitorere robotten, har vi udviklet en applikation i Microsoft.NET Compact Frameworket som fungerer på en PDA med Windows Mobile. På PDA en er det muligt at oprette forbindelse til robotten, hvorefter alle robottens sensorer, motorer og batteristatus vises på skærmen. Når forbindelsen til robotten er etableret, kan der vælges mellem flere controllere, hvis inputs herefter vil fortolkes på og anvendes til styring af robotten. RobotConnector RobotConnector-klassen anvendes til at forbinde til robotten og instantiere Robot-objektet. RobotConnector er en component, som nedarver fra Panel, hvilket betyder, at den kan inkluderes i GUI en som en Control. Når en robot er valgt i connectoren og forbindelsen er oprettet, affyres et event som indeholder den instantierede Robot. RobotMonitoringUnit RobotMonitoringUnit instantieres med en reference til et Robot-objekt. Denne klasse er også et panel som kan inkluderes i et vindue. Ved instantiering startes en tråd, som løbende henter værdier fra alle sensorer samt status for motorer og batteriet. Disse værdier vises i en række progressbars i panelet. Side 11 af 15

12 ControllerSelector ControllerSelector anvendes til at finde hvilken controller man vil anvende til styring af robotten. Vi har implementeret to styringsenheder WiiMote og Tahoe-II kort. Når en controller er valgt fra en dropdownmenu, affyres et event indeholdende hvilken type controller der er valgt. public enum Controllers { tahoe, WiiMote }; WiiMoteController WiiMoteController instantieres med en reference til et Robot-objekt. Der lyttes på den port som er koblet til indgående trafik fra bluetooth-enheden og alle inputs fra WiiMoten gemmes. En tråd anvender de gemte værdier fra WiiMoten og styrer robotten ud fra disse 20 gange pr sekund. Vi har valgt at lade værdien på y- aksen fra WiiMotens accelerometer alene bestemme de to motorers påvirkning. Dvs. at jo mere man drejer WiiMoten til venstre, jo lavere hastighed kører venstre hjul med og omvendt. Dette valg skyldes at y- værdien ved udgangsposition giver 0, imens den giver [-1;0[ og ]0;1] ved tiltning mod henholdsvis højre og venstre. Vi har desuden valgt at lade de to knapper 1 og 2 bestemme om der skal køres, bakkes, eller bremses. if (y < 0) { robot.leftwheelpower = 100; robot.rightwheelpower = (sbyte)(100 - (100 * Math.Abs(y))); } else { robot.rightwheelpower = 100; robot.leftwheelpower = (sbyte)(100 - (100 * Math.Abs(y))); } TahoeController TahoeController fungerer på samme måde som WiiMoteController. Dog har vi, grundet at accelerometret på Tahoe-II kortet giver værdier mellem -65 og +65, måttet gange en værdi på x-aksen for at lade motorerne køre ved fuld hastighed (100). Igen har vi valgt 2 knapper til styring af frem/tilbage/brems. Alternativ styring Vi havde i starten snakket om muligheden for at kunne styre robotten ved at anvende WiiMote med Nunchuck som tank-joystick. Altså hvor man holder WiiMote og Nunchuck som to joysticks og lader robotten kører fremad når begge joystick skubbes frem, baglæns når begge trækkes tilbage og dreje når Side 12 af 15

13 WiiMote og Nunchuck trækkes i hver sin retning. Implementering af dette vil være simplelt på den måde vi har implementeret styring, da det blot kræver endnu en controller, som bare håndterer WiiMotens værdier anderledes. Side 13 af 15

14 Resultater Det er i vores projekt lykkedes os at styre en lego mindstorm robot med en WiiMote eller med et Tahoe-II kort. Det er dog ikke sket på den måde som vi i projektopstarten havde tiltænkt. Vi har været nødt til at tilføje en PC til vores system og lade den stå for hovedparten af kommunikation mellem de enkelte systemkomponenter. Ting der er lykkedes: Bluetooth forbindelse mellem WiiMote og PC Bluetooth forbindelse mellem PC og PDA Bluetooth forbindelse mellem PDA og Robot Bluetooth forbindelse mellem PC og Tahoe II kort Styring af robot vha. WiiMote Styring af robot vha. Tahoe-II-kortets accelerometer Ting der er mislykkedes: Bluetooth forbindelse mellem WiiMote og Tahoe II kort Bluetooth forbindelse mellem WiiMote og PDA Bluetooth forbindelse mellem Tahoe II kort og robot Bluetooth forbindelse mellem Tahoe II kort og PDA Side 14 af 15

15 Konklusion Vi har i projektforløbet erfaret at opgaven i dens oprindelige formulering ikke kunne løses med vores hardwarekonfiguration. På trods af flere show-stoppers har vi hele tiden holdt fokus på opgavens kerne styring af en robot med en WiiMote! Det har betydet at vi i flere omgange har måttet udskifte delkomponenter i systemet, herunder tilføjet en PDA, der i sidste ende har nedtonet Tahoe-II-kortets centrale rolle i det endelige produkt. Opgaven har givet os et større indblik i en bluetooth-verden, med alle de problemer en sådan medfører. Med forskellige bluetooth-stakke, understøttelse af forskellige interfaces og ustabilitet, med eventuel forsinkelse, ved brug af mange samtidige bluetooth-forbindelser. Altså er bluetooth ikke bare bluetooth hvis det er på embedded devices, PDA er og lignende. Side 15 af 15