David Mørk og Mathias Winnard

Robottens opgave

Den opstillede opgave er, at robottens skal opbygges som en Finite State Machine (FSM) som skal følge sorte linjer rundt på et grid system via line sensors, hvor den skal transportere kasser fra position x til y. I denne opgave er der derfor sat fokus på, at få robotten til henholdsvis at hente kasse A til plads A og kasse B til plads B af en kørsels omgang. Banen er opbygget som følgende:

Hvordan ser systemet ud?

Designet af robotten er blevet lavet og en tre iterationer. Første iteration var bygget ved hjælp af alm. Lego brikker hvoraf designet blev forkastet da der var problemer med placeringen af linje sensorerne i den rigtige højde samt at motorerne var sat sammen på en måde hvorved de kunne bevæge sig lidt til siderne under kørsel (hvilket vil give problemer med at dreje og placere sig ift. Den sorte linje) . Robottens anden iteration kom dog op med en måde at takle disse problemer på. De alm. Legobrikker blev udskiftet til de mere anvendelige “Lego Technic” brikker. Dette gjorde at motorerne nu sad som de skulle, samt gav bedre placering af linje sensorerne (der nu tillod muligheden for at blive justeret ved at “slide” sådan at de mere præcist kunne blive placeret ift. Den sorte linje.) Tredje og sidste iteration havde minimale ændringer. Disse ændringer var små ting, såsom tilføjelsen af et alm. stykke Lego, som gjorde at ledningerne fra motorerne ikke var i vejen under test og så rodet ud. Tredje iteration er at se på billederne herunder samt komponent placeringerne.   

Robottens opbygning

På robottens front er der placeret en ”klo” som bliver brugt til at fragte kasserne fra x til y. Denne ”klo” er blevet bygget på robotten, da robotten derved kan “fange” en kasse på x, uden at kører helt op på det lysegrå område, som gør det muligt for robotten at dreje til højre/venstre med kassen. Robotten bliver drevet af to DC lego motorer. Disse er placeret helt i front af selve robotten (dog bag kloen). Motorerne er placeret dette sted på robotten, for at gøre robotten så stabil som muligt. Linje sensorerne er placeret et par cm foran motorerne. Linje sensorerne er placeret der, for at give robotten den bedst mulige drejning; dette er fordi de sorte krydser/skæringerne, bliver brugt til at skifte mellem states og når sensorernes input siger at den er kommet forbi krydset, skal bilen dreje. På motorerne er der placeret hjul på 5.7cm i diameteren. Denne størrelse er valgt, for at give lidt luft under bilen til at placere linje sensorerne; det var dermed vigtigt ikke at bruge for store/små hjul, for at give bilen en god fart til at kunne følge linjerne. Robotten har dermed heller ingen gearing. Gearingen er blevet fravalgt på det grundlag at det ikke har været nødvendigt pga. vægten på robotten, men også pga. hjulenes størrelse. Vægten på robotten gør robotten langsom nok, til at robottens linje sensorere har kunne nå at opfange lysforskelle og dermed retter sig til uden at lave en overstyring.     

Robottens Hardware

Til denne opgave er der blevet brugt følgende komponenter for at udføre den tildelte opgave:

  • 1 x Arduino Uno
  • 1 x Arduino Motor Shield
  • 2 x Lego DC motor
  • 1 x 200 ohm Resistor
  • 2 x SparkFun Line sensor Breakout (QRE1113)
  • 3 x 3mm. rød LEDer
  • 1 x PCB

Til opbyggelsen af robotten, er der lavet et diagram tegnining:

PCB Eagle tegning:

Robottens opførsel

En Finite State Machine fungere ved at der bliver hardcoded et sæt statements den skal igennem, hvorved den skifter fra et state til et andet, når en condition er blevet mødt.

I opgave “A+B”, skal Transporteren igennem 44 statements i alt, hvoraf den skifter mellem 4 states som er: forward, left, right og forwardblack, når fire forskellige transition-conditions bliver mødt. Måden hvorpå den skifter, kan ses i dette Flowchart:

Her kan koden der får Transporteren til at klare opgave A+B ses.

Der bliver defineret 4 funktioner: void Forward(), void Forwardblack(), void Left() og void Right().

