Kørende robot med LEGO og Arduino

Navn på robotten : Sofus    

Af Lærke Work Bøg og Ellen Saaby Lorenzen

Download (PDF, 9.72MB)

Hardware og legokontruktion

Hardwaren kan ses på figur 1. Der er brugt to forskellige sensortyper for at få bilen til at gennemføre banen. I starten af banen bruges to IR-sensorer. De virker ved at udsende infrarødt lys, som så reflekteres på den overflade de kommer i berøring med, og sendes tilbage til sensoren igen. Da lyse farver reflekterer mere lys tilbage end mørke farver, kan den bruges til, at se forskel på mørke og lyse farver. På figuren ses det, at de er forbundet til ground, Vcc og analog porte, da den sensor analog signaler tilbage om farven. Ved sort sender den meget høje værdier og ved hvid lavere værdier.

Figur 1

Senere på banen bruges en sonar sensor til at registrere afstand. Denne virker ved, at udsende ultralyd og registrerer herefter hvor lang tid der går, før lyden kommer tilbage, hvilket så kan omregnes til en afstand til den flade hvor lyden blev reflekteret. Denne sensor kræver ligeledes forsyning i form af tilslutning til ground og forsyningsspændingen. Desuden kræver den digitale tilslutninger til en trigger, som sender et lydsignal ud og til et echo som registrerer hvornår lyden kommer tilbage og sender information om dette som et digitalt signal tilbage.

 

Ud over de to sensorer er der brugt to DC motorer som hver især er tilsluttet på en motorplads på arduino motorshieldet. Når de er tilsluttet hertil, kan de styres ved at skrive til de porte, som ses på figur 1.

Der er undervejs oplevet en del problemer med forskelligt hardware der ikke fungerede, og derfor skulle udskiftes. Desuden løsner ledningerne sig efterhånden, og det er derfor nødvendigt jævnligt at tjekke at alle ledningerne sidder ordentligt fast.

 

Legokonstruktionen som selve bilen består af, kan ses på figur 2 til 4. En brugervejledning til at bygge bilen kan desuden ses i den vedlagte pdf. Der er dog en del af bilen, nemlig motoren, som ikke fandtes i programmet, og den er derfor konstrueret af nogle klodser, så de har samme størrelse som motoren. De klodser der er tale om, kan ses på figur 3, hvor de er markeret med en rød pil.

Figur 2

 

Figur 3

 

Figur 4

Under konstruktionen af bilen, blev der lagt vægt på en række forskellige ting. En af tingene var, at bilen skulle være forholdsvis smal, for lettere at kunne gennemføre banen, uden at røre ved kanterne. Dette er der blandt andet taget højde for, ved at placere arduino, og bredboard ovenpå hinanden, så det tager mindst muligt plads, og batterierne bagved, så de ikke fylder ud til siderne. Hvor det ligger henne, kan ses på billedet af bilen på figur 5.

Figur 5

Ved begge motorer, er der monteret gearing der nedsætter hastigheden af bilen. Dette er gjort fordi motorerne har det bedst med at køre hurtigt, så hvis bilen ikke skulle køre alt for stærkt, var det nødvendigt at konvertere noget af hastigheden til kraft.   

Bilen er lavet, så den kun har to hjul. Bagpå har den så en klods den støtter på. Der blev først forsøgt at lave et slæbehjul bagpå i stedet, som kunne rotere omkring sig selv. Det viste sig dog, at bilens bevægelser blev mindre forudsigelige med et roterbart hjul bagpå, da retningen afhang meget af, hvordan dette hjul vendte. Derfor blev det vurderet at det var bedre at det bare var en klods den slæbte efter sig, hvilket fungerer fint på den plane bane der blev kørt på.

