lab

 

I PF3 har vi fået til opgave hver at lave 1 af 5 robotter som ved at samarbejde skal kunne gennemføre en stafet. Stafetten går ud på at flytte en bold fra starten af banen til banens slut. På vejen er der forskellige udfordringer og de 5 robotter har hver sit område hvor de skal være i stand til at flytte bolden fra et punkt gennem forhindringerne, til et andet punkt. Jeg har valgt Robot 5, som skal modtage bolden fra robot 4 og transportere den gennem en lille labyrint.

 

Hardware

HC-SR04 – Ultrasonic sensor – Datasheet: http://jaktek.com/wp-content/uploads/2011/12/HC-SR04.pdf

hc-sr04

Resten af hardwaren kan man læse om i min PF2 blog: http://op.tek.sdu.dk//?p=125

 

Ombygning

top

For at løse PF3 har jeg lavet nogle ændringer på min robot Al Zheimer fra PF2. Lyssensoren er erstattet med en ultrasonisk lyd sensor. Baghjulet på robotten er skiftet ud med en pind, for at undgå problemer med styringen. Der er også tilføjet en ring til at transportere bolden.

side

Første iteration af Al Zheimer kan ses på min PF2 blog: http://op.tek.sdu.dk//?p=125

 

Ultrasonisk sensor

En ultrasonisk sensor erstatter Al Zheimers lyssensor. Da det ikke længere er en streg på gulvet den skal følge, men vægge i en labyrint som skal undgås. Ultralyds sensoren kan måle afstand til nærmeste objekt, som reflekterer lyden tilbage til sensoren. Ved at sende en kort lyd afsted kan man måle hvor lang tid der går før lyden bliver reflekteret tilbage til sensoren. Fordi lyd altid bevæger sig med cirka samme hastighed igennem, i dette tilfælde luften, kan man regne sig frem til hvor langt den har bevæget sig.

For et kode eksempel på at få sensoren til at måle en afstand, se ultraCalc funktionen i den samlede kode, længere nede.

Styrings Algoritme

Jeg har valgt at implementere en styrings algoritme som minder om den jeg brugte i PF2 til at følge en streg. Denne gang er det en væg der skal følges i stedet, med en lydsensor. Basis funktionen i algoritmen er den samme, hvor robotten i PF2 ”hoppede” på kanten af stregen, vil den her i PF3 ”hoppe” langs en væg. Det opnås ved at måle afstand ind til væggen, hvis robotten kommer for tæt på væggen drejer den væk. Når afstanden til væggen så bliver stor nok, drejer robotten ind mod væggen igen.

Da labyrinten har skarpe hjørner ville det være godt med 2 eller flere lydsensorer. En sensor kunne måle hvor langt robotten er fra at køre med siden ind i en væg, og en anden sensor sat i en 90 graders vinkel fra den første kunne måle fremad og se hvis robotten er på vej ind i et hjørne. Så hvis robotten nærmer sig en væg, så drejer den inden den rammer væggen.

algo1

Der er dog kun stillet 1 lydsensor til rådighed, til løsning af opgaven. Man kunne så bruge en kontakt, flexsensor eller lignende på den ene akse og lydsensoren på den anden. Labyrinten kan dog godt løses med en enkelt lydsensor. Hvis denne sensor sættes i en vinkel mellem de 2 før nævnte akser, vil den kunne tjekke både foran sig og bag ved. Det virker fint fordi robotten skal gøre det samme når den modtager en afstands værdi lavere end en fastsat grænse, uanset om væggen er foran eller ved siden af robotten.

algo2

Med denne måde at bruge sensoren på vil robotten kunne bevæge sig langs den ene væg i labyrinten. Hvis den fortsætter med at følge den ene væg vil den ende i mål. Det vil virke for alle labyrint layouts, selv en blindgyde vil bliver fulgt langs den ene væg ind, og den anden væg ud. Med mindre en blindgyde er så smal at robotten ikke kan nå at dreje rundt i bunden af den.

maze1

En labyrint med en ind og en udgang vil altid bestå af 2 dele, med vejen i gennem som skille linjen mellem dem. Så hvis robotten følger den ene side, vil den altid kunne nå ud. Ovenfor ses 2 billeder af den samme labyrint, med de to sider tegnet ind i grøn og rød.

Yderligere udfordringer fandtes i at få robotten rundt om fritstående vægge. Når sensoren passerer væggen vil den måle en stor afstand og derefter dreje skarpt ind i væggen og sætte sig fast. Det klares ved at give lidt pwm til det hjul som er ind mod væggen, når en høj afstand måles, så robotten kører i en flot bue rundt.

 

Andre algoritmer

Der findes andre metoder til at navigere labyrinter, men for at lave en der er mere effektiv kræver det flere motorer eller sensorer. Så ville man kunne opbygge et kort i robotten over omgivelserne, også navigere ud fra det, og måske endda eliminere blindgyder.

Med kun 1 sensor og 2 motorer, ville man skulle bruge 1 sensor til både at opbygge kortet over verden og vide hvor den befinder sig i forhold til det. Det kan meget nemt blive noget forfærdeligt noget, hvor hver eneste måling eller bevægelse kan føre til fejl og multiplikation af tidligere fejl.

 

Bold transport

Bolden bliver afleveret til en rampe af robot 4, og ruller ned i Al Zheimers boldholder.

rampe

I bold holderen havde jeg placeret en flexsensor til at starte Al Zheimer når den modtog bolden, men under stafet testen viste det sig at den var alt for upålidelig. Derfor skiftede jeg til at starte Al Zheimer med bluetooth når bolden er modtaget.

Kode

// inkludering af Software Serial library
#include <SoftwareSerial.h>

// definer TX og RX pins til Software Serial 
#define TXPIN 4
#define RXPIN 7

// instanciering af Software Serial objekt
SoftwareSerial BTSerial(RXPIN, TXPIN);

// sæt konstanter for motor styring
const int dirA = 12;
const int dirB = 13;
const int speedA = 3; // venstre
const int speedB = 11; // højre
const int brakeA = 9;
const int brakeB = 8;

// definer pins til styring af ultrasonisk sensor
#define trigPin 2
#define echoPin 10

// mode sættes til en værdi
char mode = '0';

// variabler til afstandsmåling
float dist;
int distTres = 20;
float distance;

void setup() {
  // Alle Pins opsættes som output-pins
  pinMode(dirA, OUTPUT);
  pinMode(dirB, OUTPUT);
  pinMode(speedA, OUTPUT);
  pinMode(speedB, OUTPUT);
  pinMode(brakeA, OUTPUT);
  pinMode(brakeB, OUTPUT);

  // motor retning
  digitalWrite(dirA, LOW);
  digitalWrite(dirB, HIGH);

  // motor bremser fra
  digitalWrite(brakeA, LOW);
  digitalWrite(brakeB, LOW);

  // Definer pins til bluetooth som input/output pins
  pinMode(RXPIN, INPUT);
  pinMode(TXPIN, OUTPUT);

  // Start kommunikation med Bluetooth 
  BTSerial.begin(115200);

  Serial.begin (115200);

  // ultrasoniske pins modes
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  }

void loop() {
  // kommunikation over bluetooth
  if( BTSerial.available() )       
  {
    mode = BTSerial.read();        
  }

  // kør beregn afstand og gem i dist
  dist = ultraCalc();

  if( mode == '0' )               // mode 0 - stop motorer
  {
    // Stop
    analogWrite(speedA, 0); 
    analogWrite(speedB, 0);

  } else if( mode == '1' )     // kør mode 1, drej venstre, væk fra væg
  {
    analogWrite(speedA, 0);
    analogWrite(speedB, 100);
    if (dist > distTres+20) { mode = '3'; }
    else if (dist > distTres) { mode = '2'; }

  } else if ( mode == '2' )    // kør mode 2, drej højre, hen mod væg
  { 
    analogWrite(speedA, 100);
    analogWrite(speedB, 0);
    if (dist > distTres+25) { mode = '3'; }
    else if (dist < distTres) { mode = '1'; }    

  } else if ( mode == '3' )    // kør mode 3, kør bue rundt om væg
  { 
    analogWrite(speedA, 100);
    analogWrite(speedB, 30);
    if (dist < distTres) { mode = '1'; }
  }

  delay(50);
}

int ultraCalc() {
  // først sikres at trigger pin er LOW
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  // send signal til trigger pin
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  // vent på svar fra sensor, max 2500 milisekunder
  float duration = pulseIn(echoPin, HIGH, 2500);
  //omregn tidsrum til afstand
  if(duration) { distance = (duration/2) / 29.1; }
    else { distance = 50; }
  // print til usb og BT
  Serial.print(distance);
  Serial.println(" cm");
  BTSerial.print(distance);
  BTSerial.println(" cm");
  //normaliser værdier
  if (distance > 50) { distance = 50; }
 return(distance); 
}

Stafet

 

Konklusion

Al Zheimer kan gennemføre sin del af stafetten. Det er dog ikke hver gang det lykkedes uden hjælp. Det kan skyldes upålidelige målinger fra sensoren. De fleste målinger bliver lavet med en skæv vinkel til væggen og refleksion af lyden er ikke lige så god som hvis sensoren var vinkel ret på væggen. Forskellig spænding på batterierne mellem test på forskellige dage kan også være et stort problem. Kollision med en væg sætter Al Zheimer ud af spil til den får hjælp, så stor præcision er nødvendig. Da Al Zheimer er lavet af lego, vil det også påvirke konstruktionen hver gang den bliver håndteret, et hjul glider måske lidt på sin akse, så det skaber mere friktion når robotten forsøger at dreje. Der er mange små ting som kan være med til at man ikke får præcis samme resultat hver gang man tester.

I denne opgave skulle vi få robotterne til at arbejde sammen, og det er lykkedes at få del elementerne til at virke hver for sig. Det bliver dog et problem når de bliver sat sammen i den samlede stafet, fordi en enkelt fejl ved en af robotterne kan bryde kæden så stafetten ikke når frem til mål.

Leave a Reply