Mads Kildegaard Kristjansen & Lucas Brynk Jensen

I denne tutorial vil vi gennemgå hvordan man bygger en robot, som ved hjælp af diverse sensorer kan køre efter en streg på gulvet indtil stregen ophører, hvorefter robotten vil køre efter lydbølger for at undgå mulig kollision. Robotten i denne tutorial er navngivet RoboSchneider (efter skuespilleren) og den er udarbejdet af Lucas Brynk og Mads K. Kristjansen. Det færdige produkt, samt forventninger til produktets opførelse kan ses på nedenstående videoer.

Video af testkørsel i Unity
Video af test i TinkerCAD

Vi starter med at åbne TinkerCAD. For at kunne forstå hvad der sker med den færdige robot i Unity, er vi nødt til at opbygge RoboSchneider i TinkerCAD først. De nødvendige elementer kan ses på nedenstående billede samt en liste med navne på elementerne.

Komponentlist:

  • Breadboard x2
  • Arduino Uno
  • 9v Batteri
  • DC Motor x2 [H16930]
  • Micro Servo [SG90]
  • Ultrasonic Distance Sensor [HC-SR04]
  • H-Bridge Motor Driver [L293D]
  • 5C Regulator [LM7805]
  • 100 ohm Resistor x2
  • 10kOhm Resistor x2
  • LED-Resistor [2542 Standard White LED] x2
  • LDR-Resistor [f.eks NORPS-12]
  • Slideswitch

Hvordan selve elementerne bliver sat i forhold til hinanden er stort set lige meget i dette projekt, da TinkerCAD modellen ikke skal bruges andre steder. For forståelsens skyld opfordres der til at opsætte elementerne som skulle man bygge en “bil”.

Første skridt er at sætte 9V batteriet til breadboardet så vi kan få strøm rundt i kredsløbet. Lad plus køre igennem en sliderswitch for at kunne slukke og tænde for RoboSchneider.

Det første breadboard bliver vores H-bridge board. Sæt L293D H-Bridge (H-bro) fast i midten af breadboarded og forbind de to motorer på hver side af H-broen. Grunden til at anvende en H-bro er for at styre strømmens polaritet gennem motorerne. Dette gør at vi kan bestemme hvilken vej hjulene skal dreje. For den ene motor vil vi lade Input 1 og 2 fra H-broen gå til pin 10 og 11 på arduinoen. Man kan bruge alle digital pins til dette, men da Arduino Uno er begrænset i antallet af Digitale pins (især PWM pins), og vores servo kommer til at optage pin 9 eller 10, så er vi nødt til at bruge hvad der er tilbage. Ligeledes for den anden motor vil køre Input 3 og 4 fra H-broen hen til pin 5 og 6 på arduinoen.

På første breadboard er der lidt plads tilbage, nede i den anden ende, til at sætte en 5V regulator [LM7805]. Fra 5V Regulatorens output trækker vi en plus og minus videre til næste breadboard. Fra andet breadboard starter vi med at trække en plus og en minus til hhv Arduinoens 5V pin og GND pin for at kunne gøre Arduino Uno uafhængig af USB-forbindelsen til en PC.

Andet breadboard bliver LDR-LED boarded. Her sætter vi 2 LDR-resistorer samt 2 modstande. Modstandene skal være på 10 kOhm grundet følgende beregninger:

Da vi kender Ohms lov kan vi stille den om og finde den ønskede modstandsværdi ved den benyttede spænding. Hertil skal det siges at vi antager fra tidligere projekt at LDR-modstandene ved maksbelysning er 20-230kOhm, samt ved totalt mørke, vil være 2MOhm.

Vout = Vin * (R2/R1+R2)

Ved at sætte den valgte modstand ind, samt indsætte de kendte værdier, kommer formlen til at se sådanne ud ved 20kOhm, 230kOhm og 2MOhm:

5V * (20000/20000+10000) = 3,33V
5V * (230000/230000+10000) = 4,79V
5V * (2000000/2000000+10000) = 4,98V

Lad LDR-resistorerne køre til hhv. pin A2 og A3. (Du kan i princippet benytte enhver A pin i dette projekt, det er dog kun indgangene markeret med ‘A’ der kan bruges her). Grunden til at vi benytter pins markeret med ‘A’ for LDR-resistorerne, er fordi vi her ikke får et digitalt signal, men et analogt signal, det vil sige at det ikke bliver bearbejdet på samme måde som Digitale signaler. Hertil skal det også siges at det signal vi får fra LDR-resistorerne bliver konverteret i Arduinoen fra Volt til en målbar enhed (altså værdien fra 0-1023).

Sæt herefter 2 LED-resistorer direkte til 5V med en modstand imellem. Størrelsen af modstanden vælges ud fra den givende LED’s data. LED-resistor Data kan findes i et Datablad. Da dette projekt er virtuelt har vi ikke ægte LED-modstande at undersøge, derfor må vi gå ud fra at vi har at gøre med en standard hvid LED 5mm (3542serialnumber). Hvis man undersøger en sådanne LED-modstand i et datablad vil man finde ud af at den maximalt kan håndtere 25mA (dog kun 20mA i TinkerCAD) før det begynder at gå ud over levetiden på LED-modstanden, og da den bliver tilført 5V (minus spændingsfaldet på 3,1V) kan vi regne os frem til, med hjælp fra Ohms lov, at en 100 Ohms modstand vil være at fortrække i TinkerCAD.

U=R*I
R=U/I 
R=(5-3,1)/0,02 = 95 Ohm 

Vi runder op således vi anvender en 100 Ohms modstand. 

For en LED udenfor TinkerCAD, som max håndterer 25mA, sætter man 0.025 ind i formlen og får dermed en beregnet modstand på 

R=(5-3,1)/0,025 = 76 ohm (rundet op til 80 ohm) 

Beregningerne er lavet ud fra data fundet på følgende hjemmeside: (https://www.kitronik.co.uk/blog/led-resistor-value-calculator/

Nu skal Micro Servo’en sættes til. Her sætter vi Power samt GND til LDR-LED Breadboardet. Micro Servo’ens Signal skal køre til pin 9. Den skal altid sættes til enten pin 9 eller 10, da det er de eneste to pins der er registreret i Arduinosprogets Servo.h bibliotek som er integreret i TinkerCAD. På en fysisk prototype undenfor TinkerCAD kan man bruge enhver PWN pin til Servoen. Måden vi fandt ud af dette på, var ved at den venstre motor stoppede med at fungere korrekt, samt højre motor kørte med fuld hastighed når andre end pin 9 eller 10 blev benyttet. Servoen kørte heller ikke korrekt. Efter lidt debugging (test af systemet slavisk) fandt vi frem til at det måtte være et problem som er opstået efter servoen blev implenteret. Hertil måtte vi ind og se i dokumentationen i Servo.h biblioteket, hvor vi fandt ud af at pin 9 og 10 er de eneste som er supporteret og integreret i Servo.h biblioteket til TinkerCAD.  

Ultrasonic Distance Sensor’s TRIG, og ECHO skal køre til henholdsvis pin 3 og pin 12 (TRIG skal altid forbindes til en PWM digital pin, og ECHO skal altid forbindes til en normal digital pin). Ultrasonic Distance Sensor’s Power og GND skal også kører hen til LDR-LED Breadboardet. Hvis systemet er opsat korrekt burde det se nogenlunde ud som på billedet nedenfor.

Et PWM-signal er et signal som ikke kun kan gå fra tændt til slukket (0-1), men har et spektrum fra 0 og op til en given værdi. Et PWM-signal fungerer lidt som en kurve. Dette kan anvendes til eksempelvis en motor, for at få en blød opstart og/eller en blød nedbremsning. Dette kan også bruges i servoen, hvor vi ikke vil have servoen til at køre hele vejen rundt, men gerne vil have en til at stille sig i et bestemt punkt hvortil den bliver stående. Dette bliver også brugt på Ultrasonic Distance Sensor hvor vi ikke kun kan nøjes med et signal der går fra 0-1, men har brug for at kende den nøjagtige værdi der bliver givet fra komponentet, altså Distance sensoren.

TinkerCAD Code 

I vores Arduino kode, som kan ses forneden, vil vi først og fremmest declare(erklære) vores variabler vi gør brug af igennem programmet. En vigtig ting at have i mente er, at når man vil gøre brug af en servo, så skal man huske at anvende komponentens bibliotek via #include <Servo.h>. Uden dette vil der opstå fejl i programmet. Foruden vores variabler anvender vi også to bools, der agerer som “triggers” for at programmet ikke springer over nogle af vores funktioner. Inde i vores setup() angiver vi vores pins og servo så vi kan bruge dem i programmet, og sætter vores motorer i en stillestående tilstand. 

#include <Servo.h>
//declaring the names for the pins
//Motor
const int RightPlus = 11;
const int RightMinus = 10;
const int LeftPlus = 6;
const int LeftMinus = 5;

//Distance Sensor
const int TRIGPin = 3;
const int ECHOPin = 12;


//Declare the needed variables
int left = A5;
int right = A4;
int sensorValue1;
int sensorValue2;
int minDiff = 50; //Maximum allowed difference between left and right sensor values

bool onLine = true;
bool firstDrive = false;

int dis = 100;
long tid;
int distance;
int servoLeft = 0;
int servoFront = 0;
int servoRight = 0; 

Servo theServo;

//Setup what needs to be stated before the robot begins its Loop
void setup()
{
  pinMode(RightPlus, OUTPUT);//right plus
  pinMode(RightMinus, OUTPUT);//right minus
  pinMode(LeftPlus, OUTPUT);//left plus
  pinMode(LeftMinus, OUTPUT);//left minus
  
  pinMode(TRIGPin, OUTPUT); // Sets the trigPin as an OUTPUT
  pinMode(ECHOPin, INPUT); // Sets the echoPin as an INPUT
  
  theServo.attach(9); // The Servo
  
  //Make sure motors are off
  digitalWrite (RightPlus, LOW);
  digitalWrite (RightMinus, LOW);
  digitalWrite (LeftPlus, LOW);
  digitalWrite (LeftMinus, LOW);
  
  Serial.begin(9600);
  
}

I vores loop(), hvor hele programmet egentlig kører, starter vi med at aflæse værdierne fra vores to LDR-modstande. Disse værdier bruger vi til at vurdere om RoboSchneider følger linjen, eller om vi skal rette kursen. Her har vi også vores første bool (onLine), som sørger for at denne del af programmet kun kører imens RoboSchneider faktisk kører på linjen.

void loop()
{
  //Assigning values to var's
  sensorValue1 = analogRead(left);
  sensorValue2 = analogRead(right);
  
  int diff = abs(sensorValue1 - sensorValue2);
  
  //"If" robot is following the ground-line; Do this:
  if (onLine == true)
  { ...

Værdierne fra LDR-modstandene angiver det lys der bliver reflekteret fra ”gulvet”. LDR-modstandene har en værdi mellem 0-1023. Denne værdi er målt som mængden af strøm der passerer igennem LDR-modstanden, i forhold til hvor stor en modstand der udvikles i selve LDR-modstanden. Det vil med andre ord sige, at det hvide gulv reflekterer meget lys, og opfanges som en høj værdi, og den sorte streg reflekterer lidt lys, der opfanges som en lav værdi.

Vi bruger værdierne fra vores LDR-modstande til at bestemme hvilke motor pins der skal tilføres strøm. Dette gør vi ved at angive den pin som er tildelt motoren og indsætter en værdi mellem 0-255, eksempel: analogWrite(pin, speed). Da motoren er tilsluttet Arduinoen via en H-bro giver det 4 pins at holde styr på. Der er en analogWrite(pin,speed); for hver “pol” af de 2 motor; RightPlus, RightMinus, LeftPlus, LeftMinus. Så hvis den ene LDR-modstand måler for lavt, altså måler ind over den sorte streg, så retter vi op på kursen ved at ændre vores motor inputs. 

//"If" robot is following the ground-line; Do this:
  if (onLine == true)
  {
    // If the robot follows the line fine:
    if (diff <= minDiff && sensorValue1 >= 100 && sensorValue2 >= 100)
    {
      	//Driving Forward
        analogWrite(LeftPlus, 100);
        analogWrite(LeftMinus, 0);
        analogWrite(RightPlus, 100);
        analogWrite(RightMinus, 0);
        delay(1000);
      	firstDrive = true;
    }
    //If the robot is turning right, it turns left slightly
    else if (sensorValue1 < sensorValue2)
    {
      	//Turn Right for 1 sec at speed (50/255)
    	analogWrite(LeftPlus, 0);
        analogWrite(LeftMinus, 0);
        analogWrite(RightPlus, 50);
        analogWrite(RightMinus, 0);
      	delay(1000);
    }
        // Vice Versa
    else if (sensorValue2 < sensorValue1)
    {
      	//Turn Left for 1 sec at speed (50/255)
        analogWrite(LeftPlus, 50);
        analogWrite(LeftMinus, 0);
        analogWrite(RightPlus, 0);
        analogWrite(RightMinus, 0);
      	delay(1000);
    }
    ...
  

Vi er lidt påpasselige med vores konstruktions hastighed, og efter noget testning fandt vi frem til nogle passende værdier for vores motor inputs, som gav en stabil performance i vores opstilling. Eksempelvis, hvis RoboSchneider har for meget fart på, uden noget nedbremsning inden svinget, så kan den risikere at køre for hurtigt til at vores sensorer kan opfange og justere i programmet, således den ender med at køre over og væk fra stregen. Vi valgte derfor en lavere og mere passende hastighed for at undgå mulige fejl i adfærden. 

Når vi rammer den første målstreg, hvilket sker når begge LDR-modstande måler under den angivet minimumsværdi, ændrer vi vores onLine bool til at være false for at komme videre til den næste del af programmet. 

...

    //When the robot hits the ending of the ground line it stops and shifts over to "Off-Line" mode 
    else if (sensorValue1 <= 100 && sensorValue2 <= 100 && firstDrive == true)
    {
      	//Motor Stop for 1 sec
        analogWrite(LeftPlus, 0);
        analogWrite(LeftMinus, 0);
        analogWrite(RightPlus, 0);
        analogWrite(RightMinus, 0);
        onLine = false;
      	Serial.print("online is now false");
      	delay(1000);
    }

Den næste del af koden foregår kun efter vi har ramt den første målstreg. Her anvender vi vores Ultrasonic Distance Sensor (afstandsmåler) til at navigere den resterende del af vores opstillede bane. Vi starter med at aktivere pin 3, altså Distance Sensorens TRIG Pin, som sender en puls ud der bliver opfanget igen af Distance Sensoren. Den opfangede værdi er tiden det tager for denne puls at vende tilbage til vores sensor, målt i mikrosekunder. Denne værdi omregner vi til en målbar enhed. Lydens hastighed er 340 m/s, eller 0,034 cm/µs. Vi tager værdien fra afstandsmåleren, ganger den med 0,034, og deler med 2 da vi skal have afstanden til væggen, og ikke fra sensor til væg til sensor igen. Denne værdi gemmer vi i en variabel ”distance” som vi kan bruge til at vurdere afstanden fra sensoren på RoboSchneider og væggen forud.  

      //THIS IS OFF-LINE MODE
        //If the end of the groundline is reached
  if (onLine == false)
  {
    //Distance Sensor Start
    digitalWrite(TRIGPin, LOW);
  	delayMicroseconds(2);
  	digitalWrite(TRIGPin, HIGH);
  	delayMicroseconds(10);
  	digitalWrite(TRIGPin, LOW);
    //Read signal from TRIGPin and store as "tid"
  	tid = pulseIn(ECHOPin, HIGH);
    //Convert "tid" to "distance"
  	distance = tid * 0.034 / 2;
...

Hvis afstanden til væggen foran RoboSchneider ikke er under den angivet minimumsværdi, så kører den bare ligeud indtil den registrerer en genstand inden for minimumsafstanden. Når afstandsmåleren registrerer at vi er under minimumsgrænsen, så stopper vi RoboSchneiders fremgang for at undgå kollision. I praksis ville vi placere vores afstandsmåler ovenpå vores servo således vi kan rotere den og måle i forskellige retninger (dette kan ses på et billede af RoboSchneider længere nede i tutoriallen). 

//if the SonarSensor signal is bigger than the chosen minimum distance it goes forward 
  	if (distance >= dis)
    {
      	//Driving Forward
    	analogWrite(LeftPlus, 100);
        analogWrite(LeftMinus, 0);
        analogWrite(RightPlus, 100);
        analogWrite(RightMinus, 0);
    }
    //if signal is smaller:
    else if (distance <= dis)
    {
      	//Motor Stop
      	analogWrite(LeftPlus, 0);
        analogWrite(LeftMinus, 0);
        analogWrite(RightPlus, 0);
        analogWrite(RightMinus, 0);
  ...

Med servo.write(), i vores eksempel har vi navngivet vores for myServo, så kan man indsætte hvilken vinkel man vil indstille servoen til, eksempelvis 0, 90, eller 180 grader. Så hvis vi vil dreje servoen med 90 grader, så skriver vi i koden: myServo.write(90). Ud fra servoens placering og orientering kan disse værdier variere. I vores eksempel antager vi at 0 grader kigger servoen til venstre, 90 grader kigger den ligeud, og 180 grader til højre. 

//The check consists of 4 steps
        //Step 1 check distance to nearest wall in front of robot
        servoFront = distance;
      	//Turning the Servo to the Right
        theServo.write(180);
        delay(2000);
        //Step 2 check distance to nearest wall left of robot
        servoLeft = distance;
      	//Turning the Servo to the Left
        theServo.write(0);
        delay(4000);
        //Step 3 check distance to nearest wall right of robot
        servoRight = distance;
      	//Turning the Servo Back to facing for forward
        theServo.write(90);
        delay(2000);
...

Vi måler de tre retninger og gemmer dem i hver deres variabel. Disse variabler kan vi bruge til at sammenligne og vurdere hvor der er en væg, og hvor der er fri bane. Afhængig af hvilken retning der er fri, så justerer vi RoboSchneiders retning, hvorefter vi indstiller den til at køre fremad indtil der bliver registreret en ny væg/forhindring. 

//Turn to left wall
        if (servoLeft >= servoFront && servoLeft >= servoRight && servoRight != 0 || servoLeft == 0)
        {
          	//Turning Left (on the spot)
        	analogWrite(LeftPlus, 0);
            analogWrite(LeftMinus, 55);
            analogWrite(RightPlus, 55);
            analogWrite(RightMinus, 0);
            delay(1000);
          	//Stopping
            analogWrite(LeftPlus, 0);
            analogWrite(LeftMinus, 0);
            analogWrite(RightPlus, 0);
            analogWrite(RightMinus, 0);
            delay(1000);
          	//Driving Forward Again
            analogWrite(LeftPlus, 130);
            analogWrite(LeftMinus, 0);
            analogWrite(RightPlus, 130);
            analogWrite(RightMinus, 0);
        }
        //Turn to right wall
        if (servoRight >= servoFront && servoRight >= servoLeft && servoLeft != 0 || servoRight == 0)
        {
          	//Turning Right (on the spot)
        	analogWrite(LeftPlus, 55);
            analogWrite(LeftMinus, 0);
            analogWrite(RightPlus, 0);
            analogWrite(RightMinus, 55);
            delay(1000);
          	//Stopping
            analogWrite(LeftPlus, 0);
            analogWrite(LeftMinus, 0);
            analogWrite(RightPlus, 0);
            analogWrite(RightMinus, 0);
            delay(1000);
          	//Driving Forward Again
            analogWrite(LeftPlus, 130);
            analogWrite(LeftMinus, 0);
            analogWrite(RightPlus, 130);
            analogWrite(RightMinus, 0);
		}
        //Continue forward
        if (servoFront >= servoLeft && servoFront >= servoRight)
        {
          	//Driving Forward
        	analogWrite(LeftPlus, 130);
            analogWrite(LeftMinus, 0);
            analogWrite(RightPlus, 130);
            analogWrite(RightMinus, 0);
          	delay(1000);
        }

Med denne opsætning er RoboSchneider i stand til at køre igennem en bane som starter med en streg i gulvet, som naturligvis skal følges til ende.

Når enden af stregen er nået starter RoboSchneider på at køre efter Ultrasonic Distance Sensor (afstandsmåler) for at navigere igennem resten af banen, til målstregen bliver mødt. Hvis alt er opsat korrekt vil RoboSchneider opføre sig som vist på den første video oppe i toppen af bloggen her på siden.  

Complete TinkerCAD code

#include <Servo.h>
//declaring the names for the pins
//Motor
const int RightPlus = 11;
const int RightMinus = 10;
const int LeftPlus = 6;
const int LeftMinus = 5;

//Distance Sensor
const int TRIGPin = 3;
const int ECHOPin = 12;


//Declare the needed variables
int left = A5;
int right = A4;
int sensorValue1;
int sensorValue2;
int minDiff = 50; //Maximum allowed difference between left and right sensor values

bool onLine = true;
bool firstDrive = false;

int dis = 100;
long tid;
int distance;
int servoLeft = 0;
int servoFront = 0;
int servoRight = 0; 

Servo theServo;

//Setup what needs to be stated before the robot begins its Loop
void setup()
{
  pinMode(RightPlus, OUTPUT);//right plus
  pinMode(RightMinus, OUTPUT);//right minus
  pinMode(LeftPlus, OUTPUT);//left plus
  pinMode(LeftMinus, OUTPUT);//left minus
  
  pinMode(TRIGPin, OUTPUT); // Sets the trigPin as an OUTPUT
  pinMode(ECHOPin, INPUT); // Sets the echoPin as an INPUT
  
  theServo.attach(9); // The Servo
  
  //Make sure motors are off
  digitalWrite (RightPlus, LOW);
  digitalWrite (RightMinus, LOW);
  digitalWrite (LeftPlus, LOW);
  digitalWrite (LeftMinus, LOW);
  
  Serial.begin(9600);
  
}

//The Loop Begins

void loop()
{
  //Assigning values to var's
  sensorValue1 = analogRead(left);
  sensorValue2 = analogRead(right);
  
  int diff = abs(sensorValue1 - sensorValue2);
  
  //"If" robot is following the ground-line; Do this:
  if (onLine == true)
  {
    // If the robot follows the line fine:
    if (diff <= minDiff && sensorValue1 >= 100 && sensorValue2 >= 100)
    {
      	//Driving Forward
        analogWrite(LeftPlus, 100);
        analogWrite(LeftMinus, 0);
        analogWrite(RightPlus, 100);
        analogWrite(RightMinus, 0);
        delay(1000);
      	firstDrive = true;
    }
    //If the robot is turning right, it turns left slightly
    else if (sensorValue1 < sensorValue2)
    {
      	//Turn Right for 1 sec at speed (50/255)
    	analogWrite(LeftPlus, 0);
        analogWrite(LeftMinus, 0);
        analogWrite(RightPlus, 50);
        analogWrite(RightMinus, 0);
      	delay(1000);
    }
        // Vice Versa
    else if (sensorValue2 < sensorValue1)
    {
      	//Turn Left for 1 sec at speed (50/255)
        analogWrite(LeftPlus, 50);
        analogWrite(LeftMinus, 0);
        analogWrite(RightPlus, 0);
        analogWrite(RightMinus, 0);
      	delay(1000);
    }
    
    //When the robot hits the ending of the ground line it stops and shifts over to "Off-Line" mode 
    else if (sensorValue1 <= 100 && sensorValue2 <= 100 && firstDrive == true)
    {
      	//Motor Stop for 1 sec
        analogWrite(LeftPlus, 0);
        analogWrite(LeftMinus, 0);
        analogWrite(RightPlus, 0);
        analogWrite(RightMinus, 0);
        onLine = false;
      	Serial.print("online is now false");
      	delay(1000);
    }
  }
        //THIS IS OFF-LINE MODE
        //If the end of the groundline is reached
  if (onLine == false)
  {
    //Distance Sensor Start
    digitalWrite(TRIGPin, LOW);
  	delayMicroseconds(2);
  	digitalWrite(TRIGPin, HIGH);
  	delayMicroseconds(10);
  	digitalWrite(TRIGPin, LOW);
    //Read signal from TRIGPin and store as "tid"
  	tid = pulseIn(ECHOPin, HIGH);
    //Convert "tid" to "distance"
  	distance = tid * 0.034 / 2;
  
  //if the SonarSensor signal is bigger than the chosen minimum distance it goes forward 
  	if (distance >= dis)
    {
      	//Driving Forward
    	analogWrite(LeftPlus, 100);
        analogWrite(LeftMinus, 0);
        analogWrite(RightPlus, 100);
        analogWrite(RightMinus, 0);
    }
    //if signal is smaller:
    else if (distance <= dis)
    {
      	//Motor Stop
      	analogWrite(LeftPlus, 0);
        analogWrite(LeftMinus, 0);
        analogWrite(RightPlus, 0);
        analogWrite(RightMinus, 0);
    
        //The check consists of 4 steps
        //Step 1 check distance to nearest wall in front of robot
        servoFront = distance;
      	//Turning the Servo to the Right
        theServo.write(180);
        delay(2000);
        //Step 2 check distance to nearest wall left of robot
        servoLeft = distance;
      	//Turning the Servo to the Left
        theServo.write(0);
        delay(4000);
        //Step 3 check distance to nearest wall right of robot
        servoRight = distance;
      	//Turning the Servo Back to facing for forward
        theServo.write(90);
        delay(2000);

        //And step 4
        //Compare the 3 checks and turn to the wall farest away.

        //if the wall at left and the wall at right is not existing, it means the end is reached. The robot will turn right and drive off the ground.
        if (servoLeft == 0 && servoRight == 0)
        {
          	//Turning Right (On the spot)
        	analogWrite(LeftPlus, 55);
            analogWrite(LeftMinus, 0);
            analogWrite(RightPlus, 0);
            analogWrite(RightMinus, 55);
            delay(1000);
          	//Stopping
            analogWrite(LeftPlus, 0);
            analogWrite(LeftMinus, 0);
            analogWrite(RightPlus, 0);
            analogWrite(RightMinus, 0);
            delay(1000);
          	//Driving Forward again
            analogWrite(LeftPlus, 255);
            analogWrite(LeftMinus, 0);
            analogWrite(RightPlus, 255);
            analogWrite(RightMinus, 0);
            delay(1000);
        }
        //Turn to left wall
        if (servoLeft >= servoFront && servoLeft >= servoRight && servoRight != 0 || servoLeft == 0)
        {
          	//Turning Left (on the spot)
        	analogWrite(LeftPlus, 0);
            analogWrite(LeftMinus, 55);
            analogWrite(RightPlus, 55);
            analogWrite(RightMinus, 0);
            delay(1000);
          	//Stopping
            analogWrite(LeftPlus, 0);
            analogWrite(LeftMinus, 0);
            analogWrite(RightPlus, 0);
            analogWrite(RightMinus, 0);
            delay(1000);
          	//Driving Forward Again
            analogWrite(LeftPlus, 130);
            analogWrite(LeftMinus, 0);
            analogWrite(RightPlus, 130);
            analogWrite(RightMinus, 0);
        }
        //Turn to right wall
        if (servoRight >= servoFront && servoRight >= servoLeft && servoLeft != 0 || servoRight == 0)
        {
          	//Turning Right (on the spot)
        	analogWrite(LeftPlus, 55);
            analogWrite(LeftMinus, 0);
            analogWrite(RightPlus, 0);
            analogWrite(RightMinus, 55);
            delay(1000);
          	//Stopping
            analogWrite(LeftPlus, 0);
            analogWrite(LeftMinus, 0);
            analogWrite(RightPlus, 0);
            analogWrite(RightMinus, 0);
            delay(1000);
          	//Driving Forward Again
            analogWrite(LeftPlus, 130);
            analogWrite(LeftMinus, 0);
            analogWrite(RightPlus, 130);
            analogWrite(RightMinus, 0);
		}
        //Continue forward
        if (servoFront >= servoLeft && servoFront >= servoRight)
        {
          	//Driving Forward
        	analogWrite(LeftPlus, 130);
            analogWrite(LeftMinus, 0);
            analogWrite(RightPlus, 130);
            analogWrite(RightMinus, 0);
          	delay(1000);
        }
     }
  }
}

Unity

Nu tænder vi for Unity. Det første der skal gøres når Unity er åbent, er at importere den udleverede Unity.package så vi har et udgangspunkt at starte fra. Den bane vi gerne vil have RoboSchneider til at køre igennem kan ses på billedet nedenfor.

Vi starter med at trække “Robot root” ind i scenen: “PF2-afleveringsbane”. På Robot root sætter vi Wheels, Breadboard, Servo og Linesensor, som “child” til “CarChassis”. Her kan man lege lidt rundt med hvordan ens Robot skal se ud. Vi har valgt at lade Robotten se ud som på billedet nedenfor.  

Her er det vigtigt at huske at gøre følgende:  

  •  Inde i Left Motor, nede i komponentet “Configurable Joint” skal CarChassis sættes ind i “Connected Body”. Ellers vil hjulene ikke følge med Robotten. Gør det samme for Right Motor.  
  • På Breadboard skal H-bridge LeftMotor og H-bridge RightMotor sættes som “child” af Breadboard. Herefter skal Left Motor sættes ind i Komponentet H Bridge (Script) for at den har en reference. Det samme skal gøre for RightMotor (med Right Motor i stedet for Left Motor selvfølgelig) 
  • På Breadboard skal der tilføjes 7 elementer i komponentet Breadboard(Script). (Man ændrer størrelsen af Arduino Object Pins ved at sætte “Size” til den ønskede størrelse). Indsæt H-bridge forwards pin LeftMotor til Element 0. Indsæt H-bridge backwards pin LeftMotor til Element 1. Indsæt H-bridge forwards pin RightMotor til Element 2. Indsæt H-bridge backwards pin RightMotor til Element 3. Indsæt DistanceSensor til Element 4. Indsæt LineSensorLeft til Element 5 samt LineSensorRight til Element 6. Disse 7 Elementer er referencer for Breadboardet, og virker som “pins” i virkeligheden.  

En kort bemærkning til placeringen af ens LDR-modstande samt hjul. Hvis sensorerne er placeret for langt fra hinanden, kan der opstå en uhensigtsmæssig adfærd når den forsøger at rette sin kurs i forhold til den sorte streg. Den vil altså lave store og lange sving hver gang den justerer. Ligeledes hvis de er for tæt på hinanden, så kan sensorerne have svært ved at vurdere som konstruktionen er på stregen eller uden for den, og i værste fald, forårsage at maskinen hele tiden hakker frem og tilbage eller fuldstændig forbiser når stregen svinger. Det anbefales at afprøve og justere i ens eget projekt mht. skalering og position af sensorerne samt hjul. Dertil kan det også anbefales at hjulene sættes foran på robotten, da dette giver en bedre intuitiv styring under opsætningen. I det endelige produkt vil placeringen af hjul og sensorer være afgørende for robottens navigations evne, og dermed hvor nøjagtigt robotten kan følge stregen i gulvet. 

Nu er Robotten bygget, så nu skal der skrives kode, så RoboSchneider bliver funktionel. Man kan med fordel tage koden fra TinkerCAD og bruge som skabelon i Unity. Desværre er kodesproget ikke det samme for Arduino, som det er for Unity, så vi skal skrive noget af koden om, samt tilføje mere kode. Vi vil i dette afsnit ikke gå for meget i dybden med kodeeksempler, men blot redegøre for nogle af de justeringer der skal til for at ”oversætte” koden til Unity. Den fulde kode kan ses forneden. Gå ind i ArduinoMain.cs. 

I Unity (C#) skal man ikke Declare (erklære) de forskellige pinModes, da vi har referencerne liggende ude i selve Editoren. #include <Servo.h> skal ikke indgå i C# koden, derfor heller ikke “Servo theServo; og theServo.attach(pin);”. Måden vi refererer til Servo’en i Unity er gennem Editoren hvor Servo’en bliver sat ind som en public Servo servo; i ArduinoMain.cs scriptet. Desuden skal “long” ændres til “ulong”. Serial virker heller ikke i C#, derfor bruger vi Debug.Log();. Dette er et meget nyttigt værktøj, som vi benyttede til at udradere ikke nævnelsesværdige fejl med, og det kan eksempelvis bruges til at opdele sin kode og se hvor eventuelle problemer opstår når man kører programmet. 

Den første del der skal tilføjes i C# koden, er en bool samt en “if” statement til at fortælle os hvornår Robotten kommer til den første forhindring. Dette uddyber vi længere nede. Til sidst skal alle “delay()” ændres til “yield return new WaitForSeconds(); da “delay()” funktionen ikke er en del af C# dokumentationen. 

Ideelt set ville vi gerne have RoboSchneider til selv at navigere rundt om denne forhindring ved hjælp af afstandsmåleren, da vi ikke længere havde en streg at køre efter udenom forhindringen, men dette voldte os store problemer og uregelmæssig adfærd, hvor robotten ikke kunne finde tilbage på rette kurs igen. Vi var derfor nødsaget til at navigere den manuelt ved at “hardcode” bevægelsen rundt om forhindringen før vi fortsætte resten af programmet til at navigere via stregen og derefter afstandsmåleren. Koden til den første forhindring er resultatet af en række afprøvelser i forhold til vores opbygning og valg af hastigheder for motorerne. 

else if (firstBlock == true)
                    {
                        analogWrite(0, 0);
                        analogWrite(1, 0);
                        analogWrite(2, 0);
                        analogWrite(3, 0);
                        yield return new WaitForSeconds(0.5f);
                        analogWrite(0, 0);
                        analogWrite(1, 55);
                        analogWrite(2, 55);
                        analogWrite(3, 0);
                        yield return new WaitForSeconds(1.1f);
                        analogWrite(0, 0);
                        analogWrite(1, 0);
                        analogWrite(2, 0);
                        analogWrite(3, 0);
                        yield return new WaitForSeconds(0.5f);
                        analogWrite(0, 100);
                        analogWrite(1, 0);
                        analogWrite(2, 100);
                        analogWrite(3, 0);
                        yield return new WaitForSeconds(0.7f);
                        analogWrite(0, 0);
                        analogWrite(1, 0);
                        analogWrite(2, 0);
                        analogWrite(3, 0);
                        yield return new WaitForSeconds(0.5f);
                        analogWrite(0, 55);
                        analogWrite(1, 0);
                        analogWrite(2, 0);
                        analogWrite(3, 55);
                        yield return new WaitForSeconds(1f);
                        analogWrite(0, 0);
                        analogWrite(1, 0);
                        analogWrite(2, 0);
                        analogWrite(3, 0);
                        yield return new WaitForSeconds(0.5f);
                        analogWrite(0, 100);
                        analogWrite(1, 0);
                        analogWrite(2, 100);
                        analogWrite(3, 0);
                        yield return new WaitForSeconds(1.5f);
                        analogWrite(0, 0);
                        analogWrite(1, 0);
                        analogWrite(2, 0);
                        analogWrite(3, 0);
                        yield return new WaitForSeconds(0.5f);
                        analogWrite(0, 55);
                        analogWrite(1, 0);
                        analogWrite(2, 0);
                        analogWrite(3, 55);
                        yield return new WaitForSeconds(1f);
                        analogWrite(0, 0);
                        analogWrite(1, 0);
                        analogWrite(2, 0);
                        analogWrite(3, 0);
                        yield return new WaitForSeconds(0.5f);
                        analogWrite(0, 100);
                        analogWrite(1, 0);
                        analogWrite(2, 100);
                        analogWrite(3, 0);
                        yield return new WaitForSeconds(0.6f);
                        analogWrite(0, 0);
                        analogWrite(1, 0);
                        analogWrite(2, 0);
                        analogWrite(3, 0);
                        yield return new WaitForSeconds(0.5f);
                        analogWrite(0, 0);
                        analogWrite(1, 55);
                        analogWrite(2, 55);
                        analogWrite(3, 0);
                        yield return new WaitForSeconds(1f);
                        analogWrite(0, 0);
                        analogWrite(1, 0);
                        analogWrite(2, 0);
                        analogWrite(3, 0);
                        yield return new WaitForSeconds(0.5f);
                    }

Det er ikke en implementering vi anbefaler, men vores metode kan ses her foroven og længere nede i den komplette Unity kode. Da man kun støder på én forhindring i denne bane følte vi det var en acceptabel løsning i forhold til dette projekt. Hvis der var flere forhindringer i banen, så ville man være nødsaget til at lave en funktion i stedet for at skrive hele omkørslen hver gang RoboSchneider skulle forbi en forhindring.

Til sidst når vi anvender afstandsmåleren til at navigere den resterende del af banen, så er de værdi vi anvender til vores motor inputs for at dreje RoboSchneider igen noget vi har testet os frem til. Selve målingen og beregning af hvilken retning der giver fri passage er ”korrekt”, men når robotten drejer er det en delvist manuel styring i forhold til de værdier og hastigheder vi har angivet for vores motor inputs. Med lidt måling og finjustering skulle det gerne give robotten en næsten nøjagtig rotation hvis banen blev udvidet, men efter for mange retningsskift kan der potentielt opstå nogle udfordringer med robottens retning. 

Hvis koden er skrevet korrekt skulle den gerne ligne noget i stil med den kode der står herunder. 

using System.Collections;
using System.Collections.Generic;
using UnityEngine;
using System.Threading;
public class ArduinoMain : MonoBehaviour
{
    public Breadboard breadboard;
    public Servo servo;
    
    //Setup what needs to be stated before the robot begins its Loop
    IEnumerator setup()
    {
        //Set Servo Motor on 90 degrees (look forward)
        servo.write(90);
        Debug.Log("GO");
        #region PremadeSetup
        yield return StartCoroutine(loop()); ;
        #endregion PremadeSetup
    }
    //Declare the needed variables
    int left = 0;
    int right = 0;
    int minDiff = 50;
    
    bool onLine = true;
    bool firstBlock = false;
    ulong dis = 900;
    ulong servoLeft = 0;
    ulong servoFront = 0;
    ulong servoRight = 0;
    //The Loop Begins
    IEnumerator loop()
    {
        //Read the LDR-Sensores
        left = analogRead(5);
        right = analogRead(6);
        int diff = Mathf.Abs(left - right);
        
        //Read the SonarSensor
        ulong tid = pulseIn(4);
        //Read the ServoMotor
        int servoPos = servo.read();
        
        //As long as we are on the groundline
        if (onLine == true)
        {
            //If the robot is turning right, it turns left slightly
            if (left < right)
            {
                analogWrite(0, 0);
                analogWrite(1, 0);
                analogWrite(2, 50);
                analogWrite(3, 0);
            }
            // Vice Versa
            else if (right < left)
            {
                analogWrite(0, 50);
                analogWrite(1, 0);
                analogWrite(2, 0);
                analogWrite(3, 0);
            }
            // If the robot follows the line fine:
            else if (diff <= minDiff)
            {
                //it goes forward
                if (tid >= dis || tid == 0)
                {
                    analogWrite(0, 100);
                    analogWrite(1, 0);
                    analogWrite(2, 100);
                    analogWrite(3, 0);
                }
                // else it hits an obsticle
                else if (tid <= dis)
                {
                    //There is a fail in the program that makes it delay just enough for the robot to bug on start-up. This "if" prevents it from stopping
                    if (firstBlock == false)
                    {
                        firstBlock = true;
                        Debug.Log("FirstBlockPassed");
                    }
                    //After the bug, the robot avoits absticles but driving around them as shown below.
                    else if (firstBlock == true)
                    {
                        analogWrite(0, 0);
                        analogWrite(1, 0);
                        analogWrite(2, 0);
                        analogWrite(3, 0);
                        yield return new WaitForSeconds(0.5f);
                        analogWrite(0, 0);
                        analogWrite(1, 55);
                        analogWrite(2, 55);
                        analogWrite(3, 0);
                        yield return new WaitForSeconds(1.1f);
                        analogWrite(0, 0);
                        analogWrite(1, 0);
                        analogWrite(2, 0);
                        analogWrite(3, 0);
                        yield return new WaitForSeconds(0.5f);
                        analogWrite(0, 100);
                        analogWrite(1, 0);
                        analogWrite(2, 100);
                        analogWrite(3, 0);
                        yield return new WaitForSeconds(0.7f);
                        analogWrite(0, 0);
                        analogWrite(1, 0);
                        analogWrite(2, 0);
                        analogWrite(3, 0);
                        yield return new WaitForSeconds(0.5f);
                        analogWrite(0, 55);
                        analogWrite(1, 0);
                        analogWrite(2, 0);
                        analogWrite(3, 55);
                        yield return new WaitForSeconds(1f);
                        analogWrite(0, 0);
                        analogWrite(1, 0);
                        analogWrite(2, 0);
                        analogWrite(3, 0);
                        yield return new WaitForSeconds(0.5f);
                        analogWrite(0, 100);
                        analogWrite(1, 0);
                        analogWrite(2, 100);
                        analogWrite(3, 0);
                        yield return new WaitForSeconds(1.5f);
                        analogWrite(0, 0);
                        analogWrite(1, 0);
                        analogWrite(2, 0);
                        analogWrite(3, 0);
                        yield return new WaitForSeconds(0.5f);
                        analogWrite(0, 55);
                        analogWrite(1, 0);
                        analogWrite(2, 0);
                        analogWrite(3, 55);
                        yield return new WaitForSeconds(1f);
                        analogWrite(0, 0);
                        analogWrite(1, 0);
                        analogWrite(2, 0);
                        analogWrite(3, 0);
                        yield return new WaitForSeconds(0.5f);
                        analogWrite(0, 100);
                        analogWrite(1, 0);
                        analogWrite(2, 100);
                        analogWrite(3, 0);
                        yield return new WaitForSeconds(0.6f);
                        analogWrite(0, 0);
                        analogWrite(1, 0);
                        analogWrite(2, 0);
                        analogWrite(3, 0);
                        yield return new WaitForSeconds(0.5f);
                        analogWrite(0, 0);
                        analogWrite(1, 55);
                        analogWrite(2, 55);
                        analogWrite(3, 0);
                        yield return new WaitForSeconds(1f);
                        analogWrite(0, 0);
                        analogWrite(1, 0);
                        analogWrite(2, 0);
                        analogWrite(3, 0);
                        yield return new WaitForSeconds(0.5f);
                    }
                }
            }
            //When the robot hits the ending of the ground line it stops and shifts over to "Off-Line" mode 
            if (left <= 1000 && right <= 1000)
            {
                analogWrite(0, 0);
                analogWrite(1, 0);
                analogWrite(2, 0);
                analogWrite(3, 0);
                onLine = false;
            }
        }
        //THIS IS OFF-LINE MODE
        //If the end of the groundline is reached
        if (onLine == false)
        {
            //Debug.log("Offline") to make it obvious for the spectators
            Debug.Log("OffLine");
            //if the SonarSensor signal is bigger than the chosen minimum distance it goes forward 
            if (tid >= dis)
            {
                analogWrite(0, 130);
                analogWrite(1, 0);
                analogWrite(2, 130);
                analogWrite(3, 0);
            }
            //if the SonarSensor signal is smaller than the chosen minimum distance it stops and makes a check 
            else if (tid <= dis)
            {
                analogWrite(0, 0);
                analogWrite(1, 0);
                analogWrite(2, 0);
                analogWrite(3, 0);
                yield return new WaitForSeconds(2);
                //The check consists of 4 steps
                //Step 1 check distance to nearest wall in front of robot
                servoFront = pulseIn(4);
                Debug.Log("Front Servo =" + servoFront);
                servo.write(0);
                yield return new WaitForSeconds(2);
                //Step 2 check distance to nearest wall left of robot
                servoLeft = pulseIn(4);
                Debug.Log("Left Servo =" + servoLeft);
                servo.write(180);
                yield return new WaitForSeconds(4);
                //Step 3 check distance to nearest wall right of robot
                servoRight = pulseIn(4);
                Debug.Log("Right Servo =" + servoRight);
                servo.write(90);
                yield return new WaitForSeconds(2);
                //And step 4
                //Compare the 3 checks and turn to the wall farest away.
                //if the wall at left and the wall at right is not existing, it means the end is reached. The robot will turn right and drive off the ground.
                if (servoLeft == 0 && servoRight == 0)
                {
                    Debug.Log("ENDGAME");
                    analogWrite(0, 55);
                    analogWrite(1, 0);
                    analogWrite(2, 0);
                    analogWrite(3, 55);
                    yield return new WaitForSeconds(1f);
                    analogWrite(0, 0);
                    analogWrite(1, 0);
                    analogWrite(2, 0);
                    analogWrite(3, 0);
                    yield return new WaitForSeconds(1f);
                    analogWrite(0, 255);
                    analogWrite(1, 0);
                    analogWrite(2, 255);
                    analogWrite(3, 0);
                    yield return new WaitForSeconds(10f);
                }
                //Turn to left wall
                if (servoLeft >= servoFront && servoLeft >= servoRight && servoRight != 0 || servoLeft == 0)
                {
                    analogWrite(0, 0);
                    analogWrite(1, 55);
                    analogWrite(2, 55);
                    analogWrite(3, 0);
                    yield return new WaitForSeconds(1f);
                    analogWrite(0, 0);
                    analogWrite(1, 0);
                    analogWrite(2, 0);
                    analogWrite(3, 0);
                    yield return new WaitForSeconds(1f);
                    analogWrite(0, 130);
                    analogWrite(1, 0);
                    analogWrite(2, 130);
                    analogWrite(3, 0);
                }
                //Turn to right wall
                if (servoRight >= servoFront && servoRight >= servoLeft && servoLeft != 0 || servoRight == 0)
                {
                    analogWrite(0, 55);
                    analogWrite(1, 0);
                    analogWrite(2, 0);
                    analogWrite(3, 55);
                    yield return new WaitForSeconds(1f);
                    analogWrite(0, 0);
                    analogWrite(1, 0);
                    analogWrite(2, 0);
                    analogWrite(3, 0);
                    yield return new WaitForSeconds(1f);
                    analogWrite(0, 130);
                    analogWrite(1, 0);
                    analogWrite(2, 130);
                    analogWrite(3, 0);
                }
                //Continue forward
                if (servoFront >= servoLeft && servoFront >= servoRight)
                {
                    analogWrite(0, 130);
                    analogWrite(1, 0);
                    analogWrite(2, 130);
                    analogWrite(3, 0);
                }
            }
        }
        
        #region DoNotDelete
        //Wait for one frame
        yield return new WaitForSeconds(0);
        //New loop():
        yield return loop();
        #endregion DoNotDelete 
    }
    
    #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
}

Konklusion 

RoboSchneider fuldfører den opstillede bane på omtrent 87 sekunder. Den navigerer rundt ved hjælp af en række sensorer, som var påkrævet af opgavebeskrivelsen, nemlig LDR-modstande samt ultralydafstandsmåler. Den er i stand til at nå banens ende og undgå al kollision undervejs. 

Perspektivering  

RoboSchneider er ikke i stand til at fuldføre banen fuldstændig på egen hånd. Med dette menes, at vi var nødsaget til at implementere en lille smule “hard coding” til at navigere rundt om den første forhindring, hvilket vi også belyser tidligere i indlægget. Ydermere har RoboSchneider ikke nødvendigvis den hurtigste tid, men den formår at opnå en sikker og stabil gennemførelse af banen. Med lidt mere justering og testing kan denne tid forbedres, men vi har valgt at beholde den i dens nuværende tilstand med en solid performance for opgavens og forståelsens skyld. 

Afslutningsvist nævner vi, at delay() funktionen ikke virker i C#. Da dette teknisk set er sandt, så er vi opmærksomme på, at Jamie og Christian var med til at udarbejde en IEnumerator “delay” funktion, men da vi havde problemer med at implementere den, så var vi nødsaget til at bruge WaitForSeconds() i stedet. 

Leave a Reply