Der er desuden tænkt meget over legokonstruktionerne omkring sensorerne. Ved dem alle er der brugt klodser med huller i, til at holde styr på deres ledninger. Det er desuden vigtigt for kørslen at både IR- og sonar-sensorerne er placeret i bilens forende, så de hurtigt kan registrere ændringer. For sonarsensoren er det vigtigt at den sidder så den peger nogenlunde vandret ud mod højre, hvilket blev ordnet med en lille klods foran sensoren og en mur bagved den. I forhold til IR-sensorerne er det vigtigt at de er placeret til at pege lodret nedad. Da sensorerne har et lille hul, er det udnyttet til at holde dem fast med en klods der kan gå igennem hullet og holde dem der (den klods der er på bilen er lidt anderledes end dem i LEGO digital designer, men hvis de kan gå igennem sensorens hul, vil de også kunne bruges). Rundt om IR-sensorerne er der bygget skyggemure, der går så tæt på jorden som muligt. Dette er gjort fordi sensorerne er meget lysfølsomme, og murerne sørger for, at så meget lys som muligt bliver holdt ude, så lysintensiteten udefra ikke ændres for meget.

Det største problem omkring legokonstruktionen er, at den er lidt ustabil. Især den lille klods der bruges i stedet for et slæbehjul falder jævnligt af, hvis den ikke nyligt er blevet presset på plads.

 

Hvordan kører robotten

Programmet er opdelt i 4 stadier.

I stadie 0 følges den sorte streg ved, at holde de to IR sensorer på hver sin side af den sorte streg. Det gøres ved at aflæse IR værdierne. For hver IR sensor er sat en skil-værdi mellem det den måler når sensoren måler hvid og det den måler når den står på sort. Når begge sensorers værdier er under skil værdien opfattes det som om at de står på hver sin side af den sorte streg. i det tilfælde køre roborten liggeud. Hvis en sensorværdi kommer under skil-værdien fortæller det at den står over sort og robotten drejer til den modsatte side for derved at holde stregen lige under sig. Hvis begge sensorer er over skil-værdierne ses det som om robotten er ved den brede streg for enden af stregen. Når det sker skiftes der til næste stadie.   

 

Stadie 1 skal have robotten til at dreje sig så sonarsensoren peger mod muren. Dette gøres ved at køre i en forprogrammeret bue. Der måles altså ikke på nogle sensorer men motoren køre med en bestemt hastighed i et fastlagt stykke tid.  Herefter skiftes til stadie 2.

 

I stadie 2 skal robotten følge muren. Hvert gennemløb starter med at aflæse sonarsensorens værdi. Derefter tjekker programmet om værdien er over eller under 8, altså om den skal dreje til højre eller venstre. I begge tilfælde bruges en formel til at udregne motorens PVM ud fra sensor værdien. Motoren kan højst køre med 255 PVM og går i stå ved under 15 PVM. Hvis værdier er over eller under det når de er udregnet, sættes PVM til henholdsvis 255 eller 15.

Når sensoren måler over 60 betyder det at den er kommet forbi muren. Hvis sensoren måler over sin max kapacitet på 3 m sender den en værdi på 0. Derfor opfattes en afstand på 0 også som om robotten er kommet forbi muren. Når robotten er forbi muren skiftes der til stadie 3. Sensoren giver af og til forkerte data. Derfor er det nødvendigt med en testværdi. Det gøres ved at robotten kun går til stadie 3 hvis den to gange i træk har målt værdier over 60 eller på 0.

 

I stadie 3 skal robotten bevæge sig rundt om muren og stille sig, så sonarsensoren kan bruges til at følge muren på den anden side. Først laves der et forprogrammeret drej. Derefter køre roborten ligeud til den ser siden af muren og kører herefter ligeud i et kort fastlagt tidsrum. Derefter vender koden tilbage og gentager drejet. Programmet køre nu ligeud i 3 gange den tid det kørte ligeud i sidst. Derefter skiftes tilbage til stadie 2 hvor robotten følger muren.  

Programmets opbygning kan ses i de flowcharts der ligger som figur 6 til 9. For en mere detaljeret beskrivelse se kommentarerne i koden.  

  #include <NewPing.h>

 // Sensor 
  int knappind=4; 
  int genskindVenstreP=3;
  int genskindHojreP=2;
  int trig = 6;
  int echo = 5;
  int max_dist = 3000;
  NewPing sonar(trig, echo, max_dist);

