Tutorial: Plæneklipper 5000 Linje-og vægfølgende lego robot bil

Robotnavn: Plæneklipper 5000

Gruppe: Anders Grøn og Christian Dam

Velkommen til denne tutorial, hvor du vil blive guidet step-by-step til, hvordan man bygger en seriøs linje- og mur-følgende, forhindrings-væltende, og græs-slående turbo lego-bil!


Okay, den kan ikke slå græs, men det ser ud som om den kan. Sen den lige:

Robotten er bygget til at gennemføre denne bane på kortest muligt tid:

Regler for baneforløbet:

Man starter ved den sorte streg foroven i nedenstående illustration af banen, og følger stregen rundt til den tværgående sorte streg. På den nederste røde markering står en ca. 10 cm høj mur, som robotten skal navigere udenom indtil den når den sidste sorte streg, hvor der umiddelbart på den anden side af stregen står et objekt, der skal væltes. Når objektet er væltet er robotten i mål. Der gives straftid på 2 sekunder for hver kontakt med de røde linjer. Væggen må gerne berøres, men ikke væltes / flyttes. Hvis man kører af linjen i det første stykke skal robotten flyttes tilbage til start. Hvis robotten kører af banen i den sidste halvdel – eller vælter / flytter væggen må den sættes tilbage til der, hvor den forlader den sorte linje i det første stykke af banen. Tiden kører hele tiden, indtil robotten er i mål eller man opgiver. Hvis robotten når i mål, er det den samlede tid + evt. samlet straftid, der noteres. Hver gruppe har tre forsøg til at gennemføre banen, med mellemliggende mulighed for pitstop med småjusteringer i kode / robot. Det er den hurtigste tid af de 3 forsøg, der gælder. For hvert forsøg skal banen være gennemført inden der er gået 3 minutter ekskl. straftid – ellers noteres tiden ikke.

Forkundskaber

Til dette projekt antages det at du har en smule erfaring med lodning. Hvis du ikke har loddet før kan du se hvordan her: https://www.youtube.com/watch?v=oqV2xU1fee8&t=187s

Hvis du ikke har prøvet at lave programmer til Arduino kan du sagtens lave dette projekt, da hele koden er medtaget. Det anbefales dog at du prøver at skrive koden selv – det er sjovere, du forstår mere, og det giver en større følelse af tilfredshed når det endelig virker.
Derudover anbefales det at programmet Lego digital designer hentes og installeres fra http://ldd.lego.com/da-dk for at kunne se bygge-guiden som animation.

Funktionalitet

Ideen til hvordan robotten skal løse opgaven er således:

To motorer driver hvert sit hjul. Robotten læner sig forover, og rører gulvet med undersiden af en klods. Den glider således hen ad gulvet, i stedet for et tredje hjul. Dermed kan robotten dreje ved kun at rotere det ene hjul. Foran på robotten sidder to Infrarøde linje-sensorer, som giver et analogt signal til Arduinoen, som den tolker som en værdi mellem 0-1023 (10-bit). Dette tal er proportionalt med hvor meget lys der bliver reflekteret tilbage til sensorerne fra gulvet: Et lavt tal indikerer meget lys (hvidt), og et højt tal indikerer meget lidt lys (sort). Til dette sættes en tærskel-værdi, som benyttes til at afgøre om sensoren ser hvidt eller sort. En sensor placeres på hver side af robottens midter-linje, således at når robotten kører, vil den sorte linje forblive imellem de to sensorer. Når linjen drejer, vil den ene sensor se “sort”, hvilket vil bremse det hjul som sidder i samme side. Dette vil dreje robotten så den følger linjen.

Ved linjens ende ser begge sensorer sort grundet den tværgående streg; dette signal vil blive tolket som at robotten er færdig med at følge linjen, og skal nu over og følge en mur i stedet.

Efter linjen er slut, vil et 90-graders venstre drej blive “hard-coded”. Den vil altså altid efter linjens ophør udføre nogle instrukser som resulterer i at den svinger til venstre.

Herefter vil robottens ultralyds sensor (sonar) stå vinkelret på væggen. Her er ideen at den selv hele tiden vil søge at opretholde en bestemt afstand til væggen. Er den for tæt på vil den køre lidt hurtigere med det inderste hjul end det yderste, og vice versa. Dette betyder også at når væggen ophører, vil robotten forsøge at “komme tættere” på væggen, og dermed lave et højre-sving. Dette fører robotten rundt om væggen, som den så vil fange igen og følge, indtil den rammer slutlinjen og vælter forhindringen.

 