Der bliver lave en switch/Case funktion hvori der bliver brugt if statements til skifte mellem case’s, hvori hver case kører en af de 4 defineret funktioner.

#include "Arduino.h"

//Variabler
const int sensorPin1 = A0;
const int sensorPin2 = A1;
int led_Forward = A3;
int led_Left = A4;
int led_Right = A5;
int Sensor1 = 0;
int Sensor2 = 0;
int threshold = 350;
int motorTurnA = 12;
int motorSpeedA = 3;
int motorTurnB = 13;
int motorSpeedB = 11;
int speedForward = 255;
int speedTurn = 0;

static unsigned int state;


void setup() {

  Serial.begin(9600);

  pinMode(sensorPin1, INPUT);
  pinMode(sensorPin2, INPUT);

  pinMode(led_Forward, OUTPUT);
  pinMode(led_Left, OUTPUT);
  pinMode(led_Right, OUTPUT);



  pinMode(motorTurnA, OUTPUT);                             
  pinMode(motorSpeedA, OUTPUT);                           
  pinMode(motorTurnB, OUTPUT);                             
  pinMode(motorSpeedB, OUTPUT);                            

  state = 1;
}

void Forward() {
 // delay(500);
  digitalWrite(12, HIGH);
  analogWrite(3, 85);  

  digitalWrite(13, HIGH);  
  analogWrite(11, 75);    

  
  if (Sensor1 > threshold) {
    analogWrite(3, 0);    
  }
  if (Sensor2 > threshold) {
    analogWrite(11, 0);  
  }

  Serial.println("forward");
  digitalWrite(led_Forward, HIGH);
  digitalWrite(led_Left, LOW);
  digitalWrite(led_Right, LOW);
  
}

void Forwardblack() {
 // delay(200);
  if(Sensor1 > threshold && Sensor2 > threshold){
  digitalWrite(12, HIGH);
  analogWrite(3, 85);  

  digitalWrite(13, HIGH);  
  analogWrite(11, 75); 
  }
 Serial.println("forwardblack");
 digitalWrite(led_Forward, HIGH);
 digitalWrite(led_Left, LOW);
 digitalWrite(led_Right, LOW);
}

void Left(){
 // delay(200);
    //Motor A forward 
  digitalWrite(12, LOW);
  analogWrite(3, 70);

  //Motor B Backward
  digitalWrite(13, HIGH);
  analogWrite(11, 140); 

  Serial.println("left");
  digitalWrite(led_Left, HIGH);
  digitalWrite(led_Forward, LOW);
  digitalWrite(led_Right, LOW);
}

void Right(){
  //delay(200);
    //Motor A Backward
  digitalWrite(12, HIGH);
  analogWrite(3, 140);

  //Motor B forward
  digitalWrite(13, LOW);
  analogWrite(11, 70);

  Serial.println("Right");
  digitalWrite(led_Right, HIGH);
  digitalWrite(led_Left, LOW);
  digitalWrite(led_Forward, LOW);
}