// motor porte 
  const int retningPinV= 13;     // pinden der styre retningen A2
  const int bremsPinV = 8;      //pind der aktivere bremsen
  const int PWMV = 11 ;           // pind der bruges til at indstille PVM  

  const int retningPinH= 12;     // pinden der styre retningen 
  const int bremsPinH= 9 ;      //pind der aktivere bremsen
  const int PWMH = 3 ;           // pind der bruges til at indstille PVM  

//Glabale variable 
  int knapveardi;
  int stadie = 0;
  int hastighedH = 61;
  int hastighedV = 50;
  
// stadie 0
  int venstreVaerdi;
  int hojreVaerdi;
  int SkilV=818;
  int SkilH=829;


// Stadie 1   
  unsigned long tidstart = 0;
  unsigned long tid = 0;
  int intervalFrem = 1400;
 
// Stadie 2
  int dist;
  double PVMBH;
  double PVMBV;

// Stadie 3 
  int intervaldrej = 1050;
  int intervallige = 200;
  int tjek_dist;
  int drejnr=0;


void setup() {
  // put your setup code here, to run once:
  Serial.begin(9600);
  pinMode(retningPinV, OUTPUT);   // sæt retningspind til udgangs pind 
  pinMode(bremsPinV, OUTPUT);     //sæt brems pind til udgangs pind 
  pinMode(retningPinH, OUTPUT);   // sæt retningspind til udgangs pind 
  pinMode(bremsPinH, OUTPUT);     //sæt brems pind til udgangs pind 
  pinMode(trig, OUTPUT);
  pinMode(echo, INPUT);
}
  