Del 1: Hardware

Til denne robot skal der bruges:

1 Arduino Uno
1 Arduino DC Motor Shield
1 Ultralyds Sensor (HC-SR04)
1 Single-pole, single-throw switch
2 SparkFun Line Sensor Breakout (QRE1113)
1 Holder til 6 AA batterier
6 1.5V AA batterier
2 Lego DC motorer
Ledninger i forskellige farver (4xGul, 4xRød og 4xBlå)

 

Samt de i del 2 anvendte lego brikker, plus de brikker man ønsker at “pynte” den med.

 

1a: Lodning af Linje-sensorer

Først skal de to linje-sensorer have loddet nogle ca 25cm. lange ledninger på. Der loddes en blå på ground, rød på Vcc, og gul på out, således:

1b: Lodning af Sonar

Dernæst skal sonaren have ledninger. Hvis du ønsker at kunne tage den af igen kan du evt. lave ledningerne om til et stik,
se
https://www.youtube.com/watch?v=-u1t7Cdf6RE . Ellers skal de blot loddes på som følgende:
Blå på ground, rød på Vcc, og to gule på hhv. Trigger og Echo:

 

1c: Stik og batteri

En tænd/sluk knap er uundværlig, da man ellers hele tiden skal hive ledninger ud når den skal stoppes. Derfor sættes en kontakt på batteriets positive ledning, mellem batteriet og Vin på shield’et, således:

 

I denne robot sidder en Single-Pole, Double-Throw (SPDT) toggle-Switch, men den fungerer som en SPST, da den anden terminal ikke benyttes.

 

1d: Samling

Nu hvor lodningen er overstået, skal det hele samles. Dette gøres efter dette skema:

Om setup’et:

Højre motor går ind i motor shield indgang A, venstre til B. De to linje-sensorer får deres strøm fra digital pin 6 og 7. Højre linje-sensors data går ind i analog 1, venstre i analog 2. Sonarens trigger pin sidder på d4, og echo på d5.

 

Del 2: Mekanik og konstruktion

Når robotten skal bygges og den medfølgende byggeguide følges, er det vigtigt at pointere, de klodser som det lille tandhjul sidder på skal repræsentere de udleveret motorer. Ud over det lille tandhjul, er det også vigtigt at når den bygges, bliver det store tandhjul placeret over det lille, nemlig 3 ”takker” hen, og ét tak op. På byggeguiden er den placeret under, da det var det som var muligt for at få det til at hænge sammen. I guiden vil man se at der er en plade i midten af førerhuset/tårnet. På denne plade skal man først placere batteripakken, og oven på den placeres Arduinoen. Det kan eventuelt være nødvendigt at bygge noget ekstra på førerhuset, for at de to komponenter ikke falder af når man kører; dette må man prøve sig frem til. På højre side af bilen vil man kunne se at der er konstrueret 2 ”huller” hvor sonaren skal sidde. når den er sat ind bør den sikres med et par klodser på bagsiden, for at sikre sig at den bliver siddende og holdes vandret. Ved fronten af bilen er der to sorte 4×1 brikker som stikker ud over den grå bundplade. Under disse to placeres de to linje-sensorer som kan fastgøres med ledning, ståltråd eller strips.       

Byggeguide

Bygge-guiden kan ses som pdf her:

Download (PDF, 3.65MB)

Og kan downloades som .zip fil her:

http://op.tek.sdu.dk/wp-content/uploads/2017/05/byggeguid.zip

Del 3: Styringskode

Den samlede kode kan ses nedenfor. Hele koden har kommentarer (på Engelsk) som beskriver hvad det er til.

Kode:

#include <NewPing.h> //Library for easy use of Sonar

#define TRIGGER_PIN  4 //Sonar variables
#define ECHO_PIN     5
#define MAX_DISTANCE 50 //All pings above 50cm will be set to 0

NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE); //Required syntax for sonar library
int Sensor1Values[] = {0,0,0,0,0}; //Declare arrays for running average filter
int Sensor2Values[] = {0,0,0,0,0}; 
const int Analog1 = 1; //Pins to read analog sensor values
const int Analog2 = 2;
const int i = 0; //Control loop variable

