Projekt - 5

Dato:
17-01-2010

Antal timer brugt:
10 timer

Deltagende personer:
Kim Munk Petersen, Andreas Koefoed-Hansen og Tim Rasmussen

Målet:

1. Sørge for at motorene slukker når den vælter.
2. Rette forskellige fejl og optimere koden.
3. Tilføje connect- og disconnet-knap til vores Bluetoothcontroller på computeren.
   
4. Tilføje understøttelse på Legwayen af flere samtidige tastetryk.
5. Få robotten til at undgå objekter vha. en ultralydssensor.
   

Planen:
Vi starter med at få Legwayen til at slukke motorene når den ikke længere har mulighed for at rette op igen. Dette kunne f.eks. være når Legwayen er ned uden 45 grader, dette er selvfølgelig noget der skal afprøves for at finde den rigtige vinkel, således den ikke slukker motorene under kørsel med Legwayen. Der er forskellige fejl kode der skal rettes. F.eks. at newCommand bliver sat i BTCommandReader, men først bliver brugt i BTDriver. Dette betyder at BTCommandReader pga. et delay kan nå at opdatere variablen igen og derved mistes en kommando. Dette skal rettes til at BTDriver der holder styr på at der er modtaget en ny kommando. Ligeledes er der forskellige optimeringen der kan laves på koden. Når dette problem er løst, er planen at tilføje en ekstra behavior til Legwayen, således den bakker væk fra en objekt hvis den kommer for tæt på. Denne behavior har højre prioritet end vores BTDriver, og vi har derfor det simple behavior-hierarki  vist herunder.

 


Processen:
En af de ting vi har irriteret os over i forbindelse med at teste Legwayen, er at motorerne går helt amok når den vælter. Grunden til at motorerne får så meget power, skyldes at gyroskopet registrerer at Legwayen er væltet eller tilter meget og derfor forsøger PID controlleren at rette op den op igen. Hvis den er så langt nede at den ikke kan rettes op igen, så er det bedre at slukke for motorerne i stedet for at forsøge at rette op. Det store spørgsmål er nu bare, hvornår Legwayen ikke længere skal forsøge at rette sig selv op?

Vi forsøgte at løse dette problem ved at gøre brug af den vinkel gyroskopet måler vha. gyro.getAngle(). Det første forsøg gik ud på at hvis Legwayen havde tiltet mere en 70 grader skulle motorene slukkes. Dette gjorde vi ved følgende kode:

private static boolean testUpright(double Psi) {
    if (Math.abs(Psi) < 70)
        return true;
    motors.stop();
    ...
}

Så længe den numeriske værdi af vinklen er under de 70 grader, så fortsætter PID controlleren med at styre motorenes hastighed, men med det sammen vinklen kommer op over det 70, slukkes motorene. Hældningen på de 70 viste sig at være for højt, eftersom motorerne ikke altid blev slukket når Legwayen væltede. Derfor ændrede vi værdien til 60, men her havde vi stadig tilfælde hvor motorene ikke slukkede når den væltede. Vi ændrede derfor værdien til 50, hvilket heller ikke var en god værdi. Problemet var nu ikke at motorene ikke slukkede, men i stedet at de slukkede for hurtigt. Selv om Legwayen kørte som den skulle, så kunne gyroskopet pludseligt returnere en værdi højere end 50 og derved slukkede motorene. En måde vi kunne løse dette på var ved at lave et filter, hvorved værdier der lå langt fra tidligere værdier bliver sorteret fra. På den måde vil enkelte høje værdier være blevet sorteret fra og vi have undgået at den væltede. Vi valgte i stedet at træffe beslutninger på baggrund af det output (power), der bliver beregnet  i PID controlleren vha. formelen:

pw = error * Kp + deriv_error * Kd + int_error * Ki;

Efter at have lavet en del tests fandt vi frem til at den optimale værdi var 8000. Så når outputtet når op over de 8000 slukkes motorerne. En bivirkning ved denne løsning er at robotten rent faktisk også slukker motorerne hvis man løfter den fra overfladen. Denne bivirkning mener vi imidlertid er positiv idet det for Legwayen er unødvendigt at forsøge at korrigere for en tiltning hvis den hjul ikke rører jorden. I forbindelse med at vi fik løst problemet med motorene, kiggede vi også på muligheden for at rejse Legwayen op igen uden at genstarte NXT'en. Dette krævede nogle ændringer på tilgangen til de variabler vi skulle ændre på, samt tilføjelser til nogle metoder. Men ud over ændringerne i koden, var det rimeligt simpelt at løse problemet. Vi tilføjede funktionen til vores testUpright() metode som ses her:

