Demonstration af Roomba’s Overlegen i Unity
Figur 1: Opbygning af Roomba’s overlegen på Tinkercad

Af Kim Dinh Le & Osman Bilal Khassouk

Introduktion

Dette blogindlæg er et forsøg på at lave en robot der kan gennemføre et specifikt forhindringsbane. Det vil siges at den ikke er beregnet til at kunne gennemføre en anden bane med tilhørende elementer. Komponenter der bliver brugt er følgende:

  • Arduino Uno R3
  • 1 H-Bridge L293D
  • 9 V batteri
  • 2 Hobby Gearmotor
  • 2 Photoresistor
  • 1 Mirco Servo
  • 1 Ultrasonic Distance Sensor
  • 1 220 Ohm resistor
  • 4 Red LED
  • 2 10k Ohm resistor

Målet med denne robot er at kunne gennemføre denne bane uden at rører væggene, og komme igennem med den hurtigeste tid.

L293D i sammenspil med Arduino og DC motor.

H-Bridge Motor Driver bruges som en central komponent i vores opbyggelse af Roomba. Komponenten benyttes som en dobbelt H-bro, som gennem sammenspil med vores Arduino Uno kontrollere de 2 DC Motor som er monteret på Roomba.

Sammenkoblingen mellem L293D og de 2 DC-motor styre Roombas, hvilken retning der skal køres. For at kunne bruge motorerne har vi koblet de positive og negative poler med vores L293Ds 4 outputs, de fire outputs i figuren for neden er 1Y, 2Y, 3Y, 4Y.

1,2 EN-stiften tænder eller slukker den ene motor der er tilkoblet h-broen, den skal sættes til HIGH for at motoren kan starte. 3,4 EN-stiften gør det samme ved den anden motor. (Se figur. 2)

Vcc1 giver strøm til L293Ds interne system. Hvor Vcc2 er den strømkilde motoren bruger. Derfor er 9V batteriet tilkoblet til Vcc 2 for at styrke de motore som bruges.

Figur 2: Datablad af vores L293D

1A, 2A, 3A, 4A er L293Ds fire inputs, de er alle koblet sammen til Arduinoens pins. De fire inputs er dem som afgør, hvilken handling Arduinoen styre de 2 DC motorers handling.

Kodeudsnit 1: Pins fra Arduinoen til L293D

LDR Modstande
Fotomodstandene som bruges til Roomba benyttes for at måle forskellen på jordoverfladen og kende forskel på hvid og sort. For at øge chance for at fotomodstandene kan opfange dette ville 4 LED’er implementeres for at belyse jorden under Roomba. Der skal dog implementeres en lille væg eller lignende mellem LED’erne og fotomodstandene så de ikke påvirker de værdier som opfanges.  De værdier som opfanges af fotomodstandene, bestemmer hvordan vores Roomba agere og hvilken handling den skal påtage sig.

Hvis man kigger på kodeudsnittet på kodeudsnit 2. Så bruger Roomba en samspil af Arduino, DC-motorer, L293D, LDR- og LED sensorer til at navigere sig selv autonomt, ved at følge en sort linje.

Kodeudsnit 2: Koden til navigation af den sorte linje.

Gennem LDR-modstandene som er sammenkoblet med vores Arduino registrer værdierne, dette gøres ved brug af analogRead(sensor1Value) og analogRead(sensor2Value). Arduino registrerer disse værdier og checker hvilken If statement er sand, dette bestemmer handling vores Roomba skal påtage sig.

sonarSensor og Servo-Motor

Vi benytter denne sensor til at måle afstand på robotten og de forhindringer som kommer foran den. Sensoren måler afstanden ved brug af en sonar. Der sendes en ultralyd impuls fra sensoren, og afstanden der skal måles bestemmes ved at måle den tid det tager for lydens ekko at komme tilbage, sonaren måler dette i meter per sekund.
Der er derfor en funktion er lavet som konvertere de mikrosekunder som Sonaren kommer med til cm, dette gøres for at give en bedre forståelse af afstanden mellem Sonaren og objektet.
Dette er i vores tilfælde ideelt for robotten i den sidste forhindring med væggene, hvor sensoren bruges for at robotten ikke skal køre ind i forhindringerne, men dreje i stedet.