void OpgaveAB() {
  
  switch (state)
  {
    case 1:
    Forward();
    if (Sensor1 > threshold && Sensor2 > threshold) {
      state = 2;
    }
    break;
    case 2:
    Forwardblack();
    if (Sensor1 < threshold || Sensor2 < threshold) {
      state = 3;
    }
    break;
    case 3:
    Forward();
    if (Sensor1 > threshold && Sensor2 > threshold) {
      state = 4;
    }
    break;
    case 4:
    Forwardblack();
    if (Sensor1 < threshold || Sensor2 < threshold) {
      state = 5;
    }
    break;
    case 5:
    Forward();
    if (Sensor1 > threshold && Sensor2 > threshold) {
      state = 6;
    }
    break;
    case 6:
    Right();
    if (Sensor1 > threshold) {
      state = 7;
    }
    break;
    case 7:
    Forward();
    if (Sensor1 > threshold && Sensor2 > threshold) {
      state = 8;
    }
    break;
    case 8:
    Forwardblack();
    if (Sensor1 < threshold || Sensor2 < threshold) {
      state = 9;
    }
    break;
    case 9:
    Forward();
    if (Sensor1 > threshold && Sensor2 > threshold) {
      state = 10;
    }
    break;
    case 10:
    Left();
    if (Sensor2 > threshold) {
      state = 11;
    }
    break;
    case 11:
    Forward();
    if (Sensor1 > threshold && Sensor2 > threshold) {
      state = 12;
    }
    break;
    case 12:
    Left();
    if (Sensor2 > threshold) {
      state = 13;
    }
    break;
    case 13:
    Forward();
    if (Sensor1 > threshold && Sensor2 > threshold) {
      state = 14;
    }
    break;
    case 14:
    Left();
    if (Sensor2 > threshold) {
      state = 15;
    }
    break;
    case 15:
    Forward();
    if (Sensor1 > threshold && Sensor2 > threshold) {
      state = 16;
    }
    break;
    case 16:
    Left();
    if (Sensor2 > threshold) {
      state = 17;
    }
    break;
    case 17:
    Forward();
    if (Sensor1 > threshold && Sensor2 > threshold) {
      state = 18;
    }
    break;
    case 18:
    Forwardblack();
    if (Sensor1 < threshold || Sensor2 < threshold) {
      state = 19;
    }
    break;
    case 19:
    Forward();
    if (Sensor1 > threshold && Sensor2 > threshold) {
      state = 20;
    }
    break;
    case 20:
    Forwardblack();
    if (Sensor1 < threshold || Sensor2 < threshold) {
      state = 21;
    }
    break;
    case 21:
    Forward();
    if (Sensor1 > threshold && Sensor2 > threshold) {
      state = 22;
    }
    break;
    case 22:
    Right();
    if (Sensor1 > threshold) {
      state = 23;
    }
    break;
    case 23:
    Forward();
    if (Sensor1 > threshold && Sensor2 > threshold) {
      state = 24;
    }
    break;
    case 24:
    Right();
    if (Sensor1 > threshold) {
      state = 25;
    }
    break;
    case 25:
    Forward();
    if (Sensor1 > threshold && Sensor2 > threshold) {
      state = 26;
    }
    break;
    case 26:
    Right();
    if (Sensor1 > threshold) {
      state = 27;
    }
    break;
    case 27:
    Forward();
    if (Sensor1 > threshold && Sensor2 > threshold) {
      state = 28;
    }
    break;
    case 28:
    Left();
    if (Sensor2 > threshold) {
      state = 29;
    }
    break;
    case 29:
    Forward();
    if (Sensor1 > threshold && Sensor2 > threshold) {
      state = 30;
    }
    break;
    case 30:
    Forwardblack();
    if (Sensor1 < threshold || Sensor2 < threshold) {
      state = 31;
    }
    break;
    case 31:
    Forward();
    if (Sensor1 > threshold && Sensor2 > threshold) {
      state = 32;
    }
    break;
    case 32:
    Right();
    if (Sensor1 > threshold) {
      state = 33;
    }
    break;
    case 33:
    Forward();
    if (Sensor1 > threshold && Sensor2 > threshold) {
      state = 34;
    }
    break;
    case 34:
    Right();
    if (Sensor1 > threshold) {
      state = 35;
    }
    break;
    case 35:
    Forward();
    if (Sensor1 > threshold && Sensor2 > threshold) {
      state = 36;
    }
    break;
    case 36:
    Right();
    if (Sensor1 > threshold) {
      state = 37;
    }
    break;
    case 37:
    Forward();
    if (Sensor1 > threshold && Sensor2 > threshold) {
      state = 38;
    }
    break;
    case 38:
    Left();
    if (Sensor2 > threshold) {
      state = 39;
    }
    break;
    case 39:
    Forward();
    if (Sensor1 > threshold && Sensor2 > threshold) {
      state = 40;
    }
    break;
    case 40:
    Right();
    if (Sensor1 > threshold) {
      state = 41;
    }
    break;
    case 41:
    Forward();
    if (Sensor1 > threshold && Sensor2 > threshold) {
       state = 42;
    } 
    break;
    case 42:
    Left();
    if (Sensor2 > threshold) {
       state = 43;
    }
    break;
    case 43:
    Forward();
    if (Sensor1 > threshold && Sensor2 > threshold) {
       state = 44;
    }
    break;
    case 44:
    Forward();
    if (Sensor1 > threshold && Sensor2 > threshold) {
       state = 14;
    }
  }
}


