Navn på robotsystemet:

Robotten hedder chocolate express, fordi den med fineste snilde leverer chokolade fra A til B.

Gruppens medlemmer

Anna Sørensen, Mathias Kloch og Mathias Clausen

Opgaven

Opgaven var at konstruere en autonom linje-følge robot, der kunne fragte kasser fra et tydeligt markeret opsamlingssted til et tydeligt afleveringssted. Robotten skulle gøre dette, i netværk af sorte streger på hvid baggrund.

Systemets opbygning

Systemet består af to Sparkfun reflektions sensorer, to LEGO DC-motorer, en arduino UNO med motorshield, PCB med 4 LED’er og en modstand og en spændingskilde.

Der er lavet en gearing ved begge hjul, hvilket gav en bedre kontrol over hastigheden for hvert hjul samt det samlede system. De to sensorer er placeret foran hjulene, for at give en god kontrol over hvornår bilen havde fuldført en funktion. Sensorerne er placeret i en afstand fra hinanden, cirka en centimeter bredere end linjerne bilen skulle følge. Denne placering er fundet gennem en iterativ proces og giver bilen de krævede funktioner.
Hjulene kunne ikke være mindre, da motorerne ellers ville gå mod underlaget. Ved større hjul blev styringen af bilen nedsat markant og det er på den baggrund at hjulenes størrelse er valgt.

Figur 1

Systemet består af to Sparkfun reflektions sensorer, to LEGO DC-motorer, en arduino UNO med motorshield, PCB med 4 LED’er og en modstand og en spændingskilde.

Der er lavet en gearing ved begge hjul, hvilket gav en bedre kontrol over hastigheden for hvert hjul samt det samlede system. De to sensorer er placeret foran hjulene, for at give en god kontrol over hvornår bilen havde fuldført en funktion. Sensorerne er placeret i en afstand fra hinanden, cirka en centimeter bredere end linjerne bilen skulle følge. Denne placering er fundet gennem en iterativ proces og giver bilen de krævede funktioner.
Hjulene kunne ikke være mindre, da motorerne ellers ville gå mod underlaget. Ved større hjul blev styringen af bilen nedsat markant og det er på den baggrund at hjulenes størrelse er valgt.

Bilens opbygning er ligeledes lavet gennem en iterativ proces. Der har været nogle store ændringer fra første udgave til den udgave der ses på billederne. De nævneværdige ændringer er:

  • Udskiftningen af baghjulet til en klods, der blot støtter bilen.
  • Batteriet blev taget af, da det ikke kunne levere den ønskede effekt.
  • Gearing af hjulene

Motorplaceringen blev valgt, da den gav en solid opbygning af resten af bilen. De fungerede som et godt grundlag og gjorde bilen stabil.

Hardware

For at løse opgaven skulle begge DC-motor kunne køre i begge retninger. Motorerne drejer den ene retning ved en positiv spænding og i den modsatte retning ved en negativ spænding. Arduinoens Motorshield bruges til at regulere hvilken spænding motorne får. Motorshieldet er baseret på dual-bridge motor driver L298 og indeholder to h-broer. En h-bro består af fire transistorer, der gør det muligt at kunne styre spændingen i positiv- og negativ retning. Motorshieldet bliver derfor brugt til at styre retning og hastighed af DC-motorerne.

Figur 2

Til systemet er der tilføjet fire LED’er der skal vise hvilke funktioner der bliver kaldt, når bilen kører rundt med kasserne. Desværre passer antallet af LED’er ikke med antallet af states. På Figur 2 kan LED’erne og deres funktion ses.

Udfordringer

Det har været minimalt med problemer angående hardwaren. De største problemer har været udformningen af bilen så hardwaren ikke havde indvirkning på bilens kørsel. Problemerne blev løst ved at lave om på konstruktionen og derefter prøvekøre bilen. Et andet problem har været at få sensorerne til at blive siddende på samme plads under kørslen. Dette blev løst ved at tilpasse to legostykker og derefter lime dem til sensorerne som kan ses på Figur 3.

Figur 3
Figur 4

Skema over elektronikken kan ses i Figur 4.

Opførsel