int V1, V2; //Vars for line sensors
int lim1 = 500; //Limit for detecting the black line
int avg1; //Variables for average value
int avg2;

const int Adir = 12; //Motor A is right side
const int Abrk = 9; //Brake pin motor A
const int Apwr = 3; //Power output pin for motor A

const int Bdir = 13; //Motor B is left side
const int Bbrk = 83; //Brake pin for motor B
const int Bpwr = 11; //Power output pin for motor B

void RAF() //Running average filter. Takes V1 and V2 and puts them into an array, pushing all existing values one slot forward, deleting the last value. 
 {        //Then it calculates the average value of the array. This is done to avoid premature "end-line" detection, by reducing spikes in sensor values. 
  Sensor1Values[4] = Sensor1Values[3];
  Sensor1Values[3] = Sensor1Values[2];
  Sensor1Values[2] = Sensor1Values[1];
  Sensor1Values[1] = Sensor1Values[0];
  Sensor1Values[0] = V1;
  Sensor2Values[4] = Sensor2Values[3];
  Sensor2Values[3] = Sensor2Values[2];
  Sensor2Values[2] = Sensor2Values[1];
  Sensor2Values[1] = Sensor2Values[0];
  Sensor2Values[0] = V2;

  avg1 = (Sensor1Values[0] + Sensor1Values[1] + Sensor1Values[2] + Sensor1Values[3] + Sensor1Values[4])/5;
  avg2 = (Sensor2Values[0] + Sensor2Values[1] + Sensor2Values[2] + Sensor2Values[3] + Sensor2Values[4])/5;
  
}

void Left_Turn() //When following the wall, car turns either left or right
{
      digitalWrite(Adir,HIGH);
      digitalWrite(Abrk,LOW);
      analogWrite(Apwr,255);
      digitalWrite(Bdir,LOW);
      digitalWrite(Bbrk,LOW);
      analogWrite(Bpwr,40);
}

void Right_Turn()
{
      digitalWrite(Adir,LOW);
      digitalWrite(Abrk,LOW);
      analogWrite(Apwr,40);
      digitalWrite(Bdir,HIGH);
      digitalWrite(Bbrk,LOW);
      analogWrite(Bpwr,255);
}



void Run(int dir, int brake, int pwr) //Function to run one motor forward
{
      digitalWrite(dir,HIGH);
      digitalWrite(brake,LOW);
      analogWrite(pwr,155);
}

void Brake(int dir, int brake, int pwr) //Function to brake one motor (run opposite direction)
{
      digitalWrite(dir,LOW);
      digitalWrite(brake,HIGH);
      analogWrite(pwr,220);
}


void setup() 
{
  // put your setup code here, to run once:
pinMode(13,OUTPUT); //For the motors
pinMode(12,OUTPUT);
pinMode(11,OUTPUT);
pinMode(9,OUTPUT);
pinMode(8,OUTPUT);
pinMode(3,OUTPUT);

pinMode(7,OUTPUT); //For the line sensors
pinMode(6,OUTPUT);

Serial.begin(9600); //Serial connection for calibration and debugging

digitalWrite(7,HIGH); //+5V for line sensors
digitalWrite(6,HIGH); 
}

