Portefølje 2 – Robot med LEGO og Arduino

Opgavebeskrivelse

Der skal konstrueres en Arduino LEGO robot, hvor styringen foregår vha. Arduino UNO samt Arduino Motor-shield. Til robotten er der udleveret to DC LEGO motorer samt et Bluetooth-modul. Gennem Bluetooth-modulet skal robotten kunne kommunikere med enten PC og/eller smartphone.

Robotten skal kunne følge en sort streg, som er placeret på et hvidt underlag med en forhindring på 10x10x10 cm placeret på midten af banen.

Der kan frit vælges sensorer til at sanse omgivelserne.

Overvejelser omkring valg af sensorer

Robotten skal forholde sig til henholdsvis to forskellige genstande i dens omgivelser, nemlig den sorte streg kontra den hvide baggrund samt forhindringen på 10x10x10 cm.

Stregen og den hvide baggrund er primært sanselig gennem syn/lys, da stregen ikke har en synderlig rumlig forskel fra baggrunden eftersom stregen er lavet af en tynd sort tape. Det vil derfor umiddelbart være mest hensigtsmæssigt at bruge en lys-sensor til at måle og sanse streg og baggrund. Fordi robotten skal laves i LEGO, er det belejligt at bruge en LEGO NXT lys-sensor, da den er nem at montere på selve LEGO robotten. NXT sensoren måler eksponeringen af lys, som i sagens natur er meget forskellig alt efter om målingen er rettet mod den sorte streg eller baggrunden. Sensoren giver kun én værdi til Arduino boardet, hvilket gør det nødvendigt at lave koncentrerede målinger af banen, da det med den ene værdi er svært at danne et helhedsbillede af banen, som vi fx gør det med vores syn. Sensoren skal derfor være tæt på banens overflade for at få den koncentrerede måling. På den måde er det kun muligt for robotten at sanse banens overflade, der hvor robotten er placeret, hvilket gør, at robotten med banens snoede forløb af og til kommer udenfor den sorte streg med sensoren. Det er med andre ord nødvendigt at forholde sig til den sanselige begrænsning i sensoren i forhold til algoritmen.

Modsat den sorte streg og baggrunden er forhindringen en mere rumlig genstand med en sådan vægt at den også giver modstand ved kollision. Det er derfor muligt at sanse forhindringen gennem direkte kontakt vha. fx en trykknap eller gennem afstandsmåling, da det er den eneste genstand på banen der har en prominent højde. I det forrige afsnit om lys-sensoren var det nødvendigt med en meget direkte kontakt med overfladen for at få en brugbar og koncentreret måling af banens overflade, hvilket kan undgås med afstandsmåling, eftersom man på den måde kan måle forhindringen længe før en potentiel kontakt/kollision. Jeg har derfor valgt at bruge en ultralydssensor, der vha. udsendelse og modtagelse af samme ultralyd kan måle afstanden fra sensoren til en kontaktflade.

Med de to sensorer er det på den måde muligt at sanse banens omgivelser, hvilket er nødvendigt for gennemførsel af opgaven.

Teknisk omkring sensorer

NXT lyssensor

NXT sensorerne forbindes gennem en sekspositions modular connector med både analoge og digitale interfaces, som det ses på tabellen.

legolys

Den første pin i lyssensoren er en open-collector transistor, som vha. af en pull-up resistor øger det analoge signal når transistoren er slukket. Omvendt er outputtet omkring 0V når transistoren er tændt.  På figur 1.3 ses ikke direkte pull-up resistoren, da Arduino UNO har indbygget pull-up resistorere for de analoge input pins. For at aktivere en pull-up resistor i Arduino UNO sættes pinMode til input og der laves et digitaltWrite sat til HIGH under setup().  De to næste pins er ground, og fjerde pin er power, som får 5V fra Arduinoen.  Den femte pin kan bruges til at aktivere lys fra lyssensoren. Det har dog ikke været nødvendigt at bruge lys, da sensoren for den byggede robot er tæt på overfladen.

light sensor nxt

Ultralydssensor