Robottens states er: FollowLine, goBack, STOP, driveBack, turnLeft, turnRight, fullTurn og goBackBlack.  De er alle bygget op på samme måde, med et while-loop styret af boolean funktionen “goon”. Derudover er der en eller flere if statements, der ud fra sensorværdierne bestemmer hvordan motorerne skal køre. Alle states har en exit-condition, som enten sætter goon=false, eller returnere til void loop();, hvori rækken af states robotten skal igennem er. Det næste state bliver derefter kaldt. De kan ses i flowchartet på Figur 5

Koden er bygget op efter trial and error og de forskellige states er derfor opbygget på lidt forskellige måder.

Den første del kode er en oversigt over de forskellige konstanter og porte der bliver anvendt.

int hastighed;
bool goon=true;

//LEDs

const int straightLED = 5; 
const int leftLED = 7; 
const int rightLED = 6; 
const int turnLED = 10;

//sensors 

float sensorLeft;
float sensorRight;

//Left Motor
const int leftMotorPWM =11;
const int leftMotorDir = 12;
const int leftMotorBrake = 9;
 
//Right Motor
const int rightMotorPWM = 3;
const int rightMotorDir = 13;
const int rightMotorBrake = 8;

Den anden del af koden er setup.

void setup() {

pinMode(straightLED,OUTPUT);
pinMode(leftLED,OUTPUT);
pinMode(rightLED,OUTPUT);
pinMode(turnLED,OUTPUT);
  
pinMode(A0,INPUT); //Venstre sensor
pinMode(A1,INPUT); //Højre sensor
pinMode(2,OUTPUT); // Sensorspænding
pinMode(4,OUTPUT); //Sensorspænding

pinMode(rightMotorDir, OUTPUT);
pinMode(rightMotorBrake, OUTPUT);
pinMode(leftMotorDir, OUTPUT);
pinMode(leftMotorBrake, OUTPUT);

digitalWrite(2,HIGH);
digitalWrite(4,HIGH);
}

Den nedenstående del er loopet bilen følger for at gennem fører banerne.
Alle undtagen E er ud kommenteret, da det er den sidste bane bilen gennemførte.

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

float sensorLeft = analogRead(A0);
float sensorRight = analogRead(A1);

//A og B ruten

/*followLine();// A  start
driveBlack();
followLine();
driveBlack();
followLine();
turnRight();
followLine();
driveBlack();
followLine();
turnLeft();
followLine();
fullTurn(); 
followLine();
turnRight();
followLine();
turnLeft();
followLine();
driveBlack();
followLine();//Står ved A
goBackBlack();//tilbage til B
goBack();
goBackBlack(); 
followLine();
fullTurn();
followLine();
turnRight();
followLine();
driveBlack();
followLine();
turnRight();
followLine(); //Ved B 
fullTurn();
followLine();
turnRight();
followLine();
turnLeft();
followLine();
driveBlack();
followLine();
turnRight();
followLine();
turnLeft();
followLine();// Står ved B
STOP(); */


//C ruten

/*followLine();
driveBlack();
followLine();
driveBlack();
followLine();
driveBlack();
followLine();
fullTurn();
followLine();
turnRight();
followLine();
turnLeft();
followLine();
driveBlack();
followLine();
driveBlack();
followLine();
turnRight();
followLine();
driveBlack();
followLine();
turnLeft();
followLine();
STOP();*/


// E  ruten
followLine();
driveBlack();
followLine();
driveBlack();
followLine();
turnLeft();
followLine();
driveBlack();
followLine();
turnRight();
followLine();
fullTurn();
followLine();
turnRight();
followLine();
driveBlack();
followLine();
driveBlack();
followLine();
STOP();
}

Den nedestående del af koden er de funktioner bilen bruger til at gennemføre banerne.

void followLine()
{
  hastighed=90;
  lightStraight();
 
  while(goon){
  sensorLeft = analogRead(A0);
  sensorRight = analogRead(A1);


if(sensorLeft < 200 && sensorRight < 200)
{
  driveStraight();
}
if(sensorLeft > 200 && sensorRight > 200)
{
  fullStop();
  goon = false;
  delay(300);
}
if(sensorLeft < 200 && sensorRight > 200)
{
  driveRight();
}

if(sensorLeft > 200 && sensorRight < 200)
{
  driveLeft();
}
}
goon=true;
return;
}