private static boolean testUpright(double power) {
    if (Math.abs(power) < 8000)
        return true;
  
    motors.stop();
    motors.reset();
    File medicFile = new File("medic.wav");
    if(medicFile.canRead())
        Sound.playSample(medicFile);
    while (!Button.ENTER.isPressed()) {
        delay(10);
    }
    int_error = 0;
    prev_error = 0;
    gyro.reset();
    ctrl.resetTiltAngle();
    return false;
}

Det vigtigste er at huske at nulstille de rigtige værdier. Dem vi fandt ud af at vi skulle nulstille var motorens TachoCounter, gyroskopets vinkel, PID controllerens forskellige error værdier og vores TiltAngle. Da disse værdier var nulstillet, kunne vi starte balanceringen igen ved at trykke på enter knappen på NXT'en.

Efter at have løst problemet med motorene, begyndte vi at rette fejl og optimere koden. En af de større fejl vi rettede var at det var BTCommandReader der afgjorde om der var modtaget en ny kommando, men pga. et delay mellem BTCommandReader og BTDriver, der begge kører på NXT'en, kunne der gå kommandoer tabt. Derfor ændrede vi det til at det var BTDriver selv der stod for at bestemme om der var forskel på den nye og den forrige kommando. Ved at rette dette opstod der så et nyt problem. Problemet lå i at vi nu ikke længere kunne holde en knap nede på tastaturer eller trykke flere gange hurtigt. Grunden til dette ligger i at hvis vi f.eks. trykkede forward, så kører Legwayen fremad i 100 millisekunder, hvorefter den venter på en ny kommando. Hvis den nye kommando er den samme som den tidligere så sker der ikke noget, men eftersom vi altid sender en stop kommando når en knap slippes så burde den være klar til at modtage en ny forward kommando, men pga. det delay der er kan denne kommando gå tabt. Problemet løste vi ved at lave det sådan at den kører frem af indtil den modtager en ny kommando. Dette viste sig at være en god løsning.

En anden af de vigtige ændringer vi har lavet er at tilføje en buttonListener[1] til main metoden, således ligegyldig hvornår man trykker på ESCAPE knappen, så lukkes programmet.

Button.ESCAPE.addButtonListener(new ButtonListener() {
    public void buttonPressed(Button b) {
        Sound.buzz();
        System.exit(0);
    }
    public void buttonReleased(Button b) { }
});

Inden vi lavede denne ændring have vi placeret flere while-loops i koden for at kontrollere om der bliver trykke på ESCAPE knappen. Ved at bruge en ButtonListener kan vi nøjes med en enkelt listener. Vi havde dog et lille problem med at få vores ButtonListener til at virker, men dette skyldes at vi havde placeret den nederst i main metoden og derved aldrig blev afviklet. Grunden til at koden ikke blev afviklet skyldes at når Arbitratioren startes, så blokerer den koder der kommer efter. Vi flyttede derfor registreringen af vores ButtonListener op i starten af main metoden, hvorefter problemet var løst.

Ud over at have rette disse fejl, har vi været i gang med at rydde op og optimere koden. Dette betyder at metoder vi ikke længere gør brug af er fjernet, mens andre metoder har fået lidt mere arbejde. Ligeledes har vi fået tilføjet en connect og disconnet knapper i vores bluetooth controller på computeren, således det nu er muligt undervejs at disconnecte forbindelsen til NXT og derefter igen bruge connet til at oprette den igen. Det har været et irritationsmoment for os at vi hele tiden skulle til at lukke og starte programmet for igen at oprette forbindelse. Et snapshot af det opdaterede program med de to knapper ses herunder.