void loop() {
  knapveardi=digitalRead(knappind);             // aflæser knappesværdi 
  
  if( knapveardi && stadie==0 ){                //Tjekker om knappen er slået til og Robotten befinder sig i stadie 0 
    venstreVaerdi=analogRead(genskindVenstreP); // Aflæser højre IR sensor  
    hojreVaerdi=analogRead(genskindHojreP);     // Aflæser venster IR sensor 


     if (venstreVaerdi< SkilV  && hojreVaerdi <SkilH){    // tjekker om begge sensore står over hvidt. hvis ja køre robotten frem af
        
         digitalWrite(retningPinH, LOW); //Højre motor kør  
         digitalWrite(bremsPinH, LOW);   //slå bremsen fra 
         analogWrite(PWMH, hastighedH);   //sæt hastighed tid den valgte hastighed 

         digitalWrite(retningPinV, HIGH); //Venster motor kør  
         digitalWrite(bremsPinV, LOW);   //slå bremsen fra 
         analogWrite(PWMV, hastighedV);   //sæt hastighed tid den valgte hastighed 

         
         
      }
      else if (venstreVaerdi> SkilV  && hojreVaerdi <SkilH){  // tjekker om venstre IR står over den sorte streg. hvis ja køre robotten til Højre 
        
         digitalWrite(retningPinH, LOW); //Højre motor kør  
         digitalWrite(bremsPinH, LOW);   //slå bremsen fra 
         analogWrite(PWMH, hastighedH*1.3);   //sæt hastighed tid den valgte hastighed 

         digitalWrite(retningPinV, LOW); //Venster motor kør  
         digitalWrite(bremsPinV, LOW);   //slå bremsen fra 
         analogWrite(PWMV, (hastighedV/4) );   //sæt hastighed tid den valgte hastighed 
         
      
     }
     else if (venstreVaerdi< SkilV  &&hojreVaerdi >SkilH){  // tjekker om Højre IR står over den sorte streg. hvis ja køre robotten til venstre  
        
         digitalWrite(retningPinH, HIGH); //Højre motor kør  
         digitalWrite(bremsPinH, LOW);   //slå bremsen fra 
         analogWrite(PWMH,(hastighedH/4) );   //sæt hastighed tid den valgte hastighed 

         digitalWrite(retningPinV, HIGH); //Venster motor kør  
         digitalWrite(bremsPinV, LOW);   //slå bremsen fra 
         analogWrite(PWMV, hastighedV*1.3);   //sæt hastighed tid den valgte hastighed 
     } 
     else if (venstreVaerdi> SkilV+20  &&hojreVaerdi >SkilH+20){    // tjekker om begge IR måler sort.
        
         stadie = 1;                      // Skift til stadie 1 
     }
  }
  else if(knapveardi && stadie == 1){     // Hvis Robotten ikke er i stadie 0, Tjek om knappen er slået til og om den er i stadie 1. 
    tid = millis();                       // Aflæs systemtid 
    
    if (tidstart==0){                     // hvis variablen tidstart er 0 
     tidstart = tid;                      // Sæt tidstart til tid 
    }
    if ((tid-tidstart)<intervalFrem){     // test om robotten har kørt frem ad i mindre tid end intervalFrem. Hvis ikke kør frem ad.
     digitalWrite(retningPinH, LOW); //Højre motor kør  
     digitalWrite(bremsPinH, LOW);   //slå bremsen fra 
     analogWrite(PWMH, (hastighedH*3));   //sæt hastighed tid den valgte hastighed 

     digitalWrite(retningPinV, HIGH); //Venstre motor kør  
     digitalWrite(bremsPinV, LOW);   //slå bremsen fra 
     analogWrite(PWMV, (hastighedV/3));   //sæt hastighed tid den valgte hastighed        
    }
    else{                             //Hvis robotten har kørt frem ad i længere tid end intervalFrem.
      stadie = 2;                     // skift til stadie 2 
    }
  }
  else if (knapveardi && stadie == 2 || stadie == 3){     //Hvis Robotten ikke er i stadie 0 eller 1, Tjek om knappen er slået til og om den er i stadie 2 eller 3.
    tjek_dist = dist;                                     // Sæt forgående afstands måling til tjek_dist 
    dist = sonar.ping_cm();                               // Aflæs sonar sensor 

  if (stadie==2) {                                       // Hvis i stadie 2 
     tidstart=0;                                          // nulstil starttid 
  
   if (dist > 3 && dist <= 8 ){                            // Hvis Robotten er tæt på muren 
     PVMBH=((-18.3) * dist + 207.4 );                     // Udregn Højre PVM i forhold til sonar værdi 
     PVMBV=(8* dist - 14);                                // Udregn Venstre PVM i forhold til sonar værdi 
                                                          // kør til venstre
     digitalWrite(retningPinH, LOW); //Højre motor kør    
     digitalWrite(bremsPinH, LOW);   //slå bremsen fra 
     analogWrite(PWMH, PVMBH );   //sæt hastighed tid den beregnede hastighed 

     digitalWrite(retningPinV, HIGH); //Venster motor kør  
     digitalWrite(bremsPinV, LOW);   //slå bremsen fra 
     analogWrite(PWMV, PVMBV );   //sæt hastighed tid den beregnede hastighed 
      }
     if (dist >8 || dist == 0){                                // Test om robotten er lidt  langt fra muren. Hvis sensorens max værdi overskrides udskriver den 0. derfor betyder 0 også at den er langt fra muren. 
        if (dist == 0 || dist > 60 && tjek_dist == 0 || tjek_dist > 60){  // test om robotten er forbi muren. tjek-dist for at sikre at der ikke er tale om en fejl måling. 
          stadie=3;                                               // skift til stadie 3 
           }
         PVMBV=(10 * dist - 30);                                   // Udregn Venstre PVM i forhold til sonar værdi                                 
        if (PVMBV > 255){                                         // Sikre at moterens max PVM ikke overskrides
          PVMBV=255;                                    
      }
      PVMBH=((-9.76)* dist + 139.08);                           // Udregn Højre PVM i forhold til sonar værdi 
      if (PVMBH < 15){                                          // Sikre at moterens min PVM ikke overskrides
        PVMBH=15;
      }                                                       // kør til højre 
      digitalWrite(retningPinH, LOW); //Højre motor kør  
      digitalWrite(bremsPinH, LOW);   //slå bremsen fra 
      analogWrite(PWMH,PVMBH );   //sæt hastighed tid den udregnede hastighed 

      digitalWrite(retningPinV, HIGH); //Venster motor kør  
      digitalWrite(bremsPinV, LOW);   //slå bremsen fra 
      analogWrite(PWMV,PVMBV  );   //sæt hastighed tid den udregnede hastighed
  }
  
} 
 else if (stadie == 3){                                         // hvis robotten ikke er i stadie 2, er den så i stadie 3 
   tid = millis();                                              // aflæs system tid 
    if (tidstart==0){                                           // hvis system tid er 0 
     tidstart = tid;                                            // sæt start tid til nuværende tid                                          
    }
    if ((tid-tidstart)<intervaldrej && drejnr < 2){             // hvis interval tid ikke overskredet og der ikke er drejet over 2 gange så drej til højre
     digitalWrite(retningPinH, LOW); //Højre motor kør          
     digitalWrite(bremsPinH, LOW);   //slå bremsen fra 
     analogWrite(PWMH, 0);   //sæt hastighed tid den valgte hastighed 

     digitalWrite(retningPinV, HIGH); //Venster motor kør  
     digitalWrite(bremsPinV, LOW);   //slå bremsen fra 
     analogWrite(PWMV, 255);   //sæt hastighed tid den valgte hastighed
    }                                                              
    else if (dist > 30 || dist == 0){                           //når interval tiden er gået og der ikke er drejet over 2 gange, tjek om afstanden er lang, hvis den er det så kør lige ud
     digitalWrite(retningPinH, LOW); //Højre motor kør  
     digitalWrite(bremsPinH, LOW);   //slå bremsen fra 
     analogWrite(PWMH, hastighedH);   //sæt hastighed tid den valgte hastighed 

     digitalWrite(retningPinV, HIGH); //Venster motor kør  
     digitalWrite(bremsPinV, LOW);   //slå bremsen fra 
     analogWrite(PWMV, hastighedV);   //sæt hastighed tid den valgte hastighed
    }
    else {                                                       // Hvis afstanden ikke er lang er. Det vil sige sensoren registere siden af muren 
     if (drejnr==1){                                             // Hvis der er drejet en gang 
         tidstart = tid;                                         // Sæt start tid til nuværende tid 
         while ((tid-tidstart)<intervallige*3){                  // Så længe 3 gange tidsintervalet ikke er over skredet køre lige ud 
            tid = millis();                                      // mål system tid 
            digitalWrite(retningPinH, LOW); //Højre motor kør  
            digitalWrite(bremsPinH, LOW);   //slå bremsen fra 
            analogWrite(PWMH, hastighedH);   //sæt hastighed tid den valgte hastighed 

            digitalWrite(retningPinV, HIGH); //Venster motor kør  
            digitalWrite(bremsPinV, LOW);   //slå bremsen fra 
            analogWrite(PWMV, hastighedV);   //sæt hastighed tid den valgte hastighed
        }
         drejnr=2;                                              // Der er nu drejet 2 gange 
         tidstart=0;                                            // nulstil start tid 
     }
        if (drejnr==0){                                          // hvis der ikke er drejet endnu 
          tidstart = tid;                                        // start tid til nuværende tid 
          while ((tid-tidstart)<intervallige){                   // Så længe interval tid ikke er overskredet, kør lige ud  
            tid = millis();                                      // Sæt tid til systemtid 
            digitalWrite(retningPinH, LOW); //Højre motor kør  
            digitalWrite(bremsPinH, LOW);   //slå bremsen fra 
            analogWrite(PWMH, hastighedH);   //sæt hastighed tid den valgte hastighed 

            digitalWrite(retningPinV, HIGH); //Venster motor kør  
            digitalWrite(bremsPinV, LOW);   //slå bremsen fra 
            analogWrite(PWMV, hastighedV);   //sæt hastighed tid den valgte hastighed
          }
          drejnr=1;                                               // register at der er drejet 1 gang 
          tidstart=0;                                             // nulstil start tid 
        }
        if (drejnr>1) {                                            //Hvis der er drejet mere end en gang 
          stadie = 2;                                             // Sæt stadie til 2 
        }
    }
}
   
  } 
  
  else {                                                          // hvis knappen ikke er slået til brems 
 
   digitalWrite(bremsPinH, HIGH);   //slå bremsen fra 
   analogWrite(PWMH, 0);   //sæt hastighed tid den valgte hastighed 
  
  digitalWrite(bremsPinV, HIGH);   //slå bremsen fra 
   analogWrite(PWMV, 0);   //sæt hastighed tid den valgte hastighed 
   
  }

}

 

 