void goBack()
{
 hastighed=80;
 lightStraight();
 while(goon){
     sensorLeft = analogRead(A0);
     sensorRight = analogRead(A1);

  if(sensorLeft < 200 && sensorRight < 200)
  {
    driveBack();
  }
  if(sensorLeft > 200 && sensorRight > 200)
  {
    fullStop();
    goon = false; //Hvis begge sensorer = sort, forlad while loop
    delay(300);
  }
  if(sensorLeft < 200 && sensorRight > 200)
  {
    driveBackLeft();
  }

  if(sensorLeft > 200 && sensorRight < 200)
  {
    driveBackRight();
  }
 }
goon=true;
return;
}

void STOP()
{
  lightStop();
  while(goon){ //bliv ved med at stoppe
  fullStop();
  }
}

void driveBlack()
{
  hastighed = 100;
  lightStraight();
  
  while(goon){
  sensorLeft = analogRead(A0);
  sensorRight = analogRead(A1);
    
  if(sensorLeft > 200 && sensorRight > 200)
  {
    driveStraight();
  }
  if(sensorLeft < 200 || sensorRight < 200 ){
    fullStop();
    delay(300);
    return;
  }
 }
}

void turnRight()
{
  hastighed = 65;
  lightRight();
  while(goon)
  {
    sensorLeft = analogRead(A0);
    sensorRight = analogRead(A1);
     
    if (sensorLeft > 200 && sensorRight > 200)
      {
      digitalWrite(rightMotorDir, HIGH);
      digitalWrite(rightMotorBrake, LOW);
      analogWrite(rightMotorPWM, hastighed);
 
      digitalWrite(leftMotorDir, LOW);
      digitalWrite(leftMotorBrake, LOW);
      analogWrite(leftMotorPWM, 100);
      }


   if (sensorLeft < 200 && sensorRight < 200) 
      {
      fullStop(); 
      delay(300);
      return; 
      }
   }
}

void turnLeft()
{
hastighed = 65;
lightLeft();
while(goon)
  {
  sensorLeft = analogRead(A0);
  sensorRight = analogRead(A1);
  if (sensorLeft > 200 && sensorRight > 200)
  {
    digitalWrite(rightMotorDir, LOW);
    digitalWrite(rightMotorBrake, LOW);
    analogWrite(rightMotorPWM, 100);
 
    digitalWrite(leftMotorDir, HIGH);
    digitalWrite(leftMotorBrake, LOW);
    analogWrite(leftMotorPWM, hastighed);
  }

  if (sensorLeft < 200 && sensorRight < 200) 
  {
    fullStop(); 
    delay(300);
    return;
  }
 }
}

void fullTurn(){
lightTurn();
while(goon)
  {
  sensorLeft = analogRead(A0);
  sensorRight = analogRead(A1);
  while(sensorLeft > 200){
    sensorLeft = analogRead(A0);
    sensorRight = analogRead(A1);
    digitalWrite(rightMotorDir, HIGH);
    digitalWrite(rightMotorBrake, LOW);
    analogWrite(rightMotorPWM, 100);
 
    digitalWrite(leftMotorDir, LOW);
    digitalWrite(leftMotorBrake, LOW);
    analogWrite(leftMotorPWM, 60);
    }

  if (sensorLeft < 200 && sensorRight < 200) {
    return; 
    }
  }
}
void goBackBlack()
{
  hastighed = 100;
  lightStraight();
  while(goon){
     sensorLeft = analogRead(A0);
     sensorRight = analogRead(A1);

  if(sensorLeft > 200 && sensorRight > 200)
  {
    driveBack();
    }
  if(sensorLeft < 200 || sensorRight < 200){
    fullStop();
    delay(300);
    return;
    }
  }
}
//hjælpefunktioner

void driveStraight()
{ 
  digitalWrite(rightMotorDir, HIGH);
  digitalWrite(rightMotorBrake, LOW);
  analogWrite(rightMotorPWM, hastighed);
 
  digitalWrite(leftMotorDir, HIGH);
  digitalWrite(leftMotorBrake, LOW);
  analogWrite(leftMotorPWM, hastighed);
 
}

void driveBack()
{
  digitalWrite(rightMotorDir, LOW);
  digitalWrite(rightMotorBrake, LOW);
  analogWrite(rightMotorPWM, hastighed);
 
  digitalWrite(leftMotorDir, LOW);
  digitalWrite(leftMotorBrake, LOW);
  analogWrite(leftMotorPWM, hastighed);
}