De to knapper var ikke svære at tilføje, men de gav lidt problemer idet tryk derpå flytter fokus fra det tekstfelt hvortil en keyEvent-listener er påhæftet. Det betyder at tastetryk ikke fanges når man har trykket på en af knapperne, medmindre man husker at trykke på tekstfeltet igen. En hurtig løsning var også at tilføje keyEvent-listener'en til begge knapper. Det viste sig ikke at virke idet vi disabler connect-knappen når man har connected og disconnect-knappen når man har disconnected, idet det ikke giver mening at oprette forbindelse hvis den allerede er forbundet, eller afbryde en forbindelse der er afbrudt. Grunden til at det ikke virker er at en disabled knap ikke er i stand til at fange key-events. Vi løste problemet ved programmatisk at ændre fokus tilbage til tekstfeltet efter man har trykket på en knap.

Vi regnede med at Legwayen ville blive nemmere at styre hvis man fx kan holde pil op og pil til højre nede på én gang og derved få den til at køre fremad mens den drejer svagt til højre. Som beskrevet i forrige indlæg kan vores PC-program der sender kommandoer til Legwayen via Bluetooth signalere at flere taster holdes nede. Vi udvidede derfor BTDriver-klassen til også at understøtte dette ved at ændre tilt-vinklen på præcis samme måde som når robotten kører frem eller tilbage og så lave et offset på en af de to motorerer således at den bremses ift. den anden hvilket resulterer i at Legwayen drejer svagt. Det virkede som forventet og Legwayen blev noget nemmere at styre præcist med denne funktionalitet tilføjet.

Vi fik den idé at robotten også skulle kunne dytte når man på en Bluetooth-forbundet computer trykker på space. Der er ingen praktisk årsag hertil, men vi synes bare at det kunne være sjovt og det ville samtidig give os erfaring med Sound-klassen. Det krævede blot at vi tilføjede en key til keyEvent-listeneren i PC-programmet, lavede en værdi i bitmasken hertil og en fortolkning af den nye bitmaske i BTDriver-klassen. Vi havde ingen tidligere erfaring med at afspille lyd på NXT-brick'en, men det viste sig at være utrolig let. NXT'en kan afspille lyd som simple toner og som forskellige instrumenter, men kan også afspille wav-filer. Vi valgte at implementere lyden via en lydfil idet det giver den mest naturlige lyd og let kan skiftes. Det eneste man skal gøre for at afspille en wav-fil er at oprette et fil-objekt der peger på lyd-filen, kontrollere at filen kan læses, dvs. findes, og så sende den med som parameter til en metode i Sound-klassen som vist herunder:

File hornFile = new File("horn.wav");
if(hornFile.canRead())
    Sound.playSample(hornFile);

Det er i princippet ikke nødvendigt at kontrollere om filen findes, men idet man kan komme til at slette den eller der kan opstå en fejl i flash-hukommelsen, gør vi det alligevel for at få vores Legway så robust som muligt. Ligeledes kan en afbrudt Bluetooth-forbindelse som beskrevet ikke få Legwayen til at gå ned. Denne tankegang understøttes af  Pattie Maes [2] der postulerer at behavior-based agenter skal være robuste og kunne overleve selvom fejl opstår.

Vi prøvede med forskellige lydfiler og erfarede at man bliver nødt til at have lidt stilhed efter selve lyden i filen idet NXT'en af ukendte årsager ikke afspiller de sidste ca. 0.3 sekunder af lydfilen.

Efter at have fundet ud af hvor simpelt det var at arbjede med lyd på NXT'en, besluttede vi at tilføje lyde der indikerer når robotten er væltet og når en Bluetoothforbindelse til en computer oprettes og afbrydes. Når Legwayen vælter afspiller den et råb om hjælp, mere præcist "Medic". Når en Bluetoothforbindelse oprettes og lukkes, afspilles pre-definerede lydsekvenser vha. metoderne Sound.sequence() og Sound.sequendeUp().

En anden ny tilføjelse til Legwayen en ekstra behavior[3]. Denne behavior går ud på at hvis robotten kommer for tæt på et objekt, så bakker den væk og drejer ca. 180 grader hvorefter den igen er klar til at modtage kommandoer. Måde vi har lavet det på er at vi har oprettet en by behavior kaldet AvoidFront som indeholder de metoder en behavior skal have.

public AvoidFront(CtrlParam ctrl) {
    sonic = new UltrasonicSensor(SensorPort.S2);
    car = new Car(ctrl);
}