Figur 6
Figur 7

 

Figur 8

 

Figur 9

Dynamisk styring

Når robotten skal køre langs muren bliver den styret dynamisk. Det vil sige at jo tættere den er på muren jo skarpere drejer den væk fra muren. Dette gøres ved at motorens PVM beregnes ud fra en formel hvor sonar sensorens værdi indgår. Denne formel er fundet ved at opsætte punkter bestående af afstanden fra muren og den hastighed der er ønsket ved den afstand. Der er så fundet en lineær funktion gennem de punkter.

 

En lignende metode er forsøgt med IR sensoren, men da der ikke er en blød overgang mellem sort og hvid virkede det ikke.

 

Kalibrering

Batteriets opladningsniveau har stor betydning for hvor skarpt motoren drejer og hvor følsomme sensorer er.

Derfor er det nødvendigt at tilpasse hvor lang tid der drejes og IR-sensorernes grænseværdier.

 

Konklusion(Lærke)

Robotten gennemfører banen ca. to ud af tre gange, hvis den nyligt er blevet kalibreret. Da batterierne hurtigt taber strøm, kræver det dog, at der jævnligt foretages en ny kalibrering, hvilket ikke er optimalt, og når batteriniveauet falder under et vis niveau, virker sonar-sensoreren slet ikke.

