Af Mads Obel Jensen og Sebastian Christiansen

Introduktion

Denne tutorial vil guide dig igennem processen at bygge en robot som er i stand til at navigere i en labyrint, at undgå at køre ind i vægge, og følge en guide streg på jorden.

Lad os komme i gang!

Du skal bruge

1 arduino uno

1 batteri på 9v 

1 breadboard 

1 stk rød led.

2 fotomodstande 

2 dioder

3 modstande, 2x 10k ohm, 1x 250 Ohm

1 stk L293D H-bro

2 stk hobby gearmotorer

1 sonar sensor

1 stepper motor

4 kondensatorer 100 nf

Opbygning

Således er robotten konstrueret i TinkerCad, ved opstart vil motorene rotere og robotten blive bragt til livs, og begynde at observere omgivelserne.

Beskrivelse

Som set i billedet af robotten ovenover består denne robot af tre primære sektioner, hjerne, krop, og lemmer. Hjernen her er vores arduino uno, som står for at holde styr på robottens sensorer (sonar sensor, fotomodstande)  og aktuatorer (jævnstrømsmotorer). Kroppen er knudepunktet for robottens hjerne og ud til lemmerne, altså breadboardet. Lemmerne er robottens måde at interagere med omverdenen, for denne robot er det at køre, dreje og rotere sonar sensoren.

Overvejelser

Det har været vigtigt at robotten er til at forstå ved syn alene, derfor er den opstillet som anvist. Dette gør det klart helt præcist hvad den kan:

  • Den kan se med sonar sensoren som sidder forrest på robotten da dette viser at den kigger fremad.
  • Til siderne sidder moterene som viser at robotten kan køre,
  • til hver side af breadboardet sidder fotomodstande, altså kan den måle lys
  • og i midten sidder h-broen, altså kan robotten ændre kørselsretning, og dreje.

Det er vigtigt at robotten skal kunne opfatte objekter som står i vejen for den. Dette kan gøres på en af to måder, hvor begge kræver nogle antagelser om robottens omgivelser.

  1. Robotten kigger omkring sig og undgår nærliggende objekter ved at rotere i den modsatte retning ifht hvor objektet blev observeret. 
  2. Robotten kigger ligeud og antager at hvis den blot følger linjen rammer den ingen objekter, dog vil den stoppe op når et objekt fremstår foran den, da dette betyder at der er sat noget foran den, eller at den har nået målet.

Vi valgte nummer 2 da det ikke var klart ud fra opgavebeskrivelsen at robotten skal kunne gebærde sig i miljøer hvori en linje på jorden fører ind i en væg som ikke er målet. Ud fra opgavebeskrivelsen var det illustreret at robotten skal kunne følge en linje som, hvis fulgt fører til mål, uden at snyde robotten med vilje, via blinde veje, eller andet. Derfor blev løsning 2 valgt. Dog fremstår der stadig en steppermotor i robottens konstruktion og kode, da den har muligheden for at dreje sensoren, det bliver dog aldrig gjort.

Komponent teori

Motorer

I robottens konstruktion er der benyttet to jævnstrøms hobby-gearmotorer, altså to motorer som konvertere elektrisk energi til kinetisk energi (bevægelse), disse sidder på hver side af robotten.

At drive motorerne fremad kræver strøm igennem motoren fra positiv til negativ på begge motorer. Baglæns kræver strøm igennem motorerne  fra negativ til positiv. At dreje kræver blot strøm fra positiv til negativ på den ene, og strøm fra negativ til positiv på den anden, dette lader robotten rotere om sin egen akse. Hvor kun at føre strøm gennem den ene motor vil resultere i at robotten roterer omkring det stillestående hjul, lader dette robotten dreje optimalt på en smal bane.

Alt dette gøres ved styring med arduinoen og korrekte input til h-broen, med passende pulsbredde (dette beskrives nærmere i afsnittet om h-broen).

Sensorer

Fotomodstand:

Fotomodstande benyttes som sensorer da disse hæver og sænker deres interne modstand afhængig af det observerede lysindfald på sensoren. Fotomodstande følger grafen nedenfor med hensyn til modstand over lux (lysindfald).

Forholdet mellem en fotomodstand og lysindfald er således, fotomodstanden har en meget høj modstand som udgangspunkt 1M ohm, dette betyder at uden lysindfald er der få elektroner som frit kan bevæge sig og hoppe i et af hullerne i P-type metallet. Når lys rammer fotomodstanden optages en del af lysets fotoner hvilket aktivere elektronerne inde i det aktive lag. Dette lader flere elektroner bevæge sig, altså at hoppe i et af hullerne i P-type metallet. Desto mere lys som falder på fotomodstanden desto flere fotoner optages i det aktive lag, og jo lavere bliver modstanden. Konstruktionen af en fotomodstand ses nedenfor

https://www.electronics-notes.com/articles/electronic_components/resistors/light-dependent-resistor-ldr.php

Sonar sensor:

Denne sensor benytter sig af et ping, altså en ultrasonisk lyd som udsendes af den ene side af sensoren, og observeres af den anden side af sensoren, tiden imellem afsending af lyden og observationen heraf giver dig afstanden til objektet i mikrosekunder. Vi ved at lydens hastighed er 340 m/s, hvilket svarer til 29 millisekunder pr centimeter. Derfor kan vi beregne afstanden i cm ud fra den observerede millisekunder imellem udsendelse og observation af lyden, ved følgende:

cm = millisekunder / 29 /2

Vi dividere med 29 for at få afstanden i centimeter, og dividere igen med to, da lyden havde samme hastighed da den blev sendt ud, til den ramte noget, og igen blev observeret, altså er vi kun interesseret i tiden det tog lyden at blive observeret fra den ramte et objekt.

For vores robot bliver denne afstand sammenlignet med den fiktive længde af robotten plus den mindste afstand robotten skal bruge for at kunne dreje omkring sig selv.

I koden ses det at der via sonar sensoren sendes et lavt signal, hvilket sørger for at høj fremstår helt klart, hvilket sendes efter lav, efterfulgt af endnu et lavt, for igen at fremhæve høj.

Pulsein metoden benyttes på arduinoen med parametrene pin for sensorens signal port, og HIGH, dette skyldes at vi ønsker at måle intervallet fra signalet på den givne pin var lavt, til det var højt. Dette måles i millisekunder og kan benyttes til at vurdere afstanden til objektet som vis ovenover.

Andre komponenter

Polariseret Elektrolyt kondensator (Polarized capacitor):

Kendes visuelt da denne blot vil have en kant rundt i skallen.

Som det fremstår på diagrammet nedenfor har en kondensator to tilledninger, en positiv og en negativ. Når denne forbindes til en jævn-spændingskilde vil denne spændingskilde lade strøm løbe igennem kondensatoren, hvis altså den er forbundet positiv til positiv og negativ til negativ med spændingskilden. spændingskilden vil så “lade” den plade som er forbundet til negativ på spændingskilden med elektroner.

Der vil derfor være et overskud af frie elektroner i den positive plade, hvilket tillader at der løber strøm i kondensatoren.

Som det ses på figuren er den negative plade større end den positive, dette resultere i en opladningen tid, hvori kondensatoren skal lades med elektroner i den negative plade førend der løber strøm i kondensatoren. Dette har den effekt at jo større kapacitet kondensatoren har (målt i farad) desto længere opladningstid vil der være. Dette medfører naturligvis også at kondensatoren har en opladningstid hvori den vil afgive sin elektriske ladning. 

Ydermere tillader en kondensator os at stabilisere en strømkilde om nødvendigt, dette gøres da strømkilden jo selv skal oplade kondensatoren, hvorefter kondensatoren vil lade strøm løbe igennem sig. I tilfælde af en en spændingskilde vil yder varierende spænding kan en kondensator stabilisere dette, grundet opladningstiden og den sekventielle afladning af kondensatorens kapacitet.

Dobbel H-bro (L293D):

Denne chip består af 2 * 8pins, herunder:

  • 4 input
  • 4 output
  • 2 enable
    • 2 ground hertil
  • 2 power 
    • med 2 tilhørende ground pins

Strøm løber over chippen via power pins, dette benyttes til at tilkoble den ønskede spændingskilde til at drive vores motore. De fire input tilkobles arduino, som via 5v signal kan åbne og lukke for de tilhørende output, dette betyder at vi kan drive to motere både fremad og baglæns. De to enable pins benyttes til pulse width modulation (PWM) eller pulsbreddemodulation.

Pulsbreddemodulation betyder at vi sender et digitalt signal via vores arduino, som bliver fortolket som et analogt signal af modtageren, altså af h-broen. Et sådant signal er illustreret nedenfor.

Det ses her at de fire signaler er digitale signaler, altså blot høj, og lav som mulige værdier. Det skal dog gøres klart at modtager af signalet skal kende til frekvensen med hvilken signalet er genereret, da signalet ellers mister sin betydning. Ud fra frekvensen kan man opdele hele det modtagne signal i dele altså i mindre perioder, perioderne vil passe 1:1 med frekvensen, og signalet kan nu fortolkes.

Det beregnes hurtigt hvor mange høje værdier ifht lave værdier signalet indeholder, ud fra dette beregnes der en procent som ud fra hvilke modtageren sender den tilsvarende procent strøm ud til de åbne outputs. Eksempelvis, kan modtageren modtage et signal med en frekvens på 10hz, hvori den beregner at der var 4 høje værdier, derfor 6 lave, altså vil det sige at der var 40% høj også kaldet en duty cycle på 40%. Altså vil modtageren tillade 40% af den tilkoblede spændingskilde at løbe igennem sig og ud til output. for en 9V spændingskilde vil det cirka svarer til 3,6V, dog vil vi nok observere mindre, da der vil gå lidt tabt i processen.

Arduino uno afsender med en frekvens på 490hz pr pin og med pins: 3, 5, 6, 9, 10, 11 i stand til at udføre analogWrite over. Parametrene for arduinos analogwrite er pin og værdi fra 0 til 255, hvor nul er 0% duty cycle, og 255 er 100% duty cycle.

I denne robot er vi i stand til at drive motorene op til 215 RPM ved 100% duty cycle, med et par hjul på med radius på 10 cm kan vi benytte følgende for at regne robottens maksimale  bevægelseshastigheden ud:

v = r × RPM × 0.10472

v = 0,1m * 215 * 0,10472 = 2,25148 m/s