Behavioer'en gør brug af en ultralydsensor til at bestemme standen, og når afstanden er mindre end 25 cm tager den kontrol. Dette er som tidligere beskrevet Arbitratoren der sørger for at give den behavior med højest prioritet, retten til motorerne. I vores tilfælde har vi to forskellige behaviors: Styring via. Bluetooth og AvoidFront. Vi vælger at give AvoidFront den højeste prioritet, eftersom det ikke vil give meget mening at have en behavior der skal sørge for at Legwayen ikke kører ind et objekt og samtidigt kan blokeres af en anden behavior hvor man kan kører Legwayen ind i objektet. Dette giver vores prioritetsliste:

Behavior[] bArray = { b1, b2 };

hvor b1 er Bluetooth styring og b2 er AvoidFront. Action-metoden i AvoidFront, som er den der udføres når behavoiren har kontrol, starter med at stoppe robotten. Når Legwayen er stoppet, får den ordre på at køre baglæns i 500 millisekunder, hvorefter den igen stoppet. Når Legwayen igen er stoppet giver vi den en left kommando i 2 sekunder, hvilket sørger for at Legwayen roterer ca. 180 grader. Denne tid skal man nødt til at forsøge sig frem til, indtil man få et fornuftigt resultat. Når vi har roteret, kalder vi igen stop. Grunden til at vi bruger stop-kommandoen imellem hver bevægelseskommando, skyldes at vi dermed sikrer os at robotten står stille inden vi giver den en ny kommando. Dette giver en mere stabil kørsel.

For at indikere at den nye behavior AvoidFront træder i kraft, afspilles også en lyd her. Efter at have tilføjet denne lyd, opdagede vi at Legwayen var blevet mere interaktiv og langt mere livagtig end tidligere. Man føler næsten at den har sin egen personlighed når man hører den sige noget. Dette forstærkes helt sikkert også af dens fysiske konstruktion hvor ultralydsensoren ligner et hoved med to øjne.

Legway'en har omkring 160KB flash hukommelse tilgængelig når vi har formateret den. Med alle vores lyde og vores program uploaded, er der kun ca. 5KB ledig. Når vi forsøger at uploade en opdateret version af vores Legway-program, fejler den pga. pladsmangel, selvom der altså er plads. Løsningen hertil er at slette det oprindelige program inden det nye uploades.

Konklusion:
Vi har denne gang fået løst flere fejl der har været til irritation undervejs. Fejlene er blevet rettet og koden gjort mere overskuelig. I sidste blogindlæg beskrev vi hvordan vi ønskede at tilføje en connect og disconnet knap til vores Bluetooth controller på computeren. Dette er lykkes os dog med lidt besvær, eftersom fokus blev flyttet når man trykkede på knapperne. Problemet blev løst og programmet fungerer nu efter hensigten.

Endnu en tilføjelse til vores Legway er vores AvoidFront behavior, som sikrer at Legwayen ikke kører ind i objekter. Denne behavior var utroligt let at implementere når der blev gjort brug af behavior interfacet og voldte derfor ikke de store problemer. Koden virkede derfor også næsten efter hensigterne under første test, men nogle af de tider der bestemmer hvor meget der skal bakkes og drejes var en smule forkert og måtte justeres.

Vores mål herfra er at få ryddet mere op i koden og evt. få fjernet brugen af sinus hastigheder mht. robottens kørsel. Dette betyder at vi igen skal igennem processen med at finde frem til nogle PID controller værdier. Grunden til at vi godt kunne tænke os at fjerne sinus kurven fra hastigheden er at når man kører med Legwayen så accelererer den op hvorefter hastigheden dæmpes. Dette kunne være rart at man kunne holde en mere konstant fart, men dette kan komme til at påvirke Legwayens balanceringsevne. Ud over dette er målet at få gennemgået vores blogindlæg og rettet evt. fejl og mangler, forberede præsentationen samt skrevet den endelige konklusion.


Referencer:

1. leJOS Threads, Listeners and Events tutorial, http://lejos.sourceforge.net/nxt/nxj/tutorial/ListenersAndEvents/Listeners_Events.htm
2. P. Maes, Modeling Adaptive Autonomous Agents,
   Artificial Life Journal, C. Langton, ed., Vol. 1, No. 1 & 2, MIT Press, 1994.
3. Sequential and reactive control strategies, Chapter 5, pp.190-233 of Fred G. Martin, Robotic Explorations: A Hands-on Introduction to Engineering, Prentice Hall,2001.