Når bilen kører, gør den det dog i en fornuftig hastighed, og den klarer sig fint uden om alle udfordringer.

På konkurrencedagen virkede robotten desværre ikke, og der var ikke tid nok til at finjustere værdier til at den kunne nå at fungere inden for de tre runder. Derfor er der ikke nogen konkurrencetid.

 

Perspektivering (Ellen

Robotten kunne forberedes ved at lave en mere stabil legoopbygning, hvor ledningerne blev fastholdt. Dette ville mindske risikoen for at en ledning løsnede sig.   

 

En anden forbedringsmulighed kunne være at sætte sonarsensoren på en bevægelig del der kunne følge muren og hele tiden dreje sig så den så den korteste afstand til muren. på den måde kunne robottens drejninger bliver styret af sensoren i stedet for at være forprogrammeret.

 

Programmet kunne opsættes sådan, at når stadie-værdien skiftes hoppes der direkte til det pågældende stadie i stedet for at køre videre i programmet. Dette ville giv robotten en smule hurtigere reaktionstid, men ville nok ikke gøre den helt store forskel.   

Koden ville være blevet overskuelig hvis nogle af de mest brugte handlinger blev lavet om til funktioner. Det kunne f.eks. være en funktion til at køre ligeud, der er havde hastighed som input.

 

Når programmet når ned til stadie 2 hvor den skal følge muren, ville det formentligt have været en fordel, hvis den var blevet sat til at dreje skarpere. Både når den er langt fra muren og når den er tæt på.

 

Det kunne være en stor forbedring hvis robotten kunne kalibrere sig selv når den startede op. Så sensor-værdierne blev tilpasset efter lysmængde i lokalet og batteriernes opladningsniveau.

Leave a Reply