void loop() {
  
  //Aflæser input for lyssensor
  delay(500);
  Sensor1 = analogRead(sensorPin1);
  Sensor2 = analogRead(sensorPin2);
  //Sensor3 = analogRead(sensorPin3);
  //Sensor4 = analogRead(sensorPin4);

  //Printer lyssensor input
  Serial.print(Sensor1); // Venstre1
  Serial.print(", ");
  Serial.println(Sensor2); // Højre1
  Serial.print(", ");

  OpgaveAB();

  
}

  

Robotten har, som tidligere sagt, fire states i alt som er disse er:

  • Forward
  • Left
  • Right
  • Forwardblack

Forward:

Dette state bliver brugt til at følge den sorte linje på banen, hvoraf der bliver benyttet to linje sensorer (QRE1113). Disse sensorer udgiver og opfanger den mængde af infrarød lys der bliver reflekteret tilbage til sensorerne og returnere en analog værdi. Hvis denne værdi er høj, ser den mørke (den sorte linje), hvorimod hvis værdien er lav, ser den lyst (den hvide dug). Robotten følger dermed linjen, ved at bruge den værdi der bliver returneret. Hvis denne værdi er høj (altså ser sort) på venstre sensor, sættes venstre motor speed til 0 for at rette ind mod linjen; det samme gøres for den højre sensor. Dette giver bilen en lidt slingrende kørsel, men linjen bliver dermed også fulgt.  

Left:

Dette state bliver brugt til at dreje bilen til venstre. Dette bliver gjort ved at sætte venstre motor speed op og sætte højre motor til at ændre retning, hvilket får hjulet til at kører baglæns.

Right:

Dette state bliver brugt til at dreje bilen til højre. Dette bliver gjort ved at sætte højre motor speed op og sætte venstre motor til at ændre retning, hvilket får hjulet til at kører baglæns.

Forwardblack:

Dette state bliver brugt til at få bilen til at kører fremad når begge sensorer registrerer mørke.

Robotten har fire transition conditions for at skifte state.  Disse fire er:

  • Sensor1 > threshold && Sensor2 > threshold
  • Sensor1 < threshold || Sensor2 < threshold
  • Sensor1 > threshold
  • Sensor2 > threshold

Sensor1 > threshold && Sensor2 > threshold:

Denne condition bliver brugt til at finde ud af, hvornår bilen er nået til et kryds på banen. Dette bliver gjort ved, at der er blevet sat et threshold (250). Når sensor værdien er højere end threshold, ser den mørkt (sort på banen), mens når den er mindre, ser sensoren lyst (hvidt på banen), dette bevirker at robotten kan se når den rammer et kryds(som er mørkt på banen) når begge sensorer er over threshold.

Sensor1 < threshold || Sensor2 < threshold:

Denne condition bliver brugt til at finde ud af hvornår bilen er forbi et kryds. Dette bliver gjort ved at se om en af sensorerne læser mindre end threshold.

Sensor1 > threshold:

Denne condition bliver brugt til at finde ud af hvornår bilen er færdig med at dreje til højre. Dette bliver gjort ved at se om sensor 1 (venstre) er over threshold.

Sensor2 > threshold:

Denne condition bliver brugt til at finde ud af hvornår bilen er færdig med at dreje til venstre. Dette bliver gjort ved at se om sensor 2 (højre) er over threshold.

Konklusion

Ud fra de opstillede krav om at robotten skal kunne hente og transporterer kasse A til position A for derefter at hente og transportere kasse B til position B, kan der konkluderes at opgaven ikke er 100% blevet løst. Opgaven er ikke blevet løst, da banen er snavset/beskidt som gør at begge sensorer kan risikere at opfange mørke og dermed skifte til næste state før dette er aktuelt. Derfor kan der ses i videoen, hvor robotten klarer A+B, at robotten bliver hjulpet undervejs, ved at sætte bilen over til det sted som passer overens med det state den skal udfører på det givne tidspunkt.

Ud fra de test der er blevet udført er der her nogle forslag der kan optimere robotten i forhold til tid og stabiltet.

  • Forbedringer af robotten
    • Bakke funktion så kasse x ikke skal fjernes når den er kommet på plads.
    • u-turn funktion kan give bilen kortere vej.
    • Placer flere linesensorer vil kunne give mere stabile sving.
    • Flere motorer så ”klo’en” kan tage fat i kassen og løfte den når kasserne skal transporteres.

Video

Leave a Reply