Kodeudsnit 3: Målingens konvertering fra meter i sekunder til Cm

Sensorens kode fungerer på den måde at sonarSensor skifter mellem “LOW”, “HIGH” også “LOW”. Sonarsensore reagere på “HIGH” pulse, der tilføjet “LOW” i starten og sat en lille tid som afstand, og en “LOW” i slutningen. At sætte en lille pause mellem “LOW” og “HIGH” i starten kan resultere i en mere effektiv “HIGH” pulse.
Dette kan give en forbedring i registreringen af objekter. Efter denne process registrere den kun ”HIGH” værdien, hvorefter konverteringen til cm finder sted. Dette kan ses i kode afsnit 4.

Kode afsnit 4: Koden som styre vores sonarSensor

Servomotoren fungere ved at sende et servo et PWM-signal (pulsbredde-modulering). Der er en minimal puls, en maksimal puls og en gentagelsesfrekvens. En servomotor kan normalt kun dreje 90 ° i begge retninger i alt 180 ° bevægelse.

Servoen benyttes til at dreje vores Sonar sensor. Sonaren er sammenkoblet ovenpå for at kunne registrere de kommende forhindringer før tid, dette bruges i den sidste del af forhindringen hvor den servoen drejer, hvilket gør sonaren i stand til at registrere muren og følge den til mål.

Man kan i Kodeafsnittet 5 se hvordan vores servomotoren drejer, den begynder med at dreje 90 grader. Dette gøres for at sonarSensoren som er monteret i toppen, kan registrere om der befinder sig objekter ved Roombas højre side.

Efter delay på 1 sekund drejer vores Roomba endnu 90 grader til 180 grader for at kunne registrere om der befinder sig objekter bag den.

Kode afsnit 5: Servo motoren

Opbygning af Program

Figur 3: Forhindringsbanen set fra fugleperspektiv

Koden er skrevet op sådan så det er delt op i to forskellige måder. Hvis man ser på banen (figur 3). Så kan man se at den første del af banen er tegnet op med en sort streg. hvoraf den sidste del af banen er frit løb. Koden er sat op i små bidder, så det er nemmere overskueligt, og giver mulighed for at skifte stadier, efter hvorhenne Roomba’s overlegen er placeret. Den første del af banen er hvor den skal følge efter den sorte streg. Ved hjælp af fotomodstander, er det blevet muligt at kunne måle værdien af roomba’s overlegen kører på enten en hvid eller sort plan, og derved roterer sig alt efter hvad der er nødvendigt.

 void followLine()
    {
        sensorLeft = analogRead(4);
        sensorRight = analogRead(5);
        if (sensorLeft > 900 && sensorRight > 900)
        {
            driveForward();
        }
        else if (sensorLeft > 900 && sensorRight < 900)
        {
            //Debug.Log("turning Right");
            turnRight();
        }
        else if (sensorLeft < 900 && sensorRight > 900)
        {
            //Debug.Log("turning Left");
            turnLeft();
        }
        else if(sensorLeft < 900 && sensorRight < 900)
        {
            driveForward();
            linefollower = false;
        }
    }

