Af Borgar Bodoy, Thomas Vedel og Martin Sørensen

Dette blogindlæg vil beskrive hvordan man kan lave en selvstyrende robot, der kan køre igennem denne bane. I den første del af banen skal robotten følge den sorte streg via lyssensorer. I den anden del skal robotten finde vej via distance sensor.

Disse komponenter skal bruges for at bygge robotten:

2 * 9V batterier

1 * SM-S2309S Micro Servo-motor

1 * Arduino Uno R3

1 * 28015 Ultrasonic sensor

1 * L293D H-Bro driver

3 * 10kOhm resistor

2 * Ambient light sensor (phototransistor)

1 * Pushbutton

2 * Hvide LED pære

2 * Hobby DC-Motorer

2 * 160ohm resistor

2 * Polarized capacitors

1 * Slideswitch

1 * LM7805 5V regulator

Opbygning af hardware i Tinkercad

Video med gennemgang af tinkercad opsbygning

Beskrivelse af hardware

Spændings regulator

For at illustrere hvordan robottens opbygning ville være hvis det var en virkelig robot og ikke kun en simulation er der anvendt en spændingsregulator til at regulere 9V ned til 5V. Der anvendes en LM7805 (https://www.sparkfun.com/datasheets/Components/LM7805.pdf) som er en fixed output regulator der har et konstant output der anvendes på arduinoen. 

I databladet findes der et diagram over hvordan den skal monteres som det ovenover. Der er derfor taget udgangspunkt i det. Spændingsregulatoren fungerer således at input spændingen kan håndtere diverse resistorer og andre komponenter hvor regulatorens output stadig vil blive på 5V. 

Når man starter simuleringen på TinkerCAD vil der altid blive tilsluttet et USB kabel til arduinoen hvor den så får strøm. Derfor har vi tilføjet en kontakt hvor man kan tænde og slukke den fiktive strømforsyning, som vist på billedet nedenfor.

Tryk knap

Der er valgt en knap for at styre hvilken tilstand robotten er i, hvilket vil sige når robotten tændes vil den være i “follow line mode” og efter blot et tryk på knappen vil den skifte over til “ultrasonic mode” hvor den ikke vil bruge sensorer til at følge linjen men heller bruge servo samt ultrasonic sensoren. Til tryk knappen bruges modstande på 10k ohm, da den bruges til en pull down modstand (https://www.arduino.cc/documents/datasheets/Button.pdf), dette laves som nedenfor.

Den grønne ledning bliver forbundet på sekundærsiden af trykknappen, således den læser inputtet fra brugeren, når brugeren trykker på knappen skabes der forbindelser mellem de to sider og sender 5V gennem den pin den grønne ledning er forbundet til på arduinoen, hvilket svarer til HIGH, og derefter er omskrevet til “1” inde i koden og LOW, “0” når den så ikke er trykket på. 

Lysdioder

De to hvide lysdioder udsender et konstant lys. Når dette lys rammer en overflade bliver det reflekteret, det reflekterede lys måles ved hjælp af de to fototransistore.

Til udregning af formodstande til de to hvide lysdioder,  skal der findes en modstand så der opstår et spændingsfald der svarer til forskellen mellem strømkilden for de 5V og de 1.8V der så optages af lysdioden, hvilket vil sige:

U_formodstand = U_strømkilde – U_lysdiode

U_formodstand = 5V – 1.8V

U_formodstand = 3.2V

Ved brug af ohms lov, findes den størrelse der skal til for at beskytte lysdioden, ved at dele den gennemgående strøm med spændingen (http://www1.futureelectronics.com/doc/EVERLIGHT%C2%A0/334-15__T1C1-4WYA.pdf).

U = I * R

R = U_formodstand / I_lysdiode

R = 3.2V / 0.02A = 160 ohm.

På breadboardet bliver det forbundet som vist på billedet nedenfor. 

Fototransistor

Til at følge linjen i første halvdel af banen bruges der to fototransistorer, som sender lys ud og måler det reflekterede lys. På denne måde er det muligt for robotten at vide om fototransistoren er over stregen, da den så vil måle en lavere mængde reflekteret lys i forhold til over den lyse overflade. Disse værdier bruges i koden, for at bestemme robottens opførsel når den opfanger stregen på banen.

På den samme måde som tryk knappen, bruger vi en pull down resistor på 10 kOhm der sørge for at der fremkommer en spændingsdeling der vi forbinder i A0 på arduinoen, som vist på figuren ovenfor. Det er det samme gældende for den anden fototransistor der er forbundet i A1.

Fototransistoren fungerer på den samme måde som en almindelig transistor bare hvor den side som skal have spænding for at slutte transistoren er udsat for lys, det vil sige når den får lys, kan vi måle det via analogRead() metoden inde i koden.

L293D H – bro driver

En H-bro bliver brugt til at skifte mellem positiv og negativ ladning. Derfor er den et centralt komponent til denne robot, da den skal bruges til at skifte retningen som DC-motorerne drejer. L293D er en quadruple half H-driver, hvilket vil sige at den kan bruges til at styre 2 DC-motorer. 

L293D er lavet til at give tovejs strøm af op til 600mA af spændinger fra 4.5 V til 36 V. Det er dog anbefalet at bruge en forsyningsstrøm på 4,5V – 7V. (https://cdn-shop.adafruit.com/datasheets/l293d.pdf

H-broen er opdelt på den måde at Enable 1,2 og Enable 3,4 bliver brugt til at bestemme hvilken fart hver DC-motor skal rotere med, dette bestemmes via PWM (Pulse Width Modulation) som fås fra to arduino pins. PWM virker ved at der bliver sendt et digitalt signal fra arduinoen, som bliver fortolket som et analog signal af h-broen. 

Dette gøres ved hjælp af analogWrite som kaldes på de to pins der er forbundet til Enable 1,2 og Enable 3,4. analogWrite metoden tager en værdi mellem 0 og 255, hvor 0 = 0% duty cycle og 255 = 100% duty cycle. Denne robots hastighed er bestemt ved at arduinoen sender værdien 50 til begge pins, hvilket svarer til en duty cycle på 19,6% (50/255 * 100 = 19,6%). H-broens Vcc 2 er forbundet til et batteri på 9V hvilket vil sige at h-broens output spænding er 9V ved 100% duty cycle. Dette passer dog ikke helt da noget af spændingen afgår i processen, men det tages der ikke højde for i dette projekt. Ved en duty cycle på 19,6% får man en gennemsnitlig output spænding til motorerne på 1.764 V (9V*0.196 = 1.764V).

Input 1 og 2 og input 3 og 4, bruges til at åbne og lukke for gates i h-broen, hvilket resulterer i at man kan skifte mellem positive og negative spænding, hvilket bestemmer retning som DC-motorerne kører. Disse gates åbnes og lukkes ved brug af digitalWrite som sender en høj eller lav værdi til en input pin.

I turnLeft metoden fra arduino koden ses hvordan en lav værdi bliver sendt til h-broens input pin 1 og 2 og 4, og en høj værdi til input pin 3. Dette resulterer i at venstre motors positive og negative gates begge er lukket, så den ikke bevæger sig. Mens den højre motors positive gate er høj og positive gate er lav. Derved kører højre motor fremad, mens venstre motor står stille, så robotten drejer.

void turnLeft() {
  digitalWrite(leftForward, LOW);
  digitalWrite(leftBackward, LOW);
  digitalWrite(rightForward, HIGH);
  digitalWrite(rightBackward, LOW);
}

H-broen har 4 output pins. Disse pins bruger inputtet fra de tilhørende input pins til at sende værdierne videre til DC-motorne. Output spændingen til DC-motorne bestemmes ud fra Vcc 2 strømkilden, mens strømmen som h-broen benytter internt kommer fra Vcc 1.

Sammenkobling med arduino

Vcc 1:

Benyttes til strømkilden som bruges til at styre H-broen. Da det er anbefalet at bruge 4.5 V – 7 V, bliver denne koblet til arduinoens 5V udgang.

Enable 1,2:

Enable12 = pinmode(5, OUTPUT);

Arduinoens pin 5 er en digital PWM pin, hvilket er vigtigt da den skal sende PWM signaler.

Input 1:

Input 1 = pinmode(9, OUTPUT);

Forbindes til arduino port 9.

Dette er input til negativ retning til den venstre DC-motor

Input 2:

Input 2 = pinmode(8, OUTPUT);

Forbindes til arduino port 8.

Dette er input til positiv retning til den venstre DC-motor

Enable 3,4:

Enable34 = pinmode(6, OUTPUT);

Skal også være en digital PWM pin

Input 3:

Input 3 = pinmode(11, OUTPUT);

Forbindes til arduino port 11.

Dette er input til negativ retning til den højre DC-motor

Input 4:

Input 4 = pinmode(10, OUTPUT);

Forbindes til arduino port 10.

Dette er input til positiv retning til den højre DC-motor

Vcc 2:

Her vælges strømkilden til DC-motorerne. De kan bruge en spændingskilde på 5V – 9V (http://www.farnell.com/datasheets/1863913.pdf). Men da der kun bliver brugt omkring 20% duty cycle forbindes Vcc 2 til et 9V batteri.

Output 1:

Forbindes til venstre DC-motors negative pin

Output 2:

Forbindes til vestre DC-motors positive pin

Output 3:

Forbindes til højre DC-motors negative pin

Output 4:

Forbindes til højre DC-motors positive pin

GND:

De 4 ground pins forbindes til ground

Disse pins hører til Enable 1,2 og Enable 3,4 og Vcc 1 og Vcc 2.

Ultrasonisk Distance Sensor & Servo-motor

For at give robotten den fleksibilitet og mulighed for at “se omkring” sig benyttes der en servo hvor en ultrasonic sensor er koblet på. Sensoren sender en puls ud og beregner hvor lang tid dette tager for at få reflekteret tilbage, hvilket anvendes både for at komme forbi den første forhindring samt anden del af banen. Afstanden bliver målt i mikrosekunder og for at simulere hvordan det ville være fysisk er tiden omregnet til cm, dette bliver gjort ved at tage højde for at lyd bevæger sig rundt regnet 340 meter i sekundet, dette svarer til omkring 29.412 mikrosekunder per centimeter, så for at udregne hvor langt lyden har bevæget sig frem og tilbage kan der siges afstand = ((tid / lydensHastighed)) / 2), der bliver divideret med 2 fordi det er afstanden det tager for lyden at rejse frem og tilbage. Derfor kan afstanden målt i cm regnes ud ved at sige cm = ((mikrosekund / 2) / 29. Som det så er blevet gjort i arduinoen.

long msToCm(long ms) {
  return (ms / 29 / 2);
}

Som nævnt før, er sensoren monteret på en servo-motor, denne servo anvendes til at give robotten et overblik over hvad afstanden fra venstre til højre er, som vist på billedet nedenfor. 

Servoen bliver forbundet med en plus (rød) som er forbundet til arduinoens 5V udgang, sort (minus) som er forbundet til ground og en signal (grøn) ledning hvilket er forbundet i arduino pin 3 som er en digital PWM pin, hvilket styrer vinklen på servo armen. Dette medfører i at den kan trække 1.1 kg/cm og hvis det skulle være brug for yderligere kraft kan der forbindes op til 6V hvor den kan trække 1.3 kg/cm (http://descargas.cetronic.es/microservo.pdf). 

Servo motoren benytter et library som konvertere et PWM signal til hvor mange grader den skal dreje. Via metoden servo.attach(3) kan librariet bruges til servoen.

I koden betyder dette blot at man skal bruge servo.write(90) til at servo armen peger fremad, servo.write(180) til 180 grader og servo.write(0) til 0 grader. Derved behøver man ikke udregne hvilke pmw værdier der skal skrives, men man kan bare skrive antal grader i stedet (https://dronebotworkshop.com/servo-motors-with-arduino/).

Tinkercad Kode

#include <Servo.h>
//Constants for pins
const int photoLedPin_1 = 13;
const int photoLedPin_2 = 4;

const int photoPin_1 = A0;
const int photoPin_2 = A1;

const int pingPin = 7;

const int enable12 = 5;
const int enable34 = 6;
const int leftForward = 8; 
const int leftBackward = 9; 
const int rightForward = 10; 
const int rightBackward = 11;

int buttonPin = 12;
int buttonState = 0;
int lastButtonState = 0;
int speed = 50;
int count = 0;

//initializing object for servo library
Servo servo;

long duration;
long distance;

void setup() {

//Variable for pushbutton/switch case
  count = 0;  
  
  Serial.begin(9600);
  servo.attach(3);  //sets pin 3 as servo pin
 
//Setting input and output pins
  pinMode(photoPin_1, INPUT);
  pinMode(photoPin_2, INPUT);
  Serial.println(count);

  pinMode(photoLedPin_1, OUTPUT);
  pinMode(photoLedPin_2, OUTPUT);
  
  pinMode(enable12, OUTPUT);
  pinMode(enable34, OUTPUT);
  pinMode(leftForward , OUTPUT);
  pinMode(leftBackward , OUTPUT);
  pinMode(rightForward , OUTPUT);
  pinMode(rightBackward , OUTPUT);
  
  pinMode(buttonPin, INPUT);
  
  servo.write(90);  //sets servo motor to 90 degrees
  delay(500);
}

void loop() {
  //Sets the rotation speed for the DC-motors
  analogWrite(enable12, speed);
  analogWrite(enable34, speed);
  readUltrasonicDistance(pingPin);
  
//Resets the count variable for chosing between following the black line and navigating with distancesensor 
  if(digitalRead(buttonPin) == 1) {
    count += 1;
    if(count % 2 == 0){
      count = 0;
    }
    delay(100);
  }

//Reads the value from the phototransistors
  int leftSensor = analogRead(photoPin_1);
  int rightSensor = analogRead(photoPin_2);
  
  switch (count) {
// case 0 is following the black line
    case 0:
 
    if (leftSensor > rightSensor) {
      digitalWrite(photoLedPin_1, HIGH);
      digitalWrite(photoLedPin_2, LOW);

      turnRight();
      Serial.println(leftSensor + "Turning right");

    } else if (leftSensor < rightSensor) {
      digitalWrite(photoLedPin_1, LOW);
      digitalWrite(photoLedPin_2, HIGH);

      turnLeft();
      Serial.println(rightSensor + "Turning left");
    } else {
      digitalWrite(photoLedPin_1, HIGH);
      digitalWrite(photoLedPin_2, HIGH);
      moveForward();
    }
    break;

    //Case 1 is for navigating with distance sensor
    case 1:

    digitalWrite(photoLedPin_1, LOW);
    digitalWrite(photoLedPin_2, LOW);
    
    if (distance < 250 && distance > 0) {
      stopMoving();
      delay(100);
      moveBackward();
      delay(400);
      stopMoving();
      delay(100);
      
      //look right
      servo.write(180);
      delay(1000);
      long distanceR = readUltrasonicDistance(pingPin);
      Serial.println(distanceR);
      delay(1000);
      servo.write(90);
      delay(1000);
      
      //look left
      servo.write(0);
      delay(1000);  
      long distanceL = readUltrasonicDistance(pingPin);
      Serial.println(distanceL);
      delay(1000);
      servo.write(90);
      delay(1000);
      
      if (distanceR >= distanceL) {
        turnRight();
        delay(1300);
        stopMoving();
        delay(500);
      
      } else {
        
        turnLeft();
        delay(1300);
        stopMoving();
        delay(600);
      }
      
    } else {
      
      moveForward();
      break;
    }
  }
}
 
//Method to read distance sensor
long readUltrasonicDistance(int pingPin) {
  pinMode(pingPin, OUTPUT);  // Clear the trigger
  digitalWrite(pingPin, LOW);
  delayMicroseconds(2);
  // Sets the trigger pin to HIGH state for 10 microseconds
  digitalWrite(pingPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(pingPin, LOW);
  pinMode(pingPin, INPUT);
  // Reads the echo pin, and returns the sound wave travel time in microseconds
  duration = pulseIn(pingPin, HIGH);
  distance = msToCm(duration);
  //Serial.println(distance);
  return distance;
}

void moveForward() {
  digitalWrite(leftForward, HIGH);
  digitalWrite(leftBackward, LOW);
  digitalWrite(rightForward, HIGH);
  digitalWrite(rightBackward, LOW);
}

void stopMoving() {
  digitalWrite(leftForward, LOW);
  digitalWrite(leftBackward, LOW);
  digitalWrite(rightForward, LOW);
  digitalWrite(rightBackward, LOW);
}

void moveBackward() {
  digitalWrite(leftForward, LOW);
  digitalWrite(leftBackward, HIGH);
  digitalWrite(rightForward, LOW);
  digitalWrite(rightBackward, HIGH);
}

void turnLeft() {
  digitalWrite(leftForward, LOW);
  digitalWrite(leftBackward, LOW);
  digitalWrite(rightForward, HIGH);
  digitalWrite(rightBackward, LOW);
}

void turnRight() {
  digitalWrite(leftForward, HIGH);
  digitalWrite(leftBackward, LOW);
  digitalWrite(rightForward, LOW);
  digitalWrite(rightBackward, LOW);
}

long msToCm(long ms) {
  return (ms / 29 / 2);
}

Sammenspil mellem mekanik, elektronik og software

Udover selve robottens krop, hvilket er den blå plade der ses på nedenstående billede, kan robotten deles op i 3 dele: Lyssensorer, hjul og servomotor med distrance sensor

Lyssensorer

Det er ikke helt ligegyldigt hvordan man placerer sensorerne. Hvis de sidder for langt fremme på robotten har den svært ved at komme vinkelret ind på slutningen af den første del af banen. Hvis de til gengæld er placeret for langt tilbage, drejer robotten alt for meget til højre og venstre når den næsten direkte fremad. Afstanden mellem sensorerne betyder også meget. Hvis afstanden er for stor, har robotten svært ved at registrere slutning på den første del af banen. Hvis afstanden er for lille, vil den ofte registrere slutningen på banen, selvom robotten egentligt bare er i gang med et sving.

Lyssensorerne virker ved at de sender et lys ud, hvilket bliver reflekteret tilbage af banens overflade og målt af sensorerne igen. På denne måde måler sensorerne en høj værdi hvis de reflekterer på noget hvidt, og en lavere værdi hvis de reflekterer på noget sort, i dette tilfælde stregen de skal følge.

Sensorerne er tilsluttet til arduinoer via en analog input pin hver. I arduino loopet bliver analogRead brugt til at få værdierne fra sensorerne hver gang loopet kører. Disse værdier bliver bl.a. brugt til at bestemme hvilke vej robotten skal dreje, som når leftsensor måler en større værdi end rightsensor skal den dreje til højre.

Hjul

Som beskrevet ovenover, har sensorernes placering stor betydning for hvordan robotten kører gennem banen, hvilket især skyldes at det er sensorerne som bestemmer hvornår robotten får besked om at dreje. Og dette betyder at placeringen af robottens hjul, ligeledes har stor betydning for robottens vej gennem banen.

I begyndelsen af projektet var hjulene placeret foran sensorerne – Dette medførte dog et par problemer. Først og fremmest betød det, at hjulene nåede at dreje robotten forholdsvis meget før sensorerne gav signal om at robotten skulle dreje den modsatte vej, dette medførte at robotten ville kører i en masse små meget skarpe sving frem ad banen, hvilket gav en langsommere tid, samt større usikkerhed omkring den vinkel hvorpå robotten kom ind i de forskellige sving. Usikkerheden omkring svingende betød, at der var meget stor forskel på hvordan robotten kom gennem banen. Ofte oplevede vi også, at svingende blev så skarpe at robottens sensorer ”mistede grebet” på stregen, da robotten drejede så skarpt at sensorerne begge registrerede den sorte streg samtidig, hvilket resulterede i at robotten kørte af banen.

Disse problemer blev løst ved at flytte hjulene, så de er placeret bag sensorerne. Dette betyder at robotten ikke når at dreje lige så meget før sensorerne registrere stregen igen – hvilket gør at robotten ikke ”vrikker” lige så meget frem ad banen, og derfor kommer ind i svingende med en vinkel som variere meget mindre, end da hjulene var placeret forrest. På den måde sørges der for, at der ikke er for stor variation i hvordan robotten klarer banen, samt fjerner risikoen for at robotten kører af banen.

Hjulene bliver styret af en dc-motor hver, der får strøm gennem en h-bro. H-broen bliver brugt til at bestemme hvilke retning motoren skal dreje i.

I arduino koden er der lavet 5 metoder, som bruges til at bestemme hvilke output-pins der skal sendes signaler fra. De 5 metoder har til ansvar at gøre det muligt for robotten henholdsvis at kører fremad, kører baglæns, dreje til venstre, dreje til højre og stoppe med at kører.

Som det kan ses i videoen af vores TinkerCad, kører vores motorer ned 3200 rpm. For at kunne beregne farten på vores robot skal vi bruge radius på vores hjul, da vi ikke har haft mulighed for at kunne teste hvilke størrelse der ville være passende, har vi valgt at bruge en radius på 2,5 cm, for at kunne give en idé om farten som vores robot ville have.

Formlen for at kunne udregne farten er således: fart = radius * rpm * 0,10472 (http://www.endmemo.com/physics/rpmlinear.php)

For vores robot får vi derfor følgende fart:
0,025m * 3200rpm * 0,10472 = 8,37m/s * 3,6 = 30,1km/t

Servo-motor og Distance sensor

Distance sensoren som er den blå kasse med de to grå arme på, er placeret ovenpå servo-armen som er den gule kapsel. På denne måde kan distance sensoren drejes ved at styre servoen. De farvede firkanter på den orange plade indikere servo-armens grader, henholdvis 0 (lyseblå), 90 (mørkeblå) og 180 (lilla).

Distance sensoren er forbundet til arduinoen i pin 4. Og dens værdi kan dermed læses ved hjælp af pulseIn(4).

Distance sensoren fungerer på den måde at den sender lyd ud i en fremadgående retning, den måler så den tid det tager for lyden at komme tilbage, når den reflekteres på en overflade. På den måde kan man registrere robottens afstand til forhindringerne på banen.

Når servoen skal styres gørs det ved hjælp af metoden servoWrite() med den grad som man vil have den til at pege i, f.eks. Sættes den til at pege mod 90 grader når programmet started ved at kalde metoden servoWrite(90).

Et af de steder distance sensoren anvendes er i banens anden del, hvor den skal finde vej uden at skulle følge en sort streg. Dette gøres ved at robotten kører fremad med servoen pegende mod 90 grader, på samme måde som beskrevet ovenover. Når så distance sensoren registrere en værdi som er større mindre end 250 og større end 0, ved vi at det er fordi den er tæt på en væg. Grunden til at værdien skal være større end 0 er, at distance sensoren returnere 0, når lyden ikke reflekteres, altså når den ikke rammer noget. Når robotten nærmer sig væggen, benyttes servoen til at styre distance sensoren, så den scanner fra 180 grader til 90, og fra 0 til 90 grader i to for loops, som er vist afsnittet om programmets opbygning. Robotten drejer så i den mod venstre, hvis den største værdi distance sensoren registreret var mellem 0 og 90 grader, og til højre hvis den største værdi blev registreret mellem 90 og 180 grader.

Opbygning af arduino koden i Unity

Nedestående er pseudocode af main loopet på arduinoen. Dette er for at give et bedre overblik over koden, da der gøres meget brug af nested if statements. Dette afsnit vil beskrive hvordan man løser de forskellige dele af koden på en overskuelig måde. I slutningen af dette afsnit er den komplette kode.

	//Stop at black lines
 if (leftSensor and rightSensor < maxValue and firstStageFinished = false) 
 	stopMoving 
	firstStageFinished = true		
 else if (leftSensor and rightSensor < maxValue and firstStageFinished = true) 
	stopMoving
	print "Track completed!"

	//Follow black line
 if (leftSensor > rightSensor)
	turnRight
 else if (leftSensor < rightSensor)
	turnLeft
 else	

	//Dodging obstacle
	if (0 < distance < 250 and firstStateFinished = false)
		turnAroundObstacle
		lookIfDodged
		turnBackOnTrack
	//Entering second stage of the track
	else if (250 < distance 350 and firstStateFinished = true)
		lookRightWithDistanceSensor
		lookLeftWithDistanceSensor				 	
		
		//Turn away from walls
	        if (rightDistance > leftDistance)
			turnRight
		else	
			turnLeft
	//end loop with moving forward
	else
		moveForward	

Til at styre motorerne på hjulene skal man bestemme hvilke output pins fra audinoen der skal sende signal til h-broen. Til dette skal man lave de 5 nedenstående metoder.

void moveForward () {
        for (int i = 0; i < maxSpeed; i++) {
            analogWrite (0, i);
            analogWrite (1, i);
            analogWrite (2, 0);
            analogWrite (3, 0);
        }
    }

    void turnLeft () {
        for (int i = 0; i < maxSpeed; i++) {
            analogWrite (0, 0);
            analogWrite (1, i);
            analogWrite (2, i);
        }
    }

    void turnRight () {
        for (int i = 0; i < maxSpeed; i++) {
            analogWrite (0, i);
            analogWrite (1, 0);
            analogWrite (3, i);
        }
    }

    void moveBackward () {
        for (int i = 0; i < maxSpeed; i++) {
            analogWrite (0, 0);
            analogWrite (1, 0);
            analogWrite (2, i);
            analogWrite (3, i);
        }
    }

    void stopMoving () {
        for (int i = maxSpeed; i > 0; i--) {
            analogWrite (0, i);
            analogWrite (1, i);
            analogWrite (2, i);
            analogWrite (3, i);
        }
    }

Pin 0 er fremad på venstre motor, pin 1 er fremad på højre motor, pin 2 er bagud på vestre motor og pin 3 er bagud på højre motor. Til hver metode bliver der brugt et for loop, hvilket bruges til at accelerere op til maxSpeed, som er sat til 50 i arduinoens setUp metode. Dette gøres for at få en blødere opstart. For at gøre det mere blødt kan man indsætte delays i metoderne.

Den første del robotten skal løse er at følge den sorte streg. Til dette benyttes lyssensor værdierne og koden er følgende

 if (leftSensor > rightSensor) {

            turnRight ();
            yield return delay (100);
            stopMoving ();
            yield return delay (50);
        } else if (leftSensor < rightSensor) {

            turnLeft ();
            yield return delay (100);
            stopMoving ();
            yield return delay (50);

        }

Hvis den venstre sensor måler højere end den højre skal robotten dreje til højre for at rette sig selv op. Det modsatte gælder den anden vej rundt. Grunden til der bruges stopMoving er, at hvis robotten kommer for hurtigt ind i svingene er der stor chance for den kører videre i stedet for at dreje.

Til at passere den første udfordring, bruges det nedenstående kode.

if (distance <= 250 && distance > 0 && firstStateFinished == false) {

                moveBackward ();
                yield return delay (1000);
                stopMoving ();
                turnLeft ();
                yield return delay (800);
                moveForward ();
                yield return delay (1000);
                stopMoving ();
                turnRight ();
                yield return delay (700);
                moveForward ();
                yield return delay (250);
                stopMoving ();

                // Look right
                servo.write (180);
                yield return delay (1000);
                distanceR_temp = pulseIn (4);
                servo.write (90);
                yield return delay (1000);
                Debug.Log ("Distance tempR : " + distanceR_temp);
                while (distanceR_temp < 500) {
                    moveForward ();
                    yield return delay (1200);
                    distanceR_temp = pulseIn (4);
                }
                turnRight ();
                yield return delay (750);
                stopMoving ();
                leftSensor = analogRead (5);
                while (leftSensor >= 1023) {
                    yield return delay (100);
                    moveForward ();
                    leftSensor = analogRead (5);
                    yield return delay (100);
                }

                stopMoving ();
                yield return delay (1000);
                turnLeft ();
                yield return delay (1200);
            } 

Måden hvorpå det fungerer er, at når robotten kommer tæt på udfordringen, bakker den en smule og kører mod venstre og derefter en smule mod højre, hvorefter den kigger til højre ved hjælp af sonar-censoren for at se om udfordringen er passeret. Hvis robotten stadig kan se udfordringen kører den fremad, drejer til højre og fortsætter derefter i den retning indtil den venstre censor opfanger den sorte streg igen. Når stregen igen opfanges drejes robotten mod venstre, så den står nogenlunde vinkelret på linjen og kører så videre på banen ved at følge stregen igen ved hjælp af den tidligere forklarede implementering.

Den næste del er delt bestemmer hvad robotten skal gøre når den kommer til slutning af den første del af banen, samt når den rammer målstregen i slutningen af banen.

 if (leftSensor < 1023 && rightSensor < 1023 && firstStateFinished == false) {
            stopMoving ();
            yield return delay (1000);
            firstStateFinished = true;
            moveForward ();
            yield return delay (1000);

            stopMoving ();

        } else if (leftSensor < 1023 && rightSensor < 1023 && firstStateFinished == true) {
            stopMoving ();
            Debug.Log ("Track completed!!");
            yield return delay (5000);
        }

Når robotten lyssensorer begge måler en værdi under 1023 samtidig, vil det sige at robotten har ramt enden af første del af banen og enden af anden del af banen.  firstStateFisnished bliver brugt til at finde ud af hvilken del af banen den er på. Når firstStateFisnished er false stopper robotten og den ved nu at den er igennem den første del af banen, og sætter firstStateFisnished til true. Grunden til moveForward bliver brugt i det if statementet er så den forsætter forbi linjen. I else if statementet ved robotten at den er kommet til slutningen af hele banen og stopper derfor helt med at bevæge sig.

Anden del af banen bliver gennemført med det nedenstående kode som sørger for, at når robotten nærmer sig en af væggene bakker robotten lidt tilbage. Når robotten er bakket tilbage køres der to for-loops som bevæger sonar-censoren så den scanner 180 grader foran robotten. Den højeste værdi som robotten ser til hver side gemmes som henholdsvis newTempL og newTempR.

 else if (distance <= 350 && distance >= 250 && firstStateFinished == true) {
                stopMoving ();
                yield return delay (100);
                moveBackward ();
                yield return delay (400);
                stopMoving ();
                yield return delay (200);

                // Look right
                servo.write (180);
                yield return delay (1200);
                distanceR = pulseIn (4);

                for (int i = 180; i >= 90; i--) {
                    distanceR_temp = pulseIn (4);
                    yield return delay (10);

                    if (distanceR_temp > newTempR) {
                        newTempR = distanceR_temp;
                    }
                    servo.write (i);
                }

                servo.write (90);
                yield return delay (1000);

                // Look left
                servo.write (0);
                yield return delay (1200);
                distanceL = pulseIn (4);

                for (int i = 0; i <= 90; i++) {
                    distanceL_temp = pulseIn (4);
                    yield return delay (10);

                    if (distanceL_temp > newTempL) {
                        newTempL = distanceL_temp;
                    }
                    servo.write (i);
                }

                servo.write (90);
                yield return delay (1000);

De to værdier newTempL og newTempR bruges til at styre hvilken vej robotten skal køre. Da banen er opbygget således at vejen mod mål altid er at følge den længste afstand. Tjekkes der i et if-statement om de scannede værdier viser at der er længst til højre eller venstre, som vist herunder.

 if (newTempR >= newTempL) {
                    turnRight ();
                    yield return delay (1250);
                    stopMoving ();
                    yield return delay (500);
                } else {
                    turnLeft ();
                    yield return delay (1250);
                    stopMoving ();
                    yield return delay (600);
                }

Komplet unity kode

using System.Collections;
using System.Collections.Generic;
using System.Threading;
using UnityEngine;

public class ArduinoMain : MonoBehaviour {
    public Breadboard breadboard;
    public Servo servo;
    public root root;
    int maxSpeed;
    ulong distance;
    bool firstStateFinished = false;

    IEnumerator setup () {
        //Your code goes here:
        maxSpeed = 50;
        servo.write (90);

        //Example of delay:
        Debug.Log ("pre-delay log");
        yield return delay (2000); //2 second delay

        //following region ensures delay-functionality for setup() & loop(). Do not delete, must always be last thing in setup.
        #region PremadeSetup
        yield return StartCoroutine (loop ());;
        #endregion PremadeSetup
    }

    IEnumerator loop () {

        ulong distanceR = 0;
        ulong distanceL = 0;
        ulong distanceR_temp = 0;
        ulong distanceL_temp = 0;
        ulong newTempR = 0;
        ulong newTempL = 0;
        yield return delay (40);

        int rightSensor = analogRead (6);
        int leftSensor = analogRead (5);

        yield return delay (40);

        if (leftSensor < 1023 && rightSensor < 1023 && firstStateFinished == false) {
            stopMoving ();
            yield return delay (1000);
            firstStateFinished = true;
            moveForward ();
            yield return delay (1000);

            stopMoving ();

        } else if (leftSensor < 1023 && rightSensor < 1023 && firstStateFinished == true) {
            stopMoving ();
            Debug.Log ("Track completed!!");
            yield return delay (5000);
        }

        if (leftSensor > rightSensor) {

            turnRight ();
            yield return delay (100);
            stopMoving ();
            yield return delay (50);
        } else if (leftSensor < rightSensor) {

            turnLeft ();
            yield return delay (100);
            stopMoving ();
            yield return delay (50);

        } else {

            if (distance <= 250 && distance > 0 && firstStateFinished == false) {

                moveBackward ();
                yield return delay (1000);
                stopMoving ();
                turnLeft ();
                yield return delay (800);
                moveForward ();
                yield return delay (1000);
                stopMoving ();
                turnRight ();
                yield return delay (700);
                moveForward ();
                yield return delay (250);
                stopMoving ();

                // Look right
                servo.write (180);
                yield return delay (1000);
                distanceR_temp = pulseIn (4);
                servo.write (90);
                yield return delay (1000);
                Debug.Log ("Distance tempR : " + distanceR_temp);
                while (distanceR_temp < 500) {
                    moveForward ();
                    yield return delay (1200);
                    distanceR_temp = pulseIn (4);
                }
                turnRight ();
                yield return delay (750);
                stopMoving ();
                leftSensor = analogRead (5);
                while (leftSensor >= 1023) {
                    yield return delay (100);
                    moveForward ();
                    leftSensor = analogRead (5);
                    yield return delay (100);
                }

                stopMoving ();
                yield return delay (1000);
                turnLeft ();
                yield return delay (1200);
            } 
            else if (distance <= 350 && distance >= 250 && firstStateFinished == true) {
                stopMoving ();
                yield return delay (100);
                moveBackward ();
                yield return delay (400);
                stopMoving ();
                yield return delay (200);

                // Look right
                servo.write (180);
                yield return delay (1200);
                distanceR = pulseIn (4);

                for (int i = 180; i >= 90; i--) {
                    distanceR_temp = pulseIn (4);
                    yield return delay (10);

                    if (distanceR_temp > newTempR) {
                        newTempR = distanceR_temp;
                    }
                    servo.write (i);
                }

                servo.write (90);
                yield return delay (1000);

                // Look left
                servo.write (0);
                yield return delay (1200);
                distanceL = pulseIn (4);

                for (int i = 0; i <= 90; i++) {
                    distanceL_temp = pulseIn (4);
                    yield return delay (10);

                    if (distanceL_temp > newTempL) {
                        newTempL = distanceL_temp;
                    }
                    servo.write (i);
                }

                servo.write (90);
                yield return delay (1000);

                if (newTempR >= newTempL) {
                    turnRight ();
                    yield return delay (1250);
                    stopMoving ();
                    yield return delay (500);
                } else {
                    turnLeft ();
                    yield return delay (1250);
                    stopMoving ();
                    yield return delay (600);
                }
            } else {
                moveForward ();
            }
        }
        distance = pulseIn (4);

        //Following region is implemented as to allow "yield return delay()" to function the same way as one would expect it to on Arduino.
        //It should always be at the end of the loop()-function, and shouldn't be edited.
        #region DoNotDelete
        //Wait for one frame
        yield return new WaitForSeconds (0);
        //New loop():
        yield return loop ();
        #endregion DoNotDelete 
    }

    void moveForward () {
        for (int i = 0; i < maxSpeed; i++) {
            analogWrite (0, i);
            analogWrite (1, i);
            analogWrite (2, 0);
            analogWrite (3, 0);
        }
    }

    void turnLeft () {
        for (int i = 0; i < maxSpeed; i++) {
            analogWrite (0, 0);
            analogWrite (1, i);
            analogWrite (2, i);
        }
    }

    void turnRight () {
        for (int i = 0; i < maxSpeed; i++) {
            analogWrite (0, i);
            analogWrite (1, 0);
            analogWrite (3, i);
        }
    }

    void moveBackward () {
        for (int i = 0; i < maxSpeed; i++) {
            analogWrite (0, 0);
            analogWrite (1, 0);
            analogWrite (2, i);
            analogWrite (3, i);
        }
    }

    void stopMoving () {
        for (int i = maxSpeed; i > 0; i--) {
            analogWrite (0, i);
            analogWrite (1, i);
            analogWrite (2, i);
            analogWrite (3, i);
        }
    }

    #region PremadeDefinitions
    void Start () {
        servo = FindObjectOfType<Servo> ();
        if (servo == null) {
            Debug.Log ("No servo found in the scene. Consider assigning it to 'ArduinoMain.cs' manually.");
        }
        Time.fixedDeltaTime = 0.005f; //4x physics steps of what unity normally does - to improve sensor-performance.
        StartCoroutine (setup ());

    }

    IEnumerator delay (int duration) {
        float durationInSeconds = ((float) duration * 0.001f);
        yield return new WaitForSeconds (durationInSeconds);
    }

    public long map (long s, long a1, long a2, long b1, long b2) {
        return b1 + (s - a1) * (b2 - b1) / (a2 - a1);
    }

    public ulong millis () {
        return (ulong) (Time.timeSinceLevelLoad * 1000f);
    }

    public ulong abs (long x) {
        return (ulong) Mathf.Abs (x);
    }

    public long constrain (long x, long a, long b) {
        return (x < a ? a : (x > b ? b : x));
    }

    #endregion PremadeDefinitions

    #region InterfacingWithBreadboard
    public int analogRead (int pin) {
        return breadboard.analogRead (pin);
    }
    public void analogWrite (int pin, int value) {
        breadboard.analogWrite (pin, value);
    }
    public bool digitalRead (int pin) {
        return breadboard.digitalRead (pin);
    }
    public void digitalWrite (int pin, bool isHigh) {
        breadboard.digitalWrite (pin, isHigh);
    }

    public ulong pulseIn (int pin) {
        return breadboard.pulseIn (pin);
    }
    #endregion InterfacingWithBreadboard
}

Video af kørsel

Konklusion – hvor godt løser robotten opgaven

Det er lykkes at udvikle robotten således at den benytter de forskellige komponenter til at kunne gennemføre banen. Det har kun været nødvendigt at hardcode robottens opførsel for at komme uden om en enkelt udfordring, derudover, kender robotten ikke til banens forløb på forhånd.

Selvom anden del af banen gennemføres uden at være hardcoded i forhold til hvilken vej robotten skal køre, er det nødvendigt at den “rigtige” vej er den med længst afstand fra robotten hver gang den scanner efter en ny vej. Robottens opførsel er blevet implementere på den måde, da vi fandt det gav den højest mulige succesrate på anden halvdel af banen.

Da robotten ikke behøves at kunne gennemføre andre end den udleverede bane, konkluderer vi at robotten klarer opgaven tilfredsstillende og med en tid, som aldrig er i fare for at komme over grænsen på de 3 minutter.

Perspektivering – Hvad kan man gøre for at gøre den bedre?

I forhold til mulige optimeringer af robotten, kiggede vi først og fremmest på robottens hastighed. Vi kørte mange tests med forskellige hastigheder, men da flere af banens udfordringer, især svingende blev kompliceret at løse med en højere hastighed, blev vi enige om, ved at med værdien 50 som den maksimale value for analogWrite metoden opnåede vi den fart, hvorpå robotten kørte bedst igennem banen.
I forhold til farten skal det dog siges, at vores udregninger viser at robotten ville have en en fart på 30,1 km/t, hvilket vi vurderer ville være en alt for høj hastighed, hvis det havde været i den virkelige verden. Dog giver et output på 50 til vores DC-Motorer en fart som vi finder gennemfører den virtuelle bane tilfredsstillende, og vi har derfor valgt at beholde den hastighed.

Udover robottens fart, undersøgte vi også mulighederne for at optimere på måden hvorpå robotten drejer da vi kan se at det primært er i svingende at robotten taber tid. Her forsøgte vi at undgå at stoppe robottens ene hjul og i stedet blot sænke farten på det ene hjul i svingende, denne implementering gav dog problemer med, at robotten af og til kørte af stregen og ud af banen. Implementering kunne lade dog lade sig gøre, hvis vi også sænkede den generelle fart på robotten, men i så fald ville der ikke vindes så meget tid som ønsket og derfor blev denne implementering ikke valgt.

Leave a Reply