(2,25148/1000)*60*60 = 8,1 km/t

Dog vil dette ikke være realistisk at lade robotten køre, da den herunder skulle kunne vurdere lysintensiteten og dreje sig korrekt og undgå at køre ind i noget, hvilket alt sammen er begrænset af arduinoens processor hastighed hvilket er 16Mhz hvilket svarer til 16 millioner cyklusser per sekund. Det skal her nævnes at hver operation kræver et par cyklusser at eksekvere, og jo mere kompleks koden bliver, som flere cyklusser skal der bruges på hver operation. 

Da robotten skal læse sensor værdier og beregne den optimale handling, og derefter eksekvere handlingen vil det sandsynligvis være problematisk at drive den ved 8 km/t. 8,1 km/t svarer ca til 2,215 m/s, hvilket er 221,5 cm/s. Dette er en signifikant afstand at tilbagelægge på et enkelt sekund, især hvis banen er snæver.

Diode:

Vi har benyttet et par dioder i konstruktionen af robottens “krop”, en diode er yderst brugbar når man vil garantere at strømmen udelukkende løber i en retning, dette vil sige at strømmen kan løbe gennem dioden fra anode til katode. Strøm som løber i den modsatte retning vil blive stoppet.

Begrænsningen af strømmen gennem dioden fungerer således:

https://www.fysik7.dk/1080-vi-bruger-elektricitet/1085-ensretning-af-vekselspaending/1085a-sadan-virker-en-diode

Dioden er opbygget af to metaltyper, P-type og N-type, set henholdsvis til højre og venstre i illustrationen ovenover. Da elektronerne bevæger sig fra minus til plus vil elektronerne strømme fra minus og ind imod grænselaget, da de bliver tiltrukket hertil. En elektron som møder et “hul” (plus på illustrationen) vil hoppe ind i hullet, og blive skubbet ud da der bliver tilført nye huller. Dette lader strøm løbe i en retning gennem dioden. Hvis dioden vendes omvendt, således at elektroner bliver indført via plus vil de herunder blive tiltrukket til P-type metallet, og hullerne blive tiltrukket N-type metallet, altså vil hullerne og elektronerne aldrig mødes på grænselaget.

LED:

En lysdiode (LED) er konstrueret ligesom en almen diode, dog med den effekt at når strøm løber over dioden, afgives en del af effekten som lys (afgivelsen af fotoner). Dette betyder at en lysdiode ligesom en almen diode kun tillader strømmen at løbe fra katode (negativ) til anode (positiv). Lys afgives af lysdioden i det at en elektron hopper i et hul på grænselaget, farven af det lys som lysdioden afgiver afhænger af materialet. En rød lysdiode med høj lysstyrke vil består af en del Aluminium gallium indiumphosphid (AlInGap) hvilket blot er et halvleder metal, hvor halvleder metallet afgiver den røde farve når strøm løber gennem den. 

I vores robot er den benyttet en rød lysdiode til, at indikere når objekter er for tæt på robotten, dette giver et klart visuelt signal for robottens opfattelse af omgivelserne.

For ikke at beskadige lysdioden benyttes der en formodstand på 150 ohm, beregnet efter følgende formel:

R = (Vs – Vled) / Iv

Modstanden i formodstanden er lig, spændingen fra spændingskilden, minus den ønskede spænding over lysdioden, divideret med den ønskede strømstyrke målt i ampere.

R = (5 – 2) / 0,02 = 150 ohm

Dette kommer af databladet for en rød lysdiode, her beskrives det at en sådan lysdiode fungere ved en spænding på mindst 2v og med en strømstyrke 20 mA (milli ampere).

Opsætning

Nu hvor vi er bekendt med komponenterne i brug, og teorien bag, bør det være klart hvorfor vi benytter lige netop disse.

Opsætningen af robotten gøres således:

  1. Breadboardet pladseres, og h-broen (L293D) sættes i midten da denne er kernen i robottens aktuatorer.
    1. Pin enable1&2 føres, som beskrevet tidligere, til arduino uno PWM pin, dette er en pin med et ~ tegn efter tallet.
    2. det samme gøres med pin enable3&4 dog til en anden PWM pin med ~ tegnet på arduinoen. Du er nu klar til at sende et analogt signal til h-broen!
    3. Nu skal der opsættes input og output for h-broen
      • De to power pins 1 og 2 tilføres strøm fra vores 9V batteri.
      • Input 1, 2, 3, og 4 skal forbindes til hver sin pin på arduinoen og skal slutte på samme bane som input pins for h-broen.
      • Output pins fra h-broen tilføres motorene, dvs output pin 1 og 2 fører til det højre hjuls motor, hvor output pin 3 og 4 føres til højre hjulsmotoren. For hver motor skal ledningerne løbe således at output 1 og 3 løber til henholdsvis venstre og højre motors positiv, og output 2 og 4 løber til motorenes negativ.
      • De to fire ground pins fra h-broen føres ud i negativ på breadboardet.
      • Der skal nu blot sættes en kondensator på hvert output – ground sæt fra h-broen, således at kondensatorerne står før ledningerne til motorene. Din h-bro bør se således ud, på nuværende stadie.
    4. Der skal nu skrive lidt kode, som kan føre robotten frem og tilbage ved at skrive til moterene. Koden ser således ud:
void setup(){
  //Motor 1
  pinMode(lMotorP, OUTPUT);
  pinMode(lMotorN, OUTPUT);
  
  //Motor 2
  pinMode(rMotorP, OUTPUT);
  pinMode(rMotorN, OUTPUT);
}
	void goForward(float speed){
  digitalWrite(lMotorP, HIGH);
  digitalWrite(lMotorN, LOW);
  digitalWrite(rMotorP, HIGH);
  digitalWrite(rMotorN, LOW);
  
  analogWrite(pinEn12, speed);
  analogWrite(pinEn34, speed);
}

void goBackward(int speed){
  	 digitalWrite(lMotorP, LOW);
 	 digitalWrite(lMotorN, HIGH);
  	 digitalWrite(rMotorP, LOW);
 	 digitalWrite(rMotorN, HIGH);
  
  	 analogWrite(pinEn12, speed);
  	 analogWrite(pinEn34, speed);
}

Som det ses her, angiver vi hvilke pins vi skriver en høj og lav værdi til, hvilket skal gøres forinden der sendes det analoge signal som fortæller h-broen hvor kraftigt den skal drive motorene. Et højt signal på h-broens input med associeret output til positiv betyder at robotten vil bevæge sig fremad. Det modsatte gør sig også gældende. Parameteren “speed” er her en reference til den duty cycle som vi ønsker at sende som signal, altså den procentmæssige høje værdi for analog signalet. Jo større speed, jo højere spænding vil motorene drives ved.

I arduino koden kan motoren nu drives således:

void loop(){
	goForward(100);
	delay(100);
	goBackward(100);
	delay(100);
}

Med denne kode kørende ses det at robotten vil køre frem ad i 100 millisekunder, hvorefter den vil køre baglæns i 100 millisekunder.

At dreje sker på samme vis som frem og tilbage, dog at den ene motor drives fremad mens den anden drives baglæns.

void turnCounterclockwise(int speed){
  digitalWrite(lMotorP, LOW);
  digitalWrite(lMotorN, HIGH);
  digitalWrite(rMotorP, HIGH);
  digitalWrite(rMotorN, LOW);
  
  analogWrite(pinEn12, speed);
  analogWrite(pinEn34, speed);
}
  1. Nu hvor robotten kører, ønsker vi at kunne følge stregen på jorden, de stregen er en kontrast farve ifht hvid, kan vi benytte en fotomodstand til at læse en repræsentation af lysindfaldet på denne komponen. Den modstatte værdi, altså 1 – lysindfaldet må være talværdien af hvor tæt fotomodstanden er på den mørke streg. Derfor kan vi skrive den følgende kode, hvilket returnere værdien som et decimal tal fra 0 til 1.
float senseDarknessLeft(){
 	return 1 - (analogRead(A0) / 1000.0); 
}

float senseDarknessRight(){
 	return 1 - (analogRead(A1) / 1000.0); 
}

Hver foto modstand opsætte som set herunder, med en diode forinden indtaget fra 5v fra arduinoen, med stregen imod fotomodstanden, hvilket sikrer at der kun løber strøm fra positiv til negativ. Der påsættes en pull-down modstand altså en 1k ohm modstand, som leder til negativ. Der kan nu læses lysforholdet ved en ledning forbundet til arduino pins og ud til fotomodstanden før pull-down modstanden.

Pull-down modstanden sikrer at vi kan måle signalet med arduinoen uden støj. Det er vigtigt at ground her fører tilbage til arduinoens ground, og ikke til batteriets ground.

Denne procedure gentages for den modsatte side af robotten, således at der er to fotomodstande forbundet til arduinoen, en i hver side af robotten, så vi kan sigte efter at kører med stregen lige i midten under robotten.Sonar sensoren forbindes til arduinoens 5v til sensores 5v pin, og ground til arduinoens ground pin. Signal pin forbindes til en vilkårlig arduino digital pin.

For at benytte sonar sensoren til at måle afstand til et objekt foran denne sensor, skal udføre det som kaldes et “Ping” altså det at sende et signal ud og modtage det når det opfanges igen på vej tilbage. Når et sådant signal returnere betyder det, at signalet har ramt et solidt objekt, og er blevet skudt tilbage.

  1. Koden for at udføre et ping er således:
bool somethingTooClose(){
  pinMode(pingPin, OUTPUT);
  digitalWrite(pingPin, LOW);
  delayMicroseconds(2);
  digitalWrite(pingPin, HIGH);
  delayMicroseconds(5);
  digitalWrite(pingPin, LOW);

  pinMode(pingPin, INPUT);
  duration = pulseIn(pingPin, HIGH);
}

Det ses her at vi sender tre signaler, et lavt, som sikrer at sensoren er “ren” altså at der ikke er noget støj. Derefter sender vi et højt, altså sender vi pinget ud. Vi følger det op med et lavt igen, for at sikre os at der igen ikke opstår støj. “duration” er her tiden i millisekunder fra ping blev sendt til det bliver modtaget i pulseIn metoden, som her venter på det høje signal, på den rette pin.

Vi kan benytte tiden i ms til at omregne det til cm via denne funktion (se yderligere forklaring af denen i sensor afsnittet).

long microsecondsToCentimeters(long microseconds) {
  return microseconds / 29 / 2;
}

Vi kan derefter tilføje disse to linjer til somethingTooClose() metoden.

cm = microsecondsToCentimeters(duration); 

return ((int)cm <= (length + minDist));

Dette sikrer at vi altid returnere et konkret svar baseret på størrelsen af robotten og objektets afstand.Det samme gøres med stepper motoren.

Vi er nu blot interesseret i at kigge omkring, med stepper motoren, denne vil rotere sonar sensoren såled at vi kan måle afstanden til objekter til hver side af robotten.

  1. Koden for at rotere stepper motoren ser således ud:
void look(int angle){
  if(angle > 0){
   	servo.write(angle);                 
    	delay(150);   
  }else{
    	servo.write(0);
  }
}

Her beder den blot om en parameter hvilket er en vinkel hvortil robotten skal kigge, og måle afstand til eventuelle objekter.

  1. Du har nu konstrueret den komplette robot, og opstillet al den kode som er nødvendig for at drive robotten, og kontrollere den. Der mangler blot en ting, og det er at opstille det korrekte loop, for den komplette robot. Loopet skal agere som et styresystem, eller som hjernen for robotten, herunder skal den indhente data fra sine sensorer og udføre den passende handling baseret på disse input. Et sådan loop kan se således ud:
void loop()
{
  if(!somethingTooClose()){
    digitalWrite(7,LOW);
    if((senseDarknessLeft() - senseDarknessRight()) - tol > 0){
      	turnCounterclockwise(turnSpeed);
    }
    if((senseDarknessRight() - senseDarknessLeft()) - tol > 0){
      	turnClockwise(turnSpeed);
    }if(getLightDifference() < tol){
      	goForward(speed - (getLightDifference()*60)); 
    }
  }else{
    digitalWrite(7,HIGH);
    Serial.println("GOALLLL!");
    stand();
  }
}

Her ses det at robotten kører indtil den opfanger et objekt for tæt på, hvilket ifølge opgavebeskrivelsens illustration af en bane, vil være målet, den retter også sig selv til efter stregen på gulvet. 

Der er blot tilføjet en “tol” variabel, hvilket er den procentmæssige forskel der må tolereres på læsningen af de to fotomodstande.

Der er også tilføjet en getLightDifference() funktion, essensen af dette, er blot at sænke duty cyklussen for det drivende signal til motorene alt efter forskellen på målingerne på de to fotomodstande. Dette sikrer at robotten kører langsommere og skævere den kører for linjen, hvilket giver den en større chance for at rette sig til, inden den kører alt for skævt.

Problemer

Det har været problematisk at opstille motorene med den nye h-bro komponent, da den her først skulle forstås, og derefter opsættes helt korrekt med korrekt ledningsføring. 

Derudover har analogwrite givet problemer da der først skulle erfares at ikke alle pins kan benyttes til PWM.

Derudover har kondensatorerne voldt problemer da der her skulle vælges en passende kapacitans hvorved det ikke tager for lang til at aktivere, og ikke aflader for længe. Formålet med at indføre disse kondensatorer var at stabilisere output til motorene fra pulsbredde modulationen, således at motorene roterer mere stabilt over længere til og forårsager mindre variation i RPM.

Den fulde kode

Flow diagram

Kode

#include 

//Servo motor
Servo servo;

int pos = 0; //initial position of the stepper motor
long duration, cm;

int pingPin = 4; //Sonar sensor pin

//Enable pins:
int pinEn34 = 6; //Enable pin for output 3 and 4
int pinEn12 = 11; //Enable pin for output 1 and 2

//Motor pins:
int rMotorP = 12; //Positive side input of the right motor
int rMotorN = 13; //Negative side input of the right motor
int lMotorP = 9; //Positive side input of the left motor
int lMotorN = 10; //Negative side input of the left motor


int length = 50; //The length of the robot
int minDist = 50; //The distance from the robot within which we want it to react
int rotTol = 30; // The amount of time it is allowed to turn in one direction giving up
int speed = 100; //The duty cycle written to the motors, regulates speed goes from 0 to 255
int turnSpeed = 10;
float tol = 0.80;

void setup()
{
  servo.attach(2); // Assign servo to pin
  servo.write(0); //Prevents the servo from drifting
  
  pinMode(7, OUTPUT);

  //Photoresistors
  pinMode(A0, INPUT); //LDR1
  pinMode(A1, INPUT); //LDR2
  
  //Sonar sensor
  pinMode(pingPin, INPUT);
  
  //Motor 1
  pinMode(lMotorP, OUTPUT);
  pinMode(lMotorN, OUTPUT);
  
  //Motor 2
  pinMode(rMotorP, OUTPUT);
  pinMode(rMotorN, OUTPUT);
  
  Serial.begin(9600);
}

void loop()
{
  if(!somethingTooClose()){
    digitalWrite(7,LOW);
    //Serial.println(senseDarknessLeft() - senseDarknessRight());
    if((senseDarknessLeft() - senseDarknessRight()) - tol > 0){
      //Left reading was largest
      //Go counterclockwise 
      turnCounterclockwise(turnSpeed);
    }
    if((senseDarknessRight() - senseDarknessLeft()) - tol > 0){
      //Right reading was largest
      //Go clockwise 
      turnClockwise(turnSpeed);
    }if(getLightDifference()  0){
    servo.write(angle);                 
    delay(150);   
  }else{
    servo.write(0);
  }
}

//This code section is gotten from a public arduino demo project using the sonar sensors
bool somethingTooClose(){
  // establish variables for duration of the ping,
  // and the distance result in inches and centimeters:
  int duration, cm;

  // The PING))) is triggered by a HIGH pulse of 2 or more microseconds.
  // Give a short LOW pulse beforehand to ensure a clean HIGH pulse:
  pinMode(pingPin, OUTPUT);
  digitalWrite(pingPin, LOW);
  delayMicroseconds(2);
  digitalWrite(pingPin, HIGH);
  delayMicroseconds(5);
  digitalWrite(pingPin, LOW);

  // The same pin is used to read the signal from the PING))): a HIGH
  // pulse whose duration is the time (in microseconds) from the sending
  // of the ping to the reception of its echo off of an object.
  pinMode(pingPin, INPUT);
  duration = pulseIn(pingPin, HIGH);

  // convert the time into a distance
  cm = microsecondsToCentimeters(duration);
  
  return ((int)cm <= (length + minDist)); //If something is too close
}


long microsecondsToCentimeters(long microseconds) {
  // The speed of sound is 340 m/s or 29 microseconds per centimeter.
  // The ping travels out and back, so to find the distance of the
  // object we take half of the distance travelled.
  return microseconds / 29 / 2;
}

//Rotates the robot around its own axis
void turnClockwise(int speed){
  digitalWrite(lMotorP, HIGH);
  digitalWrite(lMotorN, LOW);
  digitalWrite(rMotorP, LOW);
  digitalWrite(rMotorN, HIGH);
  
  analogWrite(pinEn12, speed);
  analogWrite(pinEn34, speed);
}

//Rotates the robot around its own axis
void turnCounterclockwise(int speed){
  digitalWrite(lMotorP, LOW);
  digitalWrite(lMotorN, HIGH);
  digitalWrite(rMotorP, HIGH);
  digitalWrite(rMotorN, LOW);
  
  analogWrite(pinEn12, speed);
  analogWrite(pinEn34, speed);
}

//Tells the robot to move forwards
void goForward(float speed){
  digitalWrite(lMotorP, HIGH);
  digitalWrite(lMotorN, LOW);
  digitalWrite(rMotorP, HIGH);
  digitalWrite(rMotorN, LOW);
  
  analogWrite(pinEn12, speed);
  analogWrite(pinEn34, speed);
  
}

//Instructs the robot to move in reverse
void goBackward(int speed){
  digitalWrite(lMotorP, LOW);
  digitalWrite(lMotorN, HIGH);
  digitalWrite(rMotorP, LOW);
  digitalWrite(rMotorN, HIGH);
  
  analogWrite(pinEn12, speed);
  analogWrite(pinEn34, speed);
}

//Tells the robot to stand perfectly still
void stand(){
  digitalWrite(lMotorP, LOW);
  digitalWrite(lMotorN, LOW);
  digitalWrite(rMotorP, LOW);
  digitalWrite(rMotorN, LOW);
  analogWrite(pinEn12, 0);
  analogWrite(pinEn34, 0);
  look(0);
}

Kommentarer

Som det ses af denne linje, hvor hastigheden bliver sat når robotten skal køre fremad,

goForward(speed – (getLightDifference()*60)); 

Så er hastigheden variabel, det skal forstås således at robotten sænker hastigheden jo mere den kommer ud af kurs med linjen, dette øger sandsynligheden for at den vil ramme linjen senere, da hastigheden for robotten afgør hvor langt den kører førend den næste gang måler linjens position ifht sig selv. Ved at sænke hastigheden sørger vi for at robotten retter sig mere præcist ifht linjen over en kortere afstand. Når så robotten har rettet sig ind, og står fint for linjen igen, vil den øge hastigheden.

Ydermere er der angivet en variable tol som er den tolerance der er for differencen i målingerne fra fotomodstandene førend robotten vil tage handling og rotere efter at optimere sin position. Dvs. at robotten venter på at den er en vis procent skævt placeret ifht linjen før den retter sig ind efter målingerne. Dette sikrer at den vil komme hurtigere i mål, og jo tolerance den har for forskellen på målingerne jo længere vil den køre hurtigt, hvorefter den vil begynde at dreje.

Denne løsning kommer fra erfaringer fra første porteføljeopgave, hvori vores målinger fra fotomodstande varierede meget ifht hinanden, hvilket gjorde det svært for robotten på det tidspunkt at vælge en retning at rotere, da de begge var lige attraktive på hvert deres tidspunkt, og hurtigt skiftende. Tolerancen løste problemet der, og det løser samme problem igen.

Simulationen

Vi har benyttet unity til at simulere robotten i de forventede omgivelser, dette afsnit dokumenterer dette forsøg, og resultatet heraf.

Opbygningen

Opbygningen af robotten skete i et Unity baseret projekt. Der var i projektet allerede konstrueret nogle dele, inklusiv medfølgende scripts, som skulle gøre det muligt for os at bygge robotten forholdsvist simpelt i unity, når vi nu pga COVID-19 ikke har mulighed for at møde op fysisk og gøre dette. Robotten består hovedsageligt af af seks dele, chassiset, hjulene, servoen, linje sensoren, afstandsmåleren og arduinoen. I unity projektet er der dog et “breadboard” som binder tingene sammen for os.

Koden

Koden skulle indsættes i ArduinoMain.cs som skulle formå at emulere en arduino 1:1. ArduinoMain har nogle funktioner fra arduinoen tilføjet, så man undgår at skulle ændre for meget i koden for at få den til at virke i unity, som den ville i virkeligheden, på en arduino.

public Breadboard breadboard;
    public Servo servo;
    //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.


    int lMotorP = 0;
    int lMotorN = 1;
    int rMotorP = 2;
    int rMotorN = 3;
    int pingPin = 6;

    int length = 3; //The length of the robot
    int minDist = 10; //The distance from the robot within which we want it to react
    int speed = 100; //The duty cycle written to the motors, regulates speed goes from 0 to 255
    int turnSpeed = 38;
    int turn = 0;
    float tol = (float)10.0;
    bool passed = false;
    bool onTrack = true;

    IEnumerator setup()
    {
        //Your code goes here:
        //set servo to point straight
        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:

        //Debug.Log("Light diff: " + getLightDifference());
        //Debug.Log("Light left: " + senseDarknessLeft());
        //Debug.Log("Light right: " + senseDarknessRight());
        if (onTrack)
        {
                if ((senseDarknessLeft() > senseDarknessRight()) && getLightDifference() > tol)
                {
                    //Left reading was largest
                    //Go clockwise 
                    rotateClockwise(turnSpeed);
                    yield return delay(100);
                }
                else if ((senseDarknessRight() > senseDarknessLeft()) && getLightDifference() > tol)
                {
                    //Right reading was largest
                    //Go counterclockwise 
                    //Debug.Log("turn cc");
                    rotateCounterclockwise(turnSpeed);
                    yield return delay(100);
                }
                else if ((getLightDifference() < tol + 1) && !somethingTooClose())
                {
                    //Debug.Log("go forwards");
                    goForward(speed);
                }

            if (somethingTooClose())
            {
                if (passed == false)
                {
                    passed = true;
                    goBackward(speed);
                    yield return delay(400);
                    rotateCounterclockwise(speed);
                    yield return delay(1600);
                    goForward(speed);
                    yield return delay(1000);
                    rotateClockwise(speed);
                    yield return delay(600);
                    goForward(speed);
                    yield return delay(800);
                    rotateClockwise(speed);
                    yield return delay(830);
                    goForward(speed);
                    yield return delay(600);
                    rotateCounterclockwise(speed);
                    yield return delay(900);
                    goBackward(speed);
                    yield return delay(500);
                    rotateCounterclockwise(speed);
                    yield return delay(600);

                    stand();
                }
            }
        }

        if (onTrack && ((senseDarknessLeft() < 200) && (senseDarknessRight() < 200)))
        {
            Debug.Log("offtrack");
            stand();
            onTrack = false;
        }

        if (!onTrack)
        {
            if (!somethingTooClose())
            {
                goForward(speed);
                yield return delay(100);
            }
            if (somethingTooClose())
            {
                stand();
                look(180);
                yield return delay(1200);
                Debug.Log(somethingTooClose());
                if (!somethingTooClose())
                {
                    rotateClockwise(speed);
                    look(90);
                    yield return delay(1450);
                }
                Debug.Log(somethingTooClose());
                if (somethingTooClose())
                {
                    Debug.Log("venstre");

                    look(0);
                    yield return delay(2000);
                    if (!somethingTooClose())
                    rotateCounterclockwise(speed);
                    look(90);
                    yield return delay(2450);
                }
            }
        }


        //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 
    }

    //Gets the absolute difference in light over the two light dependent resistors
    float getLightDifference()
    {
        return abs((long)((analogRead(4)) - (analogRead(5))));
    }

    //Robot must follow a dark line on a light background
    //Use this to measure the darkness using the LDR
    float senseDarknessLeft()
    {
        return (float)(analogRead(4));
    }
    //Senses the level of darkness on the right side of the robot
    float senseDarknessRight()
    {
        return (float)(analogRead(5));
    }

    //Looks to a certain degree of rotation
    void look(int angle)
    {
        if (angle > 0)
        {
            servo.write(angle);
            delay(150);
        }
        else
        {
            servo.write(0);
        }
    }

    //This code section is gotten from a public arduino demo project using the sonar sensors
    bool somethingTooClose()
    {
        // establish variables for duration of the ping,
        // and the distance result in inches and centimeters:
        int cm;
        ulong duration;
        duration = pulseIn(pingPin);

        // convert the time into a distance
        cm = (int)microsecondsToCentimeters((long)duration);

        if (cm == 0)
        {
            return false;
        }

        return (cm <= (length + minDist)); //If something is too close
    }


    long microsecondsToCentimeters(long microseconds)
    {
        // The speed of sound is 340 m/s or 29 microseconds per centimeter.
        // The ping travels out and back, so to find the distance of the
        // object we take half of the distance travelled.
        return microseconds / 29 / 2;
    }

    //Rotates the robot around its own axis
    void rotateClockwise(int speed)
    {
        analogWrite(lMotorP, speed);
        analogWrite(lMotorN, 0);
        analogWrite(rMotorP, 0);
        analogWrite(rMotorN, (int)(speed / 1.3));
    }

    //Rotates the robot around its own axis
    void rotateCounterclockwise(int speed)
    {
        analogWrite(lMotorP, 0);
        analogWrite(lMotorN, (int)(speed / 1.3));
        analogWrite(rMotorP, speed);
        analogWrite(rMotorN, 0);
    }

    //Tells the robot to move forwards
    void goForward(int speed)
    {
        analogWrite(lMotorP, speed);
        analogWrite(lMotorN, 0);
        analogWrite(rMotorP, speed);
        analogWrite(rMotorN, 0);
    }

    //Instructs the robot to move in reverse
    void goBackward(int speed)
    {
        analogWrite(lMotorP, 0);
        analogWrite(lMotorN, speed);
        analogWrite(rMotorP, 0);
        analogWrite(rMotorN, speed);
    }

    //Stops the robot
    void stand()
    {
        analogWrite(lMotorP, 0);
        analogWrite(lMotorN, 0);
        analogWrite(rMotorP, 0);
        analogWrite(rMotorN, 0);
    }

Problemer

Der var i starten problemer med at koden nogle gange af uforklarlige årsager crashede unity helt, hvilket gjorde det meget svært at fejlsøge og rette i koden, dette betød at koden blev ændret lidt og derfor afviger fra vores tinkerCad kode. Dertil kommer at unity opførte sig underligt, det virkede som om at robotten sad fast når den skulle bakke, der er et problem med det “free rolling wheel”, det er nemlig det der skaber en stor modstand, hvilket viste sig at være fordi at hjulet anvendte et fixed joint i stedet for et configurable joint. Alt i alt var det sværeste primært at få unity til at opføre sig, som ønsket. Men efter utallige crashes og lang tids fejlsøgning, lykkedes det endelig.

Videoen

Som set i videoen er vores rekordtid omkring 40 Sekunder.

Konklusion

Robotten som konstrueret i Tinkercad havde nogle uforudsete problemer da denne opbygning of program blev placeret i den simulerede verden. Det blev herunder klart, at banen indeholdte forhindringer som robotten ikke kunne omgås. Dette indebærer et stykke hvor linien på jorden fører ind i en væg, og et stykke af banen hvor der ikke er optegnet nogen linje. Dette voldte naturlige problemer da Tinkercad konstruktionen baserede sig på fundamentale antagelser draget ud fra opgavebeskrivelsen, disse værende primært at robotten blot skulle følge en linje på gulvet, og undgå vægge.

Koden for Unity simulationen ses ovenover og afviger i visse aspekter fra den angivet for Tinkercad konstruktionen. Den primære afvigelse er den, at robotten benytter sonar sensoren til at se omkring sig, for at indstille sin krop i den rette vinkel for at køre, uden at ramme ind i objekter (mere end højst nødvendigt).

Det må konkluderes at simulationen opfylder målet for opgaven, da den her kommer i mål, og benytter passende sensorer og styresystem, til at omgås omgivelserne på bedste vis.

Konstruktionen i Tinkercad, opfylder opgavebeskrivelsens mål, for sensor-aktuator forholdene, da den her vil køre efter sine indtryk af omverdenen. Dog har den huller, som det, at den ikke tager højde for eventuelle fælder på banen, som stykker uden linje på gulvet, eller linjer der fører ind i en væg. Det skal dog siges at konstruktionen i Tinkercad styres ligesom den i Unity, og derfor vil kunne håndtere ændringerne i kodebasen.

Forbedringer

Tinkercad:

Helt ideelt vil robotten orientere sig ifht alle objekter omkring den, det vil sige at den skulle rotere sin sonar sensor 180 grader frem og tilbage, for at holde maksimal afstand til alle objekter.

optimalt ville robotten også udnytte en søgealgoritme til at finde vej, uden at køre i ring, det kunne gøres ved eksempelvis A*. Da denne vil inkludere en heuristik funktion, som gør nye stier mere attraktive end stier robotten allerede har besøgt. 

Hastigheden for robottens kørsel kan altid redigeres, dette er jo blot en enkelt variable som enstemmigt styrer duty cyklussen altså den procentmæssige tid hvori robottens h-bro modtager et højt signal ifht et lavt signal. Som beskrevet i tidligere afsnit vil robotten på nuværende tidspunkt kører ca 8 km/t med hjul på 10 cm i radius, hvilket er en forholdsvis høj hastighed, men muligvis også for store hjul, det er svært at vurdere uden en testkørsel i den virkelige verden, med en fysisk robot.

Unity:

Det er klart ud fra videoen at styringen af robotten ikke er ideel, det at robotten skal stoppe op for at dreje, sænker dens rekordtid dramatisk. Robotten kunne ideelt holder sig opdateret på objekter løbende ved at rotere sonar sensoren konstant og mappe indtrykkene til et indre kort over omverdenen. Robotten kunne også benytte en passende søgealgoritme A* eller lignende til bedre at vurdere den korrekte og hurtigste handling for bedst at når målet, hvilket vil hæve vores rekordtid markant.

Leave a Reply