Hvis man ser videre på figur 3, kan man se at der er en forhindring ved den første del af banen. Dette er en rød mur som robotten skal kører rundt om. Det er gjort muligt at kører rundt om ved brug af afstandssensoren, som registerer at den nærmer sig en mur. Derefter kører koden ind til næste stadie hvor den vælger at undgå den første mur.

 if(wallCounter == 1)
        {
            if(pulseIn(6) < 500 && pulseIn(6) > 1)
            {
                Debug.Log("something here");
                stopDriving();
                yield return delay(1000);
                turnBack();
                yield return delay(1000);
                turnLeft();
                yield return delay(1000);
                driveForward();
                yield return delay(2500);
                turnRight();
                yield return delay(1000);
                driveForward();
                yield return delay(2500);
                turnRight();
                yield return delay(700);
                driveForward();
                yield return delay(900);
                turnLeft();
                yield return delay(1000);
                wallCounter++;
                servoMode = true;
                Debug.Log(wallCounter.ToString());
            }

Når den så har gennemførst den første del af banen, sætter vi en af vores booleske værdier til falsk. Dette gøres ved at fotomodstandene ikke længere måler at der er en sort plan. Når den når hen til sin første mur, drejer roomba’s overlegen statisk 90 grader i den rigtige retning, og servomotoren drejer 150 grader så den peger til højre for sig. Derefter går den til wallMode, hvor den så følger langs muren. Dette gøres ved at måle afstanden mellem roomba’s overlegen og muren.

   void wallMode()
    {
        if (readDist() < 800 && readDist() > 400)
        {
            driveForward();
        }
        else if (readDist() > 800 || readDist() == 0)
        {
            analogWrite(0, 90);
            analogWrite(2, 45);
        }
        else if (readDist() < 400 && readDist() > 1)
        {
            analogWrite(0, 0);
            analogWrite(2, 120);
        }
    }

Mekanik

Roomba er opbygget på en måde hvorpå en realistisk version ville være ligetil, der er opsæt LED’er tæt på de LDR modstande vi har lagt sammen. Dette gøres for at illusterer, hvordan sammensætningen ville se ud, der vil dog være noget fysisk som blokere for LED’erne så de ikke har indflydelse på vores værdier

Elektronik

Elektronik delen blev hvor den alle komponenterne skulle sammenspille med hindanden og skabe et funktionelt kredsløb som vi kalder Roomba. Gennem elektronikken har vi forskellige signaler fra vores kompenenter i form a lyd, lys osv. som giver information i form af værdier såsom afstand fra forhindringer, og lys som kan hjælpe med at navigere os til at forblive på den sorte linje.

Der blev brugt modstande af 10k ohm resistorer til LDR-sensorerne i vores TinkerCad. Dette bliver brugt da i vores mållinger med fotomodstande har den en minimum modstand på 20K ohm og ved maksimale modstand have en modstand på 2 Mohm.

Ved brug af Ohms lov bruges det til at finde fotomodstanden ud fra lysintensiteten.

Vi starter med at udregne når modstanden er 20K ohm. Hvilket er hvor lyset er maksimalt.

Derefter regner vi for når den rammer den sorte streg og lyset er minimalt.

Herudover kan vi så se at den synlige rækkevidde er rundt regnet 1,65V (4,98V-3,33V). Det vil altså siges, da inputtet bliver læst gennem analogRead() funktionen, så vil værdierne ligge mellem 0 og 1023. Arduino kører på 5V så det betyder at 1 værdi vil svare til 0,005V. Med dette kan vi så udregne de mulige værdier ved brug af målbart lys.

Dette burde være en tilstrækkeligt rækkevidde. Der skal dog lægges mærke til at 10k ohm, ikke er nødvendigt, og der måske er mulighed for at have en højere eller lavere modstand, så længe den mulige rækkevidde af målt lys ikke er for højt eller for lavt.

Software

Software fungere som hjernen af Roomba’s overlegen, og har et tæt sammenspil med elektronikken. Da Roomba’s overlegen funktionaliteter, gøres brug af værdierene som komponenterne registerer. Foreksempel anvender softwaren, brug af fotomodstanende værdier til at kunne måle om den er i en sort eller hvidt plan, og derved kører efter det bestemte fart. Et andet eksempel ville være afstandssensoren, som måler om Roomba’s overlegen er ved at ramme en mur, og derved justere sine handlinger alt efter hvilke stadie den er nået til.

Konklusion

Roomba’s overlegen klarer banen på 100 sekunder, som også kan ses i videon. For at kunne klarer denne forhindring er der blevet brugt en kombination af mekanik, elektronik og software, som giver den de nøvendige funktionaliteter for at kunne gennemføre banen. Dog er dette en kombination af vores Unity projekt og Tinkercad, og derfor ikke svarer til en fysisk plan. Derfor vil dette være en mere reference til en fysisk model og ikke direkte.

Perspektivering

Roomba’s overlegen klarer banen, dog har den kun mulighed for at klare denne specifikke bane, da mange af de forhindringer der blev lagt op, blev løst ved hjælp af “hardcoding”. Dette er gjort, da vores kompetenceniveau ikke er god nok til at kunne lave en selvtænkede AI. Hvis noget skulle ændres ville det være at den løser forhindringer i en mere selvtænkende forstand.

Raw code i Tinkercad

#include <Servo.h>

//LDR-Resistor
int sensor1Pin = A0;
int sensor1Value = 0;

int sensor2Pin = A1;
int sensor2Value = 0;

int cm, duration;

//Servo
Servo servoMotor;

//Sonar
const int sonarSensor = 2;


void setup()
{
  Serial.begin(9600);
  
  pinMode(10, OUTPUT);
  pinMode(11, OUTPUT);
  pinMode(12, OUTPUT);
  digitalWrite(12, HIGH); //Sæt denne pin til High for at enable motor 1
  

  pinMode(5, OUTPUT);
  pinMode(6, OUTPUT);
  pinMode(7, OUTPUT);
  digitalWrite(7, HIGH);// Sæt denne pin til High for at enable motor 2
  
  pinMode(sonarSensor, OUTPUT);

  servoMotor.attach(3);
}

void loop()
{
  sensor1Value = analogRead(sensor1Pin);
  Serial.println(sensor1Value);
  
  sensor2Value = analogRead(sensor2Pin);
  Serial.println(sensor2Value);

  
  
  //Test Servo
  servoMotor.write(90);
  delay(200);
  servoMotor.write(180);
  
  
if(sensor1Value > 850 && sensor2Value > 850)
  {
    digitalWrite(10,HIGH);
    digitalWrite(11,LOW);
    digitalWrite(5,HIGH);
    digitalWrite(6,LOW);
  }
  else if(sensor1Value > 850 && sensor2Value < 100)
  {
    digitalWrite(10,HIGH);
    digitalWrite(11,LOW);
    digitalWrite(5,HIGH);
    digitalWrite(6,HIGH);
  }
  else if(sensor1Value < 100 && sensor2Value > 850)
  {
    digitalWrite(10,HIGH);
    digitalWrite(11,HIGH);
    digitalWrite(5,HIGH);
    digitalWrite(6,LOW);
   }
  else if(sensor1Value < 100 && sensor2Value < 100)
  {
    digitalWrite(10,LOW);
    digitalWrite(11,HIGH);
    digitalWrite(5,LOW);
    digitalWrite(6,HIGH);
  }

   
  //Test Sonar
  digitalWrite(sonarSensor, LOW);
  delayMicroseconds(2);
  
  digitalWrite(sonarSensor, HIGH);
  delayMicroseconds(5);
  
  digitalWrite(sonarSensor, LOW);
  
  pinMode(sonarSensor, INPUT);
  
  duration = pulseIn(sonarSensor, HIGH);
  
  cm = mikrosekunderTilCm(duration);
  
  Serial.print(cm);
  Serial.print("cm");
  Serial.println();
  
  }
  
  int mikrosekunderTilCm(int mikrosekunder){
   return mikrosekunder / 29 / 2;
  
}

Raw code i Unity

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

public class ArduinoMain : MonoBehaviour
{
    public Breadboard breadboard;
    public Servo servo;

    int sensorLeft;
    int sensorRight;
    int wallCounter = 1;
    bool linefollower = true;
    bool servoMode;

    //On included/premade Arduino functions:
    //delay(timeInMilliseconds) : use "yield return delay(timeInMilliseconds)", to get similar functionality as delay() in arduino would give you.

    //map() : works exactly as on Arduino, maps a long from one range to another. 
    //If you want to get an int or a float from the map()-function, you can cast the output like this: (int)map(s, a1, a2, b1, b2) or (float)map(s, a1, a2, b1, b2) 

    //millis() : returns the time as a ulong since the start of the scene (and therefore also the time since setup() was run) in milliseconds.

    //If you want to do something similar to serial.println(), use Debug.Log(). 

    //analogWrite() and analogRead() works as they do in arduino - remember to give them correct input-values.
    //digitalRead() and digitalWrite() writes and returns bools. (High = true). 
    //LineSensors have both write-functions implemented, motors/hbridge have both read-functions implemented.
    //The console will display a "NotImplementedException" if you attempt to write to sensors or read from motors. 


    //Additions from 21-04-2020:

    //Distance sensor:
    //The Distance (ultrasonic) sensor is added, if you use "pulseIn()" on the pin it is assigned to, 
    //it will return the time it took sound to travel double the distance to the point of impact in microseconds (type: ulong).
    //This mimics roughly how the HC-SR04 sensor works. 
    //There is no max-range of the distance-sensor. If it doesn't hit anything, it returns a 0.

    //Servo:
    //if you add the servo-prefab to the scene, ArduinoMain will automatically find the servo object, essentially handling "servo.attach()" automatically. 
    //There can be only one servo controlled by this script.
    //servo.write() and servo.read() implemented, they function similar to a servomotor. 
    //The angles that servo.write() can handle are [0:179]. All inputs outside of this range, are clamped within the range.
    //servo.read() will return the last angle written to the servo-arm. 
    //In order to attach something to the servo, so that it rotates with the servo-arm, simply make the object you wish to rotate, a child of either: Servo-rotationCenter or Servo-arm. 
    //Make sure to take into account the position of the object relative to Servo-rotationCenter. The rotated object will rotate positively around the Y-axis (up) of the Servo-rotationCenter gameobject.




    IEnumerator setup()
    {
        //Your code goes here:
        servo.write(90);
        
        //Example of delay:
        Debug.Log("pre-delay log");
        yield return delay(2000); //2 second delay
        //Your code ends here -----

        //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()
    {
        //Your code goes here:
        if (linefollower == true)
        {
            followLine();
        }
        if(wallCounter == 1)
        {
            if(pulseIn(6) < 500 && pulseIn(6) > 1)
            {
                Debug.Log("something here");
                stopDriving();
                yield return delay(1000);
                turnBack();
                yield return delay(1000);
                turnLeft();
                yield return delay(1000);
                driveForward();
                yield return delay(2500);
                turnRight();
                yield return delay(1000);
                driveForward();
                yield return delay(2500);
                turnRight();
                yield return delay(700);
                driveForward();
                yield return delay(900);
                turnLeft();
                yield return delay(1000);
                wallCounter++;
                servoMode = true;
                Debug.Log(wallCounter.ToString());
            }
        }

        else if(wallCounter > 1 && servoMode == true)
        {
            ulong read = readDist();
            if(read < 700 && read > 1)
            {
                Debug.Log("Wall Ahead, adjusting");
                turnLeft();
                yield return delay(1500);
                servo.write(150);
                yield return (1600);
                servoMode = false;
            } 
        }
        else if(servoMode != true)
        {
            wallMode();
        }

        //Debug.Log("Distance wall: " + pulseIn(6));


        //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 driveForward()
    {
        analogWrite(0, 60);
        analogWrite(2, 60);
    }
    

    void turnLeft()
    {
        analogWrite(0, 0);
        analogWrite(2, 90);
    }

    void turnRight()
    {
        analogWrite(0, 90);
        analogWrite(2, 0);
    }

    void turnBack()
    {
        analogWrite(0, 0);
        analogWrite(2, 0);
        analogWrite(1, 30);
        analogWrite(3, 30);
    }

    void stopDriving()
    {
        analogWrite(0, 0);
        analogWrite(2, 0);
        analogWrite(1, 0);
        analogWrite(3, 0);
    }

    void wallMode()
    {
        if (readDist() < 800 && readDist() > 400)
        {
            driveForward();
        }
        else if (readDist() > 800 || readDist() == 0)
        {
            analogWrite(0, 90);
            analogWrite(2, 45);
        }
        else if (readDist() < 400 && readDist() > 1)
        {
            analogWrite(0, 0);
            analogWrite(2, 120);
        }
    }

    public ulong readDist()
    {
        ulong read = breadboard.pulseIn(6);
        Debug.Log("Distance from wall: " + read);
        return read;
    }

    void followLine()
    {
        sensorLeft = analogRead(4);
        sensorRight = analogRead(5);
        if (sensorLeft > 900 && sensorRight > 900)
        {
            driveForward();
        }
        else if (sensorLeft > 900 && sensorRight < 900)
        {
            //Debug.Log("turning Right");
            turnRight();
        }
        else if (sensorLeft < 900 && sensorRight > 900)
        {
            //Debug.Log("turning Left");
            turnLeft();
        }
        else if(sensorLeft < 900 && sensorRight < 900)
        {
            driveForward();
            linefollower = false;
        }
    }




    #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 _durationInMilliseconds)
    {
        float durationInSeconds = ((float)_durationInMilliseconds * 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
}

Leave a Reply