af Louise Præstiin Jepsen og Victor Størner-Ryberg

Link til video af testkørsel af GoBot i Unity

Robotten formår at gennemføre banen med en gennemsnitstid på omkring 73.66 sekunder. Ud af 3 forsøg har GoBot følgende tider:

  • Tid 1: 73,7 sek.
  • Tid 2: 73.9 sek.
  • Tid 3: 73.4 sek.

Oversigt over banen kan ses nedenfor:

Bane som er blevet brugt til GoBot testkørsel

Opbygning af GoBot (Hardware)

Grundet COVID-19-krisen har vi været nødt til at lave GoBot som en simulation af kredsløbet i Tinkercad (Tinkercad.com). Dvs at robotten ikke har været bygget som et fysisk produkt, og derfor kan der være få mangler hvis der skal bygges en fysisk kopi. Derudover har vi også lavet en simulation af robottens opførelse i Unity Game Engine. 

Tinkercad

GoBot’s opsætning i Tinkercad kan ses via (link: https://www.youtube.com/watch?v=8fgix4iphu4) eller kopieres og afprøves via (link: https://www.tinkercad.com/things/9A5ltFKjVB2)

Til vores opstilling i Tinkercad, brugte vi følgende komponenter: 

Komponenter brugt til GoBot

Komponentliste for GoBot:

  • 1 stk. Arduino-board til input/output
  • 2 stk. DC Motorer til hjul
  • 1 stk. L293D H-bro (forenklet og fylder mindre når der skal bygges)
  • 1 stk. 9V batteri, bruges som strømforsyning til motorer
  • 1 stk. Servomotor, bruges til at holde afstandssensor og evt. rotere sensoren
  • 1 stk. Ultrasonisk afstandssensor (Parallax PING e.l.), bruges til at måle afstand til  omkringliggende forhindringer
  • Ledninger til forbindelse mellem komponenter
  • 1 stk. Breadboard
  • 2 stk. Røde lysdioder (Valgt på baggrund af at udregninger til modstande vises ukorrekte i Tinkercad til visse andre typer lysdioder. f.eks. hvide da programmet ikke kan skelne mellem forskellige typer af lysdioder. Andre lysdioder kan bruges i den fysiske version)
  • 2 stk. Foto modstande, skal bruges til at måle lys fra to lysdioder fastspændt på undersiden af robotten. Hovedformålet er at holde robotten på den sorte linje
  • 2 stk. 10k Ω modstande til foto modstande. (Udregninger for modstande og forklaring findes i Tinkercad afsnittet)
  • 2 stk. 150 Ω modstande til Lysdioder, bruges til at begrænse strømmen til dioderne og dermed forhindre at de brændes af

Komponenter som ikke er inkluderet i Tinkercad, men som burde overvejes i et fysisk produkt: 

  • 4 hjul
  • Lego eller lignende stabile komponenter til at lave et vognstel, som kan bære alle elektroniske og mekaniske dele.
  • 2 stk. KY-033 (Tracking sensor module) som skal erstatte både foto modstande og lysdioder, samt deres tildelte modstande. Denne to i en løsning bruger infrarød lys til at holde robotten på linjen og dermed forbedres robotten ydeevne i miljøer med meget naturligt lys.

Nedenstående billede viser den fuldendte opsætning i Tinkercad. Derefter vil der være  dybere forklaring og udregninger til de forskellige komponenter.

Færdiggjort opsætning i Tinkercad

Arduino mikrokontrolleren fungerer som samlepunkt for alle ledninger, samt alt logik vedrørende sensorer (dvs input og output fra de forskellige komponenter). Derudover bruges 5V pin porten også til at levere strøm til størstedelen af komponenterne. 

Arduino Mikrokontroller

De to DC hobby gear motorer fungerer som drivkraften i robotten. Disse skal selvfølgelig have en hjul fastspændt på enden i det fysiske produkt.  Hastigheden af begge motorer bliver indstillet i koden, samt brug af PWM pins på Arduino mikrokontrolleren.

DC hobby gear motorer

Rotationsretning af hjul, samt levering af strøm til motorerne bliver delvist styret af et mindre integreret kredsløb (L293D). Dette kredsløb fungerer som to mindre H-Broer (en til hver motor), hvilket gør det muligt at drive en belastning i både positiv og negativ retning. 

Hastigheds variabler og andet motor relateret opbevares i Arduinoen.

L293D H-BRO

9V batteriet fungerer som ekstra strømforsyning til begge gearmotorer.  (Strømmen bliver  leveret gennem H-Broen). En ekstra strømforsyning er blevet valg på bagrund af at begge motorer tilsammen dræner en god mængde strøm, og Arduino mikrokontrolleren kan kun levere 5V uden yderligere ændringer. Derfor er en ekstra strømforsyning en enkel løsning til dette problem. 

L293D pins og deres funktionalitet

Et hurtigt overblik over hvad funktionaliteten over de forskellige pins til L293D kan ses på ovenstående billede. Som udgangspunkt er opsætningen meget simpel. Pin 4, 5, 12 og 13 er vores jordforbindelse. Det vil sige de sorte ledninger fra 9V batteriet og Arduino mikrokontrollerens 5V port. Pin 16 modtager 5V fra Arduino mikrokontrolleren, og bruges kun til at give strøm til selve L293D og dens interne dele. Pin 8 derimod modtager strøm fra 9V batteriet og bruges kun til de komponenter (motorer) som L293D er forbundet til. Pin 2, 7 og 10, 15 er input fra Arduinoen til L293D, hvilket skal bruges til at give output til motorerne senere hen. Pin 3, 6, 11, 14 er output fra L293D til motorerne. Hver input pin er parret med den nærmeste output pin og hver gear motor skal have to inputs. De to sidste pins, 1.2EN og 3.4EN bruges til at tænde for enten venstre eller højre side af kredsløbet.  Hovedfunktionen bag disse to pins er at styre hastigheden af motorer via brug af PWM (husk at bruge PWM pins på Arduino mikrokontrolleren),

Hvis man vil have yderligere information om L293D så opfordrer vi at kigge i databladet til komponenten. F.eks. L293-L293D fra Texas Instruments (link: http://www.ti.com/lit/ds/symlink/l293.pdf)

9V batteri som ekstra strømforsyning

Servomotoreren og den ultrasoniske afstandssensor (i dette tilfælde Parallax PING) fungerer tilsammen som robottens øjne. Afstandssensoren udsender højfrekvens lydbølger og venter på at bølgerne bliver reflekteret tilbage. Ved brug af denne interaktion kan man så opfange omkringliggende forhindringer eller objekter i form af varighed mellem udsendelse og modtagelse af lydbølger. Denne varighed kan derefter blive konverteret til meter e.lig. ved brug af lydens hastighed (se eventuelt mere i kode afsnittet for Tinkercad). 

Servomotoren holder afstandssensoren og tillader at skifte hvilken retning sensoren peger, uafhængigt af hvilken vej robotten kører i. 

Ultrasonisk afstandssensor og servomotor

Foto modstandene og lysdioderne gør det som nævnt tidligere muligt for robotten at følge  den sorte linje. Begge disse komponenter skal være spændt fast på undersiden af robotten. Der skal være en lysdiode og en fotomodstand på hver side af robotten (dvs at den sorte streg bliver i centeret af robottens vognstel). Dioderne skal derudover ikke lyse direkte på foto modstandene, men derimod lyse på overfladen som robotten kører på. Det reflekterede lys vil derefter blive opfanget af foto modstandene. Afhængigt af overfladens farve vil der blive reflekteret mere eller mindre lys og dermed vil foto modstandenes værdier skifte. Derefter skal der bare opsættes grænseværdier for hvornår robotten fraviger for meget fra den sorte streg. Som alternativ kan der også bruges en to i en løsning såsom KY-033 (Tracking sensor module).

Foto modstande og røde LED dioder

Til de to røde lysdioder bliver der tildelt en 150 Ω modstand til hver som bruges til at begrænse strømmen til selve dioden og dermed forhindre at den brændes af. Udregninger for valg af disse modstande er baseret på data sheet til 5 mm røde LED lysdioder fra RoHS (link: http://descargas.cetronic.es/WW05A3SRP4-N%20.pdf).

For at udregne modstand værdierne til så bruger vi to værdier fra datasheetet. Disse værdier er “Forward current” og “Forward Voltage”, hvilket henholdsvis er den mængde elektrisk strøm (i milliampere) og spænding (i Volt) som skal bruges for at dioden kan fungere.

Værdier fra datasheet:

Forward Current20mA
Forward Voltage2.0V
Data sheet værdier for røde lysdioder

Nu hvor vi ved spændingsfaldet over dioden så kan vi udregne spændingsfaldet over modstandene ved brug af denne værdi samt outputtet fra vores strømforsyning (ikke 9V batteriet men 5V fra Arduino mikrokontrolleren). Dette gøres ved en simpel beregning:

  • Vres er spændingsfaldet over modstand
  • Vs er spænding fra strømforsyningen
  • Vled er spændingsfaldet over LED dioden

Nu ved vi altså at spændingsfaldet over modstanene skal være 3V. Derudover ved vi at den elektriske strøm der flyder gennem dioderne er den samme i modstandene som dioderne da de sidder i serieforbindelse. Ud fra denne information kan vi så udregne modstanenes værdi ved brug af Ohms lov.

  • R er modstanden (målt i ohm) 
  • U er spændingen (målt i volt)
  • I er strømstyrken (målt i ampere)

Ud fra ovenstående udregning kan vi så bekræfte at begge lysdioder skal bruge en modstand på omkring 150 Ω for at fungere.

Til fotomodstandene bruges der to 10k Ω modstande. Disse modstande skal hver bruges til at lave det man kalder en spændingsdeler. Grunden til at vi bruger en ekstra modstand til vores fotomodstande er at fotomodstande er variable modstande. Dvs. at deres modstandsværdi skifter ud fra forskellige omstændigheder (i dette tilfælde forskellige mængder lys). Arduino mikrokontrolleren har svært ved at måle disse ændringer i modstand, men har lettere ved at måle ændringer i spænding ved brug af det indbyggede system kaldet Analog to Digital Converter. Analog to Digital Converter også kaldet ADC kan konvertere spændingsændringer modtaget fra Analog IN portene på kontrolleren om til binære tal som så kan gemmes i variabler i selve koden. 

Hvis vi prøver at måle spændingsændringer kun med fotomodstanden, så opstår der dog problemer da der ikke er et referencepunkt udover strømforsyningen (5V) og jordforbindelsen til at udregne resultatet. Dog hvis vi indsætter vores valgte modstand på 10k Ω mellem foto modstanden og jordforbindelsen så har vi et fastlagt referencepunkt til at måle spændingsændringer. Dette gør i princippet bare at når lysmængden stiger, så falder modstanden i foto modstanden, hvilket gør at spændingen over vores anden modstand stiger.

Grunden til at vi har valgt præcis 10k Ω modstande er baseret på følgende:

Vi har nu et mindre system som har et spændingsinput, output og to modstande i serieforbindelse. Skulle meget gerne ligne nedenstående billede.

System til måling af spændings output

Med denne information kan vi nu udregne spændings outputtet (Vout) ved brug af formlen:

  • Vin er vores strømforsyning, dvs 5V fra Arduino mikrokontrolleren 
  • R1 er foto modstandens værdi målt i Ω, denne værdi varierer ud fra forskellige mængder lys.
  • R2 er vores 10k Ω modstand
  • Vout er spændings outputtet som sendes tilbage til Arduino mikrokontrolleren

Den varierende maximum og minimumsværdi fra foto modstanden kan findes ved brug af data sheetet som er tilknyttet til modellen. Da Tinkercad ikke har et modelnummer e.lig. så må vi tage udgangspunkt i en selvvalgt model. I dette tilfælde har vi valgt LDR NORP12 fra RS (link: https://components101.com/sites/default/files/component_datasheet/LDR%20Datasheet.pdf).

Denne model har en minimumsværdi på omkring 400 Ω ved fuld lysstyrke og en maksimumsværdi på 9K Ω i omgivelser uden for meget lys. (Absolut maks er selvfølgelig 1 MΩ når der er intet lys til stede, men vi er interesseret i rækkevidden hvor sensoren kan se lys) Med formlen fra tidligere kan vi nu udregne outputtet ved forskellig mængder lys.

Dette giver os en synlig rækkevidde på omkring (4.81 – 2.63) = 2.18 V. Da inputtet bliver læst gennem analogRead() funktionen så ved vi at der maksimalt kan gives værdier mellem 0 og 1023. 1023 vil selvfølgelig være de 5 V total, så det betyder at der går omkring 1 værdi pr 0.005 V. Ud fra dette ved vi at der er følgende antal forskellige mulige værdier ved brug af målbart lys: 

Hvilket burde være en acceptabel rækkevidde. Dog er modellen i selve Tinkercad anderledes og derfor kan dens værdier godt afvige fra udregningerne. Det fastholdes dog stadig at den faste modstand på 10k Ω ikke må ændres til noget der er for højt eller lavt, fordi så risikerer du at få en mulig rækkevidde af målt lys som er for vag. Det betyder altså også at 10k Ω ikke er direkte nødvendig, men mere et udgangspunkt for noget som vi ved virker.  

Koden for alle Tinkercad komponenter kan findes nederst på blogindlægget. Selve koden er dog mest af alt en teoretisk opbygning, og kan derfor ikke bruges direkte til en fysisk robot. På nuværende tidspunkt virker alle sensorer og som hovedformål er de sat op til det som de skal kunne (f.eks. at se en væg eller følge linjen). Specifikke elementer såsom fart, lysmængder og korrekte drejninger er ikke sat op i Tinkercad, men kan findes i Unity afsnittet hvor koden er væsentligt udbygget.  Dette skyldes at der er mange limitationer i Tinkercad som gør det umuligt at replikere et fysisk miljø. (f.eks. at se hvornår robotten er kommet forbi to vægge mm.)

Unity simulation

Før robottens opførelse kunne programmeres, skulle den først bygges virtuelt. Til dette fik vi et Unity projekt som indeholdte følgende Unity objekter: 

  • Linje Sensorer
  • Afstandssensor 
  • Hjul 
  • H-bro_Left
  • H-bro_Right
  • Vognstel 
  • Servomotor 
  • Breadboard

Derudover var der opgivet en bane som robotten skulle kunne gennemføre på 3 minutter. For at gøre vores sammensætning proces mere overskuelig har vi opdelt den i 5 steps. 

Step 1: Servomotor og afstandssensor

For at sammensætte robotten skulle de forskellige komponenter sættes på vognstellet som i Unity var navngivet “CarChassis”. 

Ovenstående billede viser de forskellige Unity objekter som der er i scenen. 

Det første vi satte på robotten var servomotoren, og afstandssensoren. Det er vigtigt at man påsætter afstandssensorer, som et barn af Servomotorens “arm”, da det er armen der bliver roteret. 

Step 2: Hjul

Dernæst påsatte vi vores hjul. Hjulene har et joint komponent som sørger for at de er påsat robotten, og kan roterer, uden at roterer vognstellet.  

Her kan Configurable Joint komponentet ses, samt de forskellige parameter der kan ændres. 

Ved hvert hjul skulle vognstellet angives som “connected body”, under Configurable Joint komponented.

Step 3: H-broer

Hver H-bro blev påsat vognstellet hvorefter, man skulle angive hvilken motor den hørte til.  

Dette billede viser hvor motoren skal refereres til i Unity editoren. 

Step 4: Linjesensor 

Linjesensorerne skulle også sættes på robottens vognstel, og de placeres under robotten. 

Step 5: Breadboard

Breadboardet bruges til at angive hvilke pins de forskellige dele er sat til. Det skal også bare sættes på robotten, og derefter skal hvert pin angives.

Dette er et eksempel på hvordan man kan referer til de robottens andre dele i breadboardet komponentet. 

Derudover indeholder breadboarded også vores ArduinoMain script, hvilket vi bruger til at kode robottens opførelse med. 

Placering af komponenterne 

Det er vigtigt at nævne at placeringen af de forskellige sensorer har stor betydning for robottens datainput. Hvis servormotoren og afstandssensoren placeres bag på bilen, ændre det selvfølgelig afstanden den har fra en mur. Det kan dog være en fordel, da den kan opfange hvornår robotten er passeret forbi en mur, når den drejer. Derfor skal der enten tages højde for bilens længde når en afstands måles, eller bilens længde når den forbipassere en mur. 

Ovenstående billede viser bud på forskellige placeringer af servomotoren og distancesensoren. 

Linjesensorernes placering i forhold til hinanden har også stor betydning når robotten skal følge en streg. Hvis sensorerne er tættere på hinanden får den et mere præcis input, og bilen ville dreje skarpere da den opfanger en ændring meget hurtigere. Dette kan dog skabe problemer hvis bilen kører for stærkt, da der den har et mindre areal at måle på. 

Dette viser et billede af linjesensorerne med forskellig afstand mellem hinanden.

Hvis linje sensorerne skal placeres langt fra hinanden skal de eventuel kodes, så den ikke drejer for skarpt når den rammer en kant. Dette kunne gøres ved at give den en grænseværdi, når den skal dreje eller kører ligeud. Ud over dette betyder linjesensorernes placering på robotten også meget for dens opførelse. Hvis de sidder forrest registrere den banen meget mere præcis, hvilket gør den drejer bilen med det samme der er et sving. Hvis den er bagerst registrerer den inputtet senere, hvilket gør bilen drejer meget sent. Dette kan gøre at bilen drejer for sent og kører af sporet. 

Hjulene påvirker også hvor robottens opførelse, da deres midtpunkt udgør robottens omdrejningspunkt. Hvis de derfor flyttes skal man være opmærksom på at måden robotten drejer på også ændres. 

Vores placering 

Vi valgte at placere vores afstandssensor forrest, da det giver en mere præcis idé om hvor tæt robotten er på at støde ind i noget. For at sikre sig at robotten havde forbipasseret en mur blev der bare tilføjet et delay, hvor kører lidt frem. Hvilket svarer til størrelsen på bilen. 

Ovenstående billede er den opstilling vi brugte til at løse opgaven. 

Derudover placerede vi vores linjesensorer nogenlunde i midten af bilen, med lidt mindre afstand fra hinanden end den var fra start. Dette var grundet en masse trial and error, hvor vi fandt frem til det der var mest optimal for vores metode. Vi placerede også vores hjul i midten af bilen, for at gøre omdrejningspunktet centreret, da det gør det nemmere at arbejde med. 

Unity kode – programmeringsvejledning

Robotten er programmeret til at følge den sorte streg, indtil den kommer til en mur. Hvis der er en mur, skal den tjekke hvorvidt den er på stregen. Hvis den er på stregen sættes en bool til true, som fortæller robotten at den er ved den første mur.

Udfordringen ved den første mur er bl.a. at robotten kommer opfanger muren lige efter et skarpt sving. Robottens distance sensor ville derfor opfange muren mens der er roteret. Dette kan løses ved enten at få robotten til at bakke og rette sin position før den drejer, eller få den til at dreje skarpere end hvis vinklen var ret. Derudover skal robotten også tilbage på stregen igen, efter den er kommet forbi muren. Hvorimod den sidste mur skal den bare kører lige ud til mål.  Derfor valgte vi at lave to forskellige handlinger ud fra hvilken mur den støder på. 

Flowchart over robottens opførelse 

Dette viser et flowchart over robottens opførelse, i forhold til Unity projektet. Flowchartet blev brugt som udgangspunkt for at kode selve robotten. 

Linje sensor – Unity kode

Vores linjesensor blev anvendt ud fra “Braitenberg vehicle” konceptet, som går på at ændre et køretøjs/robots adfærd ud fra dens sensor input. Dette blev gjort ved at lave et par if-statements som kontrollerede inputtet fra vores linjesensor. 

Når robotten skulle kører ligeud på stregen, blev der lavet en if-statement som tjekkede differencen mellem de to linjesensor inputs. Hvis difference var mindre en bestemt variable fik robotten lov til at kører ligeud. Dette sikrede os at robotten ikke hele tiden ville rette sig efter små differencer mellem linjesensorerne.  

Hvis den ene linjesensors input var højere end den anden, var der en if-statement der fik robotten til at dreje, så værdien ca. ville være ens igen. 

Afstandssensor – Unity kode 

Vores afstandssensor blev brugt til at måle distancen mellem robotten og en mur. Hvis robotten kom inden for en vis distance og den ikke var foran en mur allerede, skal den sætte den bool der fortæller om den er foran en mur til sand. Når den så er forbi muren bliver samme bool sat til falsk. På denne måde kan koden gentages, hvis den skulle støde på en mur igen. 

Helt konkret er robotten programmeret så når den støder på en mur får den følgende kommandoer: 

  • Sæt en bool (kaldet “wall”) til true som viser den er foran en mur 
  • drej mod venstre (i et bestemt antal sekunder som ca. svarer til 90 grader) 
  • kør lidt frem (ca. længden af bilen) 
  • Drej servomotoren (mod murens retning/90 grader) 
  • kør ligeud

Derefter har vi en if-statement som tjekker om den er forbi muren og om “wall” boolen er sand. Hvis dette er tilfældet skal den gøre følgende: 

  • kør lidt frem (ca. længden af bilen) 
  • Drej mod højre 
  • kør lidt frem igen (ca. længden af bilen) 
  • tilføj +1 til en variable (kaldet “WallCount”), som tjekker hvor langt du er om væggen

Derefter har vi to if-statements den kun kan komme ind i når den har kørt ovenstående kode igennem 2 gange, da “Wallcount” skal være lig 2. På denne måde sikre vi os at den kører højre om muren 2 gange. Når den er rundt om muren tjekker den om “Wallcount == 2” og om vi er ved første mur. Hvis vi er ved første mur skal den vende sig mod venstre, sætte servomotorens rotation til ligeud, og sætte vores bool der fortæller om der er en mur eller ej til falsk.  Hvis den er ved sidste mur skal det samme, ud over den ikke skal dreje til venstre.

Konklusion

I forhold til Unity simuleringen, formår vores robot at gennemføre den valgte bane med en acceptabel tid. Den undgår diverse forhindringer og afviger ikke fra de angivne striber. Vores Tinkercad kredsløb fungerer også som det skal, uden at have nogle direkte mangler (udover kode til drejninger). Hvis robotten skulle fremstilles fysisk skal der dog ændres i både opstillingen af vores komponenter og kode. Vores Unity projekt og Tinkercad opstilling kan derfor kun bruges som referencemateriale til en fysisk model og ikke direkte. 

Perspektivering

I forhold til Unity er GoBot på nuværende tidspunkt kodet specifikt til den fremviste bane. Den kan  med stor sandsynlighed gennemføre lignende baner. Den ville dog ikke kunne komme i mål hvis banen f.eks. krævede at den skulle dreje højre om en mur, da den altid drejer venstre om. Dette kunne eventuelt løses ved at skrive mere dynamisk kode som f.eks. får robotten til at undersøge forholdene omkring sig før den drejer om en væg. 

For at optimere denne løsning kunne man også give GoBot nogle flere sensorer og ændre koden en del så den har mere information at arbejde med. F.eks. en eller flere Ultrasonisk afstandssensorer som opfanger indkommende vægge i de blinde vinkler som robotten har på nuværende tidspunkt. Hvis robotten eksempelvis drejede om en mur, og servomotoren var drejet mod muren, ville den ikke kunne detektere hvis den kørte mod endnu en mur.  

Derudover ville det være en god idé at bygge en fysisk kopi af GoBot, da den i princippet kun virker teoretisk i Unity. Mindre fejl eller mangler i programmet kan gøre at den giver forkerte resultater, og derfor burde den også testes i et fysisk miljø. 

I forhold til Tinkercad ville det være oplagt at udbytte både foto modstande og lysdioder for et simplificeret “Tracking sensor module” såsom KY-033. Hvilket ville gøre robotten mere kompakt. 

Tinkercad kode:

//Motors and photo resistors with the number 1 is the right side of the robot
//Motors and photo resistors with the number 2 is the left side of the robot
//Most values, such as speeds, light threshholds and variables for if statements,are not finalized and need to be readjusted for physical product. 
//Define all pins for motor 1
#define motorSpeedPin1 11
#define forwardPin1 10
#define reversePin1 9

//Define all pins for motor 2
#define motorSpeedPin2 5
#define forwardPin2 4
#define reversePin2 3

//Define pin and values for ultrasonic distance sensor
#define pingPin 7
long pingDuration; //full duration for ping to return to sensor
long cmToWall; //centimeters to closests object

//Include outside libary for servo and create servo object + servo posistion variable
#include <Servo.h>
Servo myservo;
int pos = 0;

//Define pins and variables for photoresistors
#define photo1 A0
#define photo2 A1
int photo1Value = 0;
int photo2Value = 0;

void setup() //Basic setup of all pins + console
{
   //Setup for motor 1 pins as output
   pinMode(motorSpeedPin1, OUTPUT);
   pinMode(forwardPin1, OUTPUT);
   pinMode(reversePin1, OUTPUT);
  
   //Setup for motor 2 pins as output
   pinMode(motorSpeedPin2, OUTPUT);
   pinMode(forwardPin2, OUTPUT);
   pinMode(reversePin2, OUTPUT);
  
   myservo.attach(6); //Attaches servo connected to pin 6 to the servo object
     
   Serial.begin(9600);  /* just for debugging console */
}

void loop() //Main loop
{
  photo1Value = analogRead(photo1); //read value from photoresistor
  photo2Value = analogRead(photo2); 
  
  UltrasonicDistanceSensor(); //Using the function to detect and measure distance to incoming walls
  
  /*The code shown below is just a shortened showcase/concept of how the robot is supposed to work. This means that it misses crucial parts, such as 
  swinging around obstacles, detecting second wall and more. See Unity code if you want specific information about the loop.*/
  
  if (photo1Value > 900 && photo2Value > 900) //default behavior, robot follows line by driving forward
  {
      M1spinForward (); //spin right motor
  	  M2spinForward (); //spin left motor
  }
  else if (photo1Value <= 900) //If right photoresistor receives less light (meaning it is on the black line). Then readjust by spinning only the left motor.
  {
      M2spinForward (); //spin left motor
      M1Stop(); //stop right motor from spinning
  }
   else if (photo2Value <= 900) //Same procedure but just for the left photoresistor instead.
  {
      M1spinForward (); //spin right motor
      M2Stop(); //stop left motor from spinning
  }
   
  if (cmToWall < 100) //Stop the robot if there is a close wall and do the turn, turn not implemented
  {
    M1Stop(); //stops robot
    M2Stop(); //stops robot
    
    //Code to turn the robot around walls should be implemented here, check Unity code for more info.
    
    myservo.write(180); //Spin servo right to make ultra sonic distance sensor face wall while turning
  }
  else
  {
  	myservo.write(93); //Reset servo posistion to make ultra sonic distance sensor face walls in front of it.
  }

  delay(100); //small delay
}

//Spin motor 1 forward
void M1spinForward () {
  digitalWrite(forwardPin1, HIGH);
  digitalWrite(reversePin1, LOW);
  analogWrite(motorSpeedPin1, 10);
}

//Spin motor 2 forward
void M2spinForward () {
  digitalWrite(forwardPin2, HIGH);
  digitalWrite(reversePin2, LOW);
  analogWrite(motorSpeedPin2, 10);
}

//Stop motor 1 from spinning
void M1Stop() {
  digitalWrite(forwardPin1, LOW);
  digitalWrite(reversePin1, LOW);
}

//Stop motor 2 from spinning
void M2Stop() {
  digitalWrite(forwardPin2, LOW);
  digitalWrite(reversePin2, LOW);
}

//Find range using Ultrasonic Distance Sensor
void UltrasonicDistanceSensor()
{
  pinMode(pingPin, OUTPUT); //set ping pin as output for ping
  digitalWrite(pingPin, LOW); //Start with a short low pulse to ensure clean high pulse
  delayMicroseconds(2);
  
  digitalWrite(pingPin, HIGH); //High pulse for ping
  delayMicroseconds(5);
  digitalWrite(pingPin, LOW); //reset
  
  pinMode(pingPin, INPUT); //Ping is read using the same pin, pulse is caught in microseconds. 
  pingDuration = pulseIn(pingPin, HIGH); //Duration caught
  cmToWall = microsecToCm(pingDuration); //Convert the time into a distance with this function
 // Serial.println(cmToWall);  //Check distance as a print
}

//Conversion function used in measuring distance with the ultrasonic sensor
long microsecToCm(long microsec) { 
   microsec = microsec / 29; //To convert duration to distance we use the speed of sound. Which is 340 m/s (in microseconds per centimeter is about 29 μs/cm)
   microsec = microsec / 2; //ping duration is measured traveling out and back. To find the distance to the object we take half of it.
return microsec;
}

//---------------- NOT USED IN TINKERCAD BUT IN UNITY ------------------- //

//Spin motor 1 backwards(not used)
void M1spinReverse () {
  digitalWrite(forwardPin1, LOW);
  digitalWrite(reversePin1, HIGH);
  analogWrite(motorSpeedPin1, 10);
}

//Spin motor 2 backwards(not used)
void M2spinReverse () {
  digitalWrite(forwardPin2, LOW);
  digitalWrite(reversePin2, HIGH);
  analogWrite(motorSpeedPin2, 10);
}

Unity kode:

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

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

    
    ulong distance;     //afstand som afstandssensoren måler
    bool wall;          //Om den er på vej rundt om en væg
    int WallCount;      //Hvor langt den er rundt om en væg    (  0 = bagved væg      1 = venstre om væg     2 = foran væg ) 


    bool online;        //Om robotten er på linjen 
    bool wallOne;       //Om det er første væg
    bool almostGoal;    //Om den næsten er i mål


    int speed = 80;
    int turnSpeed = 40;



    IEnumerator setup()
    {
        //Your code goes here:
        //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()
    {
        //Your code goes here:
 
        //få værdier 
        distance = pulseIn(4) / 100;
        int Line1= analogRead(5);
        int Line2 = analogRead(6);

     
        // --- Linjesensor --- 

        //kør ligeud hvis differencen mellem linje 1 og 2 ikke er så stor 
        if (Line1 - Line2 < 400 && wall == false)
        {
            analogWrite(1, speed);
            analogWrite(0, speed);
        }

        //drej til højre hvis den ene value er for høj
        if (Line1 < Line2 && wall == false)
        {
            analogWrite(0, turnSpeed);
            analogWrite(1, 0);
        }

        //drej til venstre hvis den anden value er for høj
        if (Line1 > Line2  && wall == false)
        {
            analogWrite(0, 0);
            analogWrite(1, turnSpeed);
        }

        //tjek om den ikke er på linjen 
        if (Line1 > 0 || Line2 > 0)
            online = true;

        //tjek om den ikke er på linjen 
        if(Line1 == 1023 && Line2 == 1023)
            online = false;


        //tjek om den støder på en linje
        //dette gøres for at den kører lidt mere ligeud ved når linjen stopper 
        if (Line1 < 200 && Line2 < 200)
        {
            analogWrite(1, speed);
            analogWrite(0, speed);
            yield return delay(200);
        }


        // --- Distance Sensor --- 

        //Hvis den støder på en mur skal den begynde at dreje om muren
        if (distance >= 1 && distance <= 2  && wall == false)
        {
            wall = true;

            //tjekker om den er på linjen eller ej 
            //hvis den er på linjen betyder det den er ved første mur 
            if (online == true)
                wallOne = true;
            else
                wallOne = false;

            //hvis den er ved første mur skal den dreje mere end anden mur (da den kommer ind fra en vinkel) 
            if(wallOne == true)
            {
                StartCoroutine("Venstre", 1450); //angiver delayed den skal vente 
                yield return delay(1700);
            }

            else
            {
                StartCoroutine("Venstre", 1250);
                yield return delay(1500);
            }

            analogWrite(0, speed);
            analogWrite(1, speed);
        }


        if (distance > 15 && wall == true || distance == 0 && wall == true)
        {
            //kør lidt frem så hele robotten er forbi muren
            analogWrite(0, speed);
            analogWrite(1, speed);
            yield return delay(700);

            StartCoroutine("Højre");
            yield return delay(1500);

            analogWrite(0, speed);
            analogWrite(1, speed);
            yield return delay(850); 
            WallCount++;

            //hvis det er første væg og det er sidste sving, skal den dreje 
            if (wallOne == true && WallCount== 2)
            {
                StartCoroutine("Venstre", 1300);
                yield return delay(1500);
                servo.write(0);
                yield return delay(200);

                wall = false; //den skal ikke længere forbi en mur
            }

            //hvis det ikke er første væg og det er sidste sving, skal den køre ligeud 
            if (wallOne == false && WallCount == 2)
            {
                servo.write(0);
                yield return delay(1000);

                analogWrite(0, speed);
                analogWrite(1, speed);

                almostGoal = true; //den er næsten i mål 
                wall = false; //den skal ikke længere forbi en mur
            }
        }

        // -- Stop når du når i mål 
        if (almostGoal == true && online == true)
        {
            analogWrite(0, 0);
            analogWrite(1, 0);
        }

        #region DoNotDelete
        //Wait for one frame
        yield return new WaitForSeconds(0);
        //New loop():
        yield return loop();
        #endregion DoNotDelete 
    }
  
    IEnumerator Venstre(int timeDelay)
    {
        //drej mod venstre
        analogWrite(0, 0);
        analogWrite(1, turnSpeed);
        analogWrite(2, turnSpeed);
        analogWrite(3, 0);
        servo.write(90);
        yield return delay(timeDelay); // 1500 for meget og 1200 for fint

        //stop alt
        analogWrite(1, 0);
        analogWrite(2, 0);
        yield return delay(100);
    }

    IEnumerator Højre()
    {

        //drej mod højre
        analogWrite(0, turnSpeed);
        analogWrite(1, 0);
        analogWrite(2, 0);
        analogWrite(3, turnSpeed);
        servo.write(90);
        yield return delay(1250); //1200

        //stop alt
        analogWrite(0, 0);
        analogWrite(3, 0);
        yield return delay(100);
    }


    #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