void fullStop()
{ 
  digitalWrite(rightMotorDir, LOW);
  digitalWrite(rightMotorBrake, HIGH);
  analogWrite(rightMotorPWM, 0);
 
  digitalWrite(leftMotorDir, LOW);
  digitalWrite(leftMotorBrake, HIGH);
  analogWrite(leftMotorPWM, 0);
 
}
void driveRight()
{ 
  digitalWrite(rightMotorDir, HIGH);
  digitalWrite(rightMotorBrake, HIGH);
  analogWrite(rightMotorPWM, 0);
 
  digitalWrite(leftMotorDir, HIGH);
  digitalWrite(leftMotorBrake, LOW);
  analogWrite(leftMotorPWM, hastighed);
  
}
void driveLeft()
{ 
  digitalWrite(leftMotorDir, HIGH);
  digitalWrite(leftMotorBrake, HIGH);
  analogWrite(leftMotorPWM, 0);
  
  digitalWrite(rightMotorDir, HIGH);
  digitalWrite(rightMotorBrake, LOW);
  analogWrite(rightMotorPWM, hastighed);
}


void driveBackLeft(){
  digitalWrite(leftMotorDir, LOW);
  digitalWrite(leftMotorBrake, LOW);
  analogWrite(leftMotorPWM, hastighed);
  
  digitalWrite(rightMotorDir, LOW);
  digitalWrite(rightMotorBrake, HIGH);
  analogWrite(rightMotorPWM, 0);
}

void driveBackRight(){
  digitalWrite(rightMotorDir, LOW);
  digitalWrite(rightMotorBrake, LOW);
  analogWrite(rightMotorPWM, hastighed);
 
  digitalWrite(leftMotorDir, LOW);
  digitalWrite(leftMotorBrake, HIGH);
  analogWrite(leftMotorPWM, 0);
}


//LED functions
void lightStraight(){
  digitalWrite(straightLED, HIGH);
  digitalWrite(leftLED,LOW);
  digitalWrite(rightLED,LOW);
  digitalWrite(turnLED,LOW);
}
  
void lightLeft(){
  digitalWrite(leftLED,HIGH);
  digitalWrite(straightLED, LOW);
  digitalWrite(rightLED,LOW);
  digitalWrite(turnLED,LOW);
  
}

void lightRight(){
  digitalWrite(rightLED,HIGH);
  digitalWrite(straightLED, LOW);
  digitalWrite(leftLED,LOW);
  digitalWrite(turnLED,LOW);
  }

void lightTurn(){
 digitalWrite(turnLED,HIGH);
  digitalWrite(straightLED, LOW);
  digitalWrite(rightLED,LOW);
  digitalWrite(leftLED,LOW);

}

void lightStop(){
  digitalWrite(straightLED, HIGH);
  delay(100);
  digitalWrite(straightLED, LOW);
  digitalWrite(turnLED,HIGH);
  delay(100);
  digitalWrite(turnLED, LOW);
  digitalWrite(leftLED,HIGH);
  delay(100);
  digitalWrite(leftLED,LOW);
  digitalWrite(rightLED,HIGH);
  delay(100);
  }

Vi har især haft problemer med at programmere u-vendingen. Men det blev løst ved at vende som en tre-punkts-vending, der er en kombination af tre states. Det første får robotten til at dreje bakkende via fullTurn, kører frem til den sorte linje med FollowLine, og drejer tilbage på linjen med enten turnRight eller turnLeft.

Vores oplevelse er, at bilen kører meget stabilt, hvilket dog er bekostning af hastigheden.

Figur 5

Hvad kunne vi have gjort anderledes

Vi er af den overbevisning, at robotten havde kørt bedre, hvis hjulene havde siddet tættere. Robotten har problemer med at klare D-ruten, fordi den er for bred.
Derudover havde en tredje sensor været at foretrække, når robotten skal bakke og dreje.

Ift. Koden havde det været at foretrække at have en U-vending i et state i stedet for at vende ved hjælp af tre forskellige, fx. implementeret via en counter for forskellige conditions ( eg. begge sensorer hvide eller højre sensor sort)

Konklusion

Robotten klarer A+B-ruten, C-ruten og E-ruten. State’ene er ikke særlig pænt implementeret, men robotten kører stabilt og løser opgaven. Videoerne herunder viser de tre ruter

Video af bane A, B, C og E

Leave a Reply