void loop() 
{
// put your main code here, to run repeatedly:

  while (i == 0) //Enter this loop first
  {
Run(Adir,Abrk,Apwr); //Go go go!
Run(Bdir,Bbrk,Bpwr);
    V1 = analogRead(Analog1); //Read line sensor values
    V2 = analogRead(Analog2); 
    Serial.println(V1); //Print for calibration
    Serial.println(V2);
    Serial.println(" ");

//If one is high and the other is low, or opposite
    if ( (V1 > lim1 and V2 < lim1) or (V1 < lim1 and V2 > lim1) )
    {
        if (V1 > lim1) //If right sensor is high ( = black line )
        {
          Brake(Adir,Abrk,Apwr); //Brake right side wheel
          Serial.println("Brake A"); //Print to confirm
          delay(1);
        }
    
        if (V2 > lim1)
        {
          Brake(Bdir,Bbrk,Bpwr);
          Serial.println("Brake B");
          delay(1);
        }
    }
             RAF(); //Run the moving average filter
        if (avg1 > 400 and avg2 > 400) //If average value is above finish line,
        {
          break; //Break out of the "line following" loop
        }

  }

      //Hard-coded left turn
      digitalWrite(Adir,LOW);
      digitalWrite(Abrk,HIGH);
      analogWrite(Apwr,0);
      digitalWrite(Bdir,LOW);
      digitalWrite(Bbrk,HIGH);
      analogWrite(Bpwr,0);
      delay(10);
      digitalWrite(Adir,HIGH);
      digitalWrite(Abrk,LOW);
      analogWrite(Apwr,255);
      digitalWrite(Bdir,HIGH);
      digitalWrite(Bbrk,LOW);
      analogWrite(Bpwr,255);
      delay(800);
      digitalWrite(Adir,HIGH);
      digitalWrite(Abrk,HIGH);
      analogWrite(Apwr,150);
      digitalWrite(Bdir,LOW);
      digitalWrite(Bbrk,HIGH);
      analogWrite(Bpwr,15);
      delay(1350);    

      while (i == 0) //This next loop follows the wall instead
      {
        Run(Adir,Abrk,Apwr); //Run forward
        Run(Bdir,Bbrk,Bpwr);
        Serial.println(sonar.ping_cm()); //Get distance from ultrasonic sensor
        
        if (sonar.ping_cm() > 15) //If distance is more than 15cm or exactly zero (wall gone)
        {
        Serial.println("right");  //Turn right
        Right_Turn();
        }
        else if (sonar.ping_cm() < 14 and sonar.ping_cm() > 1) //If wall is less than 14cm away, turn left
        {
        Left_Turn(); 
        Serial.println("left");
        }
        
        else if (sonar.ping_cm() == 0) //If wall is gone, turn right slowly
        {
        digitalWrite(Adir,LOW);
        digitalWrite(Abrk,LOW);
        analogWrite(Apwr,5);
        digitalWrite(Bdir,HIGH);
        digitalWrite(Bbrk,LOW);
        analogWrite(Bpwr,255); 
        }
        delay(10);

      }
  
}

 

Video af test-kørsel på bane:

Konklusion

Der gik lang tid med at finde de rette grænseværdier, hastigheder, osv. Men nå de endelig var på plads, kørte robotten banen igennem meget konsistent, uden problemer. Gennemsnitstiden for et forløb ligger på ca. 26 sekunder, og tiderne for konkurrencekørslen er:

1:   1:09 (Fejl ved væggen)

2:   0:28

3:   0:26

Alt i alt løser robotten opgaven, og den gør det i et godt tempo. Robotten kom på en flot 2. plads til konkurrencen, som blev vundet med 0:19.

Sammenspillet mellem mekanik, elektronik og software

Robotten er designet til at kører med så få sensorer som muligt. Der er lavet en gering på 8/40 for at få en god balance mellem hastighed og torque. Arduinoen og batteriet er blevet indkapslet for at holde på ledningerne; dette gør dog at det er lidt besværligt at skifte batterier. Linje sensorerne er meget følsomme over for afstanden til gulvet, så de skal placeres 3-6 mm fra gulvet for at virke optimalt. Sonar sensoren er forsøgt placeret forskellige steder, men de bedste resultater og den mest præcise kørsel blev opnået med sensoren foran hjulet. Det er vigtigt at sensoren peger vinkelret ind på væggen, for at kunne måle afstanden til bedst muligt. Man kan med fordel køre de mange ledninger igennem nogle kabel-rør, for at holde lidt styr på dem.

Perspektivering

Robotten er designet til en specifik bane – hvis den fik en ekstra sonar på venstre side også ville den også kunne følge en mur langs venstre side.
Det venstresving som er hard-coded kunne måske undgås med en yderligere sensor foran på bilen, som vil kunne detektere væggen.
Hastigheden på bilen vil kunne øges, evt. med flere batterier eller en højere PWM, men det resulterer i høj risiko for at linje-sensorerne ikke når at opfange den sorte linje. Dette kan kompenseres for ved at sænke den tærskelværdi som aktiverer bremsen, men det skaber et andet problem; det betyder at en smule snavs på banen vil kunne triggere venstre-svinget. Der er altså en begrænsende faktor på hastigheden, og den synes at ligge i linje-sensorerne. Derudover vil det være svært for robotten at nå at bremse hvis den har for høj inerti.

Leave a Reply