POPGO2 – Porteføljeopgave 2 – LEGO Black’n’Track

Af Daniel Hansen Pedersen, Søren Holmbjerg Schibler og Tobias Holm Krab

Bedste konkurrencetid: 27,48 sek

Opbygning af hardware

• 2 LEGO DC Motorer
• Arduino Uno
• Arduino Motorshield
• 2 QRE1113 sensorer
•  HC – SR04
• Ledninger
• Lego
• 6 AA 1,2V Batterier

Robottens opbygning er baseret på en Braitenberg Vehicle 2a.
Denne består af to aktuatorer (motorer) forbundet til to sensorer samt en distancesensor. Jo større sensorstimuli fra de to lyssensorer, jo mere bevæger motoren sig. Sensorerne er placeret på hver sin side af den sorte streg til en start, og når de ser hvid, vil de køre.

Opbygning af robot i LEGO

Robotten er opbygget på den måde at den har 2 styrende hjul samt en støttepind med en glider monteret. I starten af opbygningen af robotten blev der overvejet, hvordan robotten skulle køre; ideen har hele tiden været at hver motor, skulle trække hver sin side af robotten. Det var dog ikke klart hvordan, man skulle lave understøtningen. Der blev eksperimenteret med, at en motor skulle trække 2 hjul. Det blev vurderet, at en motor ikke ville have nok motorkraft til at trække begge hjul. Derfor blev der kigget nærmere på at montere 2 støttehjul bag på robotten, som kunne dreje med, når de styrende drejer. Dette gik fint så længe, batterierne ikke sad på robotten. Men da batteriernes vægt kom på, blev vægten for stor i forhold til støttehjulene, og de kunne derfor ikke dreje med de styrende hjul, og derfor blev der skabt en meget stor friktion. Derfor blev det vurderet at en støttepind med en glider, ville være mere passende. Dog var vægten af batterierne stadig væk noget ,der hæmmede robotten, og derfor blev de flyttet op til motorerne for at skabe mere balance på robotten. Da batterierne var flyttet længere frem, kunne der koncerteres om gearingen, her blev det først afprøvet med en 40/24 hvilket ville have forøget motorenes kraft med 1.7 gange motorernes udgangs kraft. Dette viste sig at have en indflydelse på motorerne når robotten skulle korrigerer med linjen, da friktionen blev for stor i dem. Derfor blev gearingen ændret til 1/1 forhold, herved kan motorerne nu trække robotten.
Herefter blev der kigget på lyssensorernes placering på robotten, først afstanden mellem dem hvor det blev vurderet at jo tættere de sad sammen jo større en præcision ville robotten også få. Derefter blev der kigget på hvor langt lyssensorerne skulle sidde fra de styrende hjul for at opnå de bedst mulige betingelser. Sensorerne blev både sat meget langt foran de styrende hjul dette skabte dog meget korrugering under dens kørsel, hvilket gjorde at den hele tiden skulle dreje. Derefter blev det afprøvet at sensorerne sad på de styrende aksler hvilket gjorde robotten blev forsinket i sin reaktioner og den derfor også hele tiden skulle dreje for at tilpasse sig linjen. Den endelige løsning på lyssensorernes placering blev en montering lig foran de styrende hjul hvilket blev vurderet til at være den bedste placering.
Nu havde robotten dog et positivt problem da den nu kørte så hurtigt, forhold til gearingen, hvilket betød at den ikke kunne nå at registrerer linjen. Gearingen den blev sat ned på var en ,hvilket løste problemet med at følge linjen ordenligt da den nu kunne nå at registrerer linjen i dens input for at give det rigtige output. Så blev sonaren monteret på robotten, der gør at den kan følge den væg som man skal omkring for at komme i mål. Dette ledte dog til store problemer da sonaren får fejl værdier ved hjørner hvilket at den får forkerte inputs og derved giver forkerte outputs, og derved ikke følger væggen. Dette blev løst ved ikke at bruge sonaren værdi 0, hvilket betyder at den godt registrerer nul men ikke ændre output til motorerne. Nu kom der så et ny problem med gearingen da det viste sig at robotten ikke fik nok inputs fra sonaren, og derfor ikke fik ændret retning i tide, samtidig med at den havde for voldsomme bevægelser. Derfor måtte gearingen yderligere justerres ned til en ,dette gjorde at den nu både kunne følge linjen og væggen for derved finde vej til mål. Efter mange overvejelser Kom robotten i mål dog med en betydeligere lavere hastighed end hvad vi havde troet til at starte med.

 

 

Robotten i aktion

Opbygning af program

#include <NewPing.h>

#define TRIGGER_PIN  7
#define ECHO_PIN     5
#define MAX_DISTANCE 400
     
NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE);

const int LS1 = A2; //sensor 1 til analog 2
const int LS2 = A3; //sensor 2 til analog 3 
int MotorA = 3; //PWM forbundet til digital 3 
int MotorB = 11; //PWM forbundet til digital 11 
int driveA = 12; //kør motor A
int driveB = 13; //kør motor B  

int valA = 0; int valB = 0; //variable til sensorerværdier 
int mapA = 0; int mapB = 0; 
bool fase2= false;

void setup() {
  pinMode(LS1, INPUT); //sensor 1 som input
  pinMode(LS2, INPUT); //sensor 2 som input 
  pinMode(MotorA, OUTPUT); //motor A som output
  pinMode(MotorB, OUTPUT); //motor B som output 
  pinMode(driveA, OUTPUT);
  pinMode(driveB, OUTPUT); 
  Serial.begin(9600); 
}

void loop() {
valA = analogRead(LS1); //læser værdi fra LS1
valB = analogRead(LS2); //læser værdi fra LS2
mapA = map(valA, 1023, 0, 0, 255); //vænner værdierne om så de går fra 0 til 255
mapB = map(valB, 1023, 0, 0, 255); 

if (fase2 == false) {
if (mapA >= 50) {
  digitalWrite(driveA, LOW); //50 
  analogWrite(MotorA, mapA);
}
else if (mapA < 50) {
  digitalWrite(driveA, HIGH); 
  analogWrite(MotorA, mapB); 
}

if (mapB >= 50) {
  digitalWrite(driveB, HIGH); 
  analogWrite(MotorB, mapB);
}
else if (mapB < 50) {
  digitalWrite(driveB, LOW);
  analogWrite(MotorB, mapA); 
} 
} 
if (fase2 == false && mapB < 50 && mapA < 50){ // Hvis den registrerer sort på begge sensorer 
  digitalWrite(driveA, LOW);
  digitalWrite(driveB, HIGH);
  analogWrite(MotorA, 200);
  analogWrite(MotorB, 25);
  delay(2500);
  fase2=true;
}
if (fase2 == true) {
  int uS = sonar.ping();
  int cm = uS/US_ROUNDTRIP_CM;

  if (cm < 5 && cm > 1){
    // kør lidt mod venstre

    digitalWrite(driveA, LOW);
    digitalWrite(driveB, LOW);
    analogWrite(MotorA, 150);
    analogWrite(MotorB, 150);
  }
  else if (cm >=9){
    // kør lidt mod højre

    digitalWrite(driveA, LOW);
    digitalWrite(driveB, HIGH);
    analogWrite(MotorA, 10);
    analogWrite(MotorB, 200);
  }
  else if (cm < 9 && cm >=5 ){

  //kør ligeud
    digitalWrite(driveA, LOW);
    digitalWrite(driveB, HIGH);
    analogWrite(MotorB, 150);
    analogWrite(MotorA, 150);
  }
}

 

Samspil mellem elektronik, mekanik og software

Til implementering af linjefølger-opførsel er der anvendt to analoge SparkFun Line Sensor Breakout, QRE1113. Sensoren virker ved at detektere mængden af infrarødt lys, der reflekteres tilbage på den.
Den består af en LED, der udsender IR (i serie med en 100Ω modstand for at begrænse strømmen) og en IR-sensitiv fototransistor.
Sensoren er tilkoblet Arduino Uno via V_cc, ground samt et analogt input. Den har en indbygget 10k Ω pull-up resistor, der sætter output-pin til høj (5V) og som nedjusteres proportionelt med stigende refleksion – altså vil sort medføre en høj output-værdi og registrering af hvid medføre en lav. Da koden er opbygget til at sætte hastigheden proportionalt med lysintensiteten (altså hurtigere jo mere ’hvidt’ den ser), er der vendt rundt på dette i koden, så sort medfører analogt input nær 0 og hvid 1023 (da Arduino jo er 10 bit).
Da motorshieldet giver LEGO DC motoren et 8 bit PWM-signal mappes det analoge output fra sensoren til en værdi mellem 0 og 255.
Opførslen er dermed implementeret således, at når sensorerne ser hvid, vil motorerne køre fremad med maksimal hastighed. For at robotten kan følge stregen i sving, har det været en nødvendighed at indsætte en tærskelværdi i koden, der får det modsatte hjul af den sensor, der registrer sort, til at køre modsat vej med en hastighed svarende til det analoge input fra den modsatte sensor (der ser hvid). Det har altså ikke været muligt at implementere opførslen udelukkende på Braitenberg Vehicle 2a-principper, når der var et kriterie, at robotten også skulle kunne gennemføre banen hurtigst muligt.

Perspektivering

Det kunne implementeres, at robotten stopper ved den sidste stoplinje ved at tjekke for, om fase 2 er sand og begge line tracker sensorer ser sort. Dog ift. konkurrencen, skal den køre ind i noget, så derfor er dette ikke implementeret (kunne implementeres med delay, hvorefter den stopper).
Robotten har på linjen en meget hakkende opførsel. Denne kunne fjernes ved enten at benytte sig af PDI-regulering eller implementere et FIR filter, som fx et moving average.

Konklusion

Det er lykkedes at få robotten til både at følge linjen samt efterfølgende køre langs muren til målstregen, så kriterierne er i den forstand lykkedes. Det har krævet flere ombygninger af robotten undervejs, først mht. placering af sensorer, men mest af alt ift. optimal gearing ift. at kunne trække robotten, køre med maksimal hastighed mens samtidigt at input fra sensorerne gives i et passende tidsomfang, så robotten kan nå at korrige sin opførsel herefter.