Ultralydssensoren består af fire pins, hvor den første er Vcc og fjerde ground. Ultralydssensoren bruger ifølge datablad 5V, og den kan derfor tilsluttes direkte til Arduino UNO. Anden og tredje pin er henholdsvis Trigger Pulse Input og Echo Pulse Output, som sættes i digitale porte da der er tale om puls in- og output. Modulet fungerer på den måde, at triggeren ved høj niveau signal udsender otte 40 kHz, hvorefter modulet tjekker om der er et pulssignal tilbage. Tiden fra signalet udsendes til det igen modtages bruges til at beregne afstanden fra sensor til objekt. Echo giver et puls output , som er i enheden uS. Ved følgende formel kan afstanden bestemmes:

range = high level time * velocity (340M/S) / 2

Hvis afstanden ønskes i centimeter, kan der dog blot divideres med 58,2.

Arduino Motor Shield

Arduino Motor Shield er lavet til at køre relæ, solenoider, DC motorer (som dem der bruges i opgaven) og stepping motorer. Der er mulighed for at kører to motorer af gangen, hvor der både kan bestemmes hastighed, retning og bremsning for hver enkel. Shieldet er lavet sådan, at den sættes ovenpå Arduino boardets pins, hvorefter det stadig er muligt at tilgå de forskellige pins gennem shieldet. Nogle af Arduino boardets pins er dog allerede brugt til motorstyring. Disse er:

Function

pins per Ch. A

pins per Ch. B

Direction

D12

D13

PWM

D3

D11

Brake

D9

D8

Current Sensing

A0

A1

Selve motorerne tilsluttes via  skrue terminaler med en + og en – pin for hver terminal.

 LEGO motorer

Der er udleveret to DC 9V LEGO motorer. Med denne type motor bestemmes hastigheden af PWM, men motorens modstand, ved fx vægt på robotten, gør det svært at bestemme hvor hurtigt motoren kører for de forskellige PWM. Det er derfor en udfordring at sikre at begge motorer kører lige hurtigt samt at finjustere rotationsgrader. Dette skaber en usikkerhed for hvor meget motoren og dermed også robotten drejer.

Bluetooth modul

Bluetooth modulet er af modellen Bluesmirf fra SparkFun. Ved tilslutning af strøm er modulet allerede klar til at koble sig til computer eller anden bluetooth-enhed  såsom smartphones. For at kunne kommunikere mellem Arduino og forbundet bluetooth-enhed skal der opsættes en serial forbindelse til Bluesmirf gennem Bluesmirfs to pins TX og RX, hvor TX sender data og RX modtager. På Arduino boardet er en RX og en TX pin, som kører over samme Serial, som USB forbindelsen på Arduino boardet normalt bruger. Dette betyder at Bluesmirf og USB konflikter med hinanden når de er forbundet samtidig. For at undgå konflikten kan Arduino biblioteket SoftwareSerial bruges, som bruger Arduinos almindelige digitale pins til henholdsvis TX og RX. Udover udskiftning af pins bruger biblioteket de samme funktioner som det almindelige serial bibliotek. Bibliotekets maks hastighed er 115200 bps, som er den samme hastighed som bluetooth-modulet bruger som standard.

Robottens konstruktion

robotbillede

Robotten er trehjulet, hvoraf to af hjulene har tilsluttet en motor til hver og det sidste er et roterbart  baghjul. Robotten er bygget symmetrisk for at sikrer en fornuftig vægtbalance på de to trækkende hjul , så den ikke kører alt for skævt.  Derudover er hjulene trukket længere frem i forhold til motorerne vha. kæder, da de to sensorer samt batteri er foran motorerne og udgør størstedelen af robottens vægt. Robotten er blevet lavt gearet, så robotten kan få høj PWM uden at hastigheden bliver for voldsom.

Robottens algoritme

Robotten skal forholde sig til to forskellige ting på banen, nemlig underlaget, i form af streg og hvid baggrund, og forhindring. Jeg vil først forklare mine overvejelser omkring robottens håndtering af underlaget.

Underlaget

Robotten har kun en sensor til at sanse underlaget, og den kan kun sanse et koncentreret område (ved robottens position) af gangen. Robotten kan med andre ord ikke forudse banens forløb, og den vil derfor pga. banens snoede forløb komme ud i det hvide felt fordi den ganske enkelt ikke ved hvornår den skal dreje.

I min algoritme kører robotten ligeud indtil sensoren ligger udenfor den sorte streg. I det tilfælde at robotten kommer udenfor den sorte streg søger den efter den sorte streg ved skiftevis at dreje til højre og venstre omkring sin egen akse (ved at køre kontra), hvor graderne den drejer øges for hver gang den har kørt til venstre. Det gradvist øgede søgefelt, er for at undgå tidsspild da robotten sjældent er kommet meget ud af kurs. Det er vigtigt, at sensoren ikke sidder i midten af samme akse som hjulene, da sensoren ellers blot drejer om det samme felt.  Når robotten fanger den sorte streg igen fortsætter den med at køre ligeud som før. I praksis kører robotten dog ikke helt lige, på grund af gamle motorer, og den har derfor en tendens til at trække mod venstre. Af samme årsag har jeg i koden valgt at den skal starte med at søge mod højre, da det er der, at stregen sandsynligvis er.

Forhindringen

Hvad forhindringen angår, kan robotten forudse den før kontakt gennem afstandsmåleren. Den nuværende tanke bag algoritmen er derfor, at robotten skal følge en procedure, når den er tæt ved forhindringen. Proceduren er, at robotten først skal spille en melodi (lidt for sjov), hvorefter den drejer cirka 90 grader om sin egen akse. Derefter kører robotten i en blød bue indtil den rammer den sorte streg igen.

I en fremtidig iteration ville jeg formentlig i stedet for den meget rigide procedure dreje robotten indtil den ikke længere kan se forhindringen. Herefter skal den dreje en smule mere for at undgå at ramme ind i forhindringen, hvorefter den drejer i sin bløde bue. (den fremtidige iteration er implementeret og virker. Derudover er baghjul skiftet ud med en rund bund, der sikrer bedre kørsel.

En anden problematik ved robotten er, at dens bue er meget bestemt af baghjulets position, som af og til sætter sig fast.

Koden

int colorSensorPin = A3;
int val = 0;
boolean started = true;

// Ultra sonic sensor
int echoPin = 7;
int triggerPin = 4;

// Bluetooth device
int btRXPin = 10;
int btTXPin = 5;

#include <SoftwareSerial.h>
//SoftwareSerial mySerial(btRXPin, btTXPin); // RX, TX
SoftwareSerial mySerial(btTXPin, btRXPin); // RX, TX

// Melody
#include “pitches.h”
// notes in the melody:
int melody[] = {
NOTE_C4, NOTE_G3,NOTE_G3, NOTE_A3, NOTE_G3,0, NOTE_B3, NOTE_C4};

// note durations: 4 = quarter note, 8 = eighth note, etc.:
int noteDurations[] = {
4, 8, 8, 4,4,4,4,4 };

void setup() {
// setup serial
//Serial.begin(9600);
mySerial.begin(115200);

// Setting pinModes
// Ultrasonic sensor
pinMode(triggerPin, OUTPUT);
pinMode(echoPin, INPUT);

// Color sensor
pinMode(colorSensorPin,INPUT);
digitalWrite(colorSensorPin, HIGH); // Pull-up resistor – ON

// Motors
//Setup Channel A
pinMode(12, OUTPUT); //Initiates Motor Channel A pin
pinMode(9, OUTPUT); //Initiates Brake Channel A pin

//Setup Channel B
pinMode(13, OUTPUT); //Initiates Motor Channel A pin
pinMode(8, OUTPUT); //Initiates Brake Channel Aa pin

}

void loop(){
// Reads from serial
char go = mySerial.read();
if (go == ‘d’) // Starts the programme if d is pressed
started = true;
if (go == ‘s’) // Stops the programme if s is pressed
started = false;
if (started) { // Runs the programme if started
if(isBlack()) { // Runs the following code if its on the black line
runMotorA(40,true); // Both motors runs forward
runMotorB(40,true);
long distance = calcDistance(); // Calculates the distance
if(distance!=-1 && distance<8) { // If an object is close it runs the following code
breakMotorA();
breakMotorB();
playMelody(); // Plays a melody
runMotorA(100,true); // Makes a turn
runMotorB(100,false);
delay(350);
breakMotorA();
breakMotorB();
while(!isBlack()) {
runMotorA(35,true); // Drives in a curve until it hits the black line
runMotorB(80,true);
delay(10);
}
breakMotorA();
breakMotorB();
}
delay(10);
} else { // The following code runs if its off the black line
breakMotorA();
breakMotorB();
int degree = 15; // This is not actual degrees, but instead a value for how long it drives to each side
boolean right = true; // Its starts with turning right
while(!isBlack()) { // Loop runs until it hits the black line
if(right) { // If the current direction is right
for(int i=0;i<degree; i++) { // The turn is divided into small bits, so it can check if it hits the black line during the turn
runMotorB(50,false);
runMotorA(50,true);
if(degree>15) {
delay(20);
} else {
delay(10);
}

if(isBlack())
break;
}
} else { // If the current direction is left
for(int i=0;i<degree; i++) {
runMotorB(50,true);
runMotorA(50,false);
delay(20);
if(isBlack())
break;
}
degree+=5;
}
breakMotorA();
breakMotorB();

if (right == true) { // changes the current direction
right = false;
} else {
right = true;
}
}
}
} else {
breakMotorA();
breakMotorB();
}
}

void runMotorB(int speed, boolean forward) {
if (forward) {
digitalWrite(12,HIGH);
} else {
digitalWrite(12,LOW);
}
digitalWrite(9,LOW);
analogWrite(3,(int) 255*(speed/100.0));
}
void runMotorA(int speed, boolean forward) {
if (forward) {
digitalWrite(13,HIGH);
} else {
digitalWrite(13,LOW);
}
digitalWrite(8,LOW);
analogWrite(11,(int) 255*(speed/100.0));
}
void breakMotorB() {
digitalWrite(9, HIGH); //Engage the Brake for Channel A
}

void breakMotorA() {
digitalWrite(8, HIGH); //Engage the Brake for Channel B
}

long calcDistance() {
/* The following trigPin/echoPin cycle is used to determine the
distance of the nearest object by bouncing soundwaves off of it. */
digitalWrite(triggerPin, LOW);
delayMicroseconds(2);

digitalWrite(triggerPin, HIGH);
delayMicroseconds(10);

digitalWrite(triggerPin, LOW);
long duration = pulseIn(echoPin, HIGH);

if (duration==0)
return -1;
//Calculate the distance (in cm) based on the speed of sound.
return duration/58.2;
}

boolean isBlack() {
if (analogRead(colorSensorPin)>100) {
return true;
} else {
return false;
}
}

void playMelody() {
for (int thisNote = 0; thisNote < 8; thisNote++) {

// to calculate the note duration, take one second
// divided by the note type.
//e.g. quarter note = 1000 / 4, eighth note = 1000/8, etc.
int noteDuration = 1000/noteDurations[thisNote];
tone(2, melody[thisNote],noteDuration);

// to distinguish the notes, set a minimum time between them.
// the note’s duration + 30% seems to work well:
int pauseBetweenNotes = noteDuration * 1.30;
delay(pauseBetweenNotes);
// stop the tone playing:
noTone(2);
}
}

Konkurrence

Robotten kom på tredjepladsen, da den ikke sansede stregen ordentlig pga. lysindfald. Gns. tiden var ??. Efter der blev trukket persienner for kørte den en runde med forhindring på 18,8 s.

Video

 

Konklusion

Flash Sinatra kan kommunikere med anden bluetooth-enhed gennem Bluesmirf. Den kan endvidere gennemføre banen ud fra sensoriske input. Opgaven har vist, at der kan være stor forskel på teori og praksis når man taler om robotter, da omgivelserne og delene imellem har stor betydning for udfaldet. Dette ses fx når robotten kører skævt eller når et hjul sætter sig fast. Kode der virker på en simulation virker derfor nødvendigvis ikke i praksis.

Links til datablad mm.

Datablad til HCSR04 ultralydssensor

Leave a Reply