Al Zheimer – Arduino Lego robot
Vi har på 4. Semester Optek, i Hardware og Robotteknologi, fået til opgave at lave en Robot ud af Lego som styres af en Arduino. Robotten skal kunne følge en sort streg på et hvidt underlag, og undervejs kunne undgå en forhindring. Robotten skal desuden kunne stoppes og startes over en bluetooth forbindelse.
Til det har vi fået udleveret 2 Lego technic motorer og en Arduino med et motorshield. Derudover har vi lånt en Lego NXT lyssensor, en Lego NXT ultrasonisksensor og et BlueSMiRF bluetooth modem.
Ved opgavens slut vil der være en konkurrence i at gennemføre banen hurtigst muligt, dog under 1 minut.
Hardware
2 x Lego Technic motor
Lego NXT – lys sensor
BlueSMiRF – Bluetooth modem fra Sparkfun
Arduino Uno R3
Arduino Motorshield R3
Startup
Fordi der er flere dele i denne opgave som skal spille sammen, har jeg valgt at fokusere på de enkelte dele, og få dem til at virke hver for sig først, derefter bygge dem sammen til en robot.
Jeg kunne ikke få Lego NXT ultrasonicsensoren til at virke, i stedet bruger jeg en hjemmelavet knap. Mere information om den senere.
Trådløs kommunikation
Til trådløs kommunikation bruger jeg det udleverede bluetooth modem BlueSMiRF. Som standard bruger Bluesmirf serial kommunikation med 115200 Baud, så det skal man lave om, eller også bruge 115200 Baud arduino koden også. For at kunne bruge USB forbindelsen uden at skulle slukke for Bluesmirf, kan man sætte en SoftwareSerial forbindelse op. Med SoftwareSerial kan Bluesmirf køre fra 2 almindelige digitale pins på aduinoen i stedet for at skulle bruge de dedikerede TX og RX pins. Det frigør Arduinoens standard serial forbindelse, så den kan bruges til USB.
Sådan sættes BlueSMiRF til arduino – fra:http://wiring.org.co/learning/tutorials/bluetooth/
Når man bruger Software Serial sættes TX og RX fra Bluetooth modulet til de pins man i sin kode valgt til at være RX og TX. RX til TX og TX til RX.
Jeg fandt eksempel kode til Software Serial på: http://arduino.cc/en/Reference/SoftwareSerial
Her er det kode jeg bruger til bluetooth komminukation:
Derefter kan BlueSMiRF kommunikere med en Android telefon, eller konsollen i Arduino, eller hvad man nu har brug for. Til at styre min robot og overvåge de værdier den sender bruger jeg en gratis App ved navn BlueTerm. Efter at have etableret forbindelse med BlueTerm kan man se hvad der bliver sendt af Arduinoen over bluetooth, og ved at sende tekst beskeder kan man kommunikere med Arduinoen.
Lego NXT lys sensor
Lys sensoren fra Lego NXT er en analog sensor. Målinger skal læses af en analog port på arduinoen. Sensoren giver en spænding, som arduinoen digitaliserer til en værdi mellem 0 og 1023. Den værdi kan så bruges til at vurdere lys forholdene omkring sensoren. For at få en værdi der kan bruges til noget tænder jeg Arduinoens interne pull-up resistor på 20k ohm. Det giver et bedre spænd af værdier, som er til at arbejde med i robottens styrings algoritme.
Lego NXT Lyssensoren sættes til arduinoen ved at skære dens kabel over, også sætte de forskellige ledninger til arduinoen. Lyssensorens kabel har følgende opbygning:
Billede fra: https://sites.google.com/site/mccolganrobotics/
Legokabel – Arduino
Pin 1 – Pin A3
Pin 2 – Gnd
Pin 3 – Gnd
Pin 4 – 5v
Målingerne fra sensoren kan så aflæses med følgende Arduino kode
Motorer og PWM
Når man kun har 2 motorer kan man lave 2 simple former for kørende robot. En hvor den ene motor bruges til at styre robotten, og den anden til fremdrift. Og den metode jeg har valgt, at sætte begge motorer til fremdrift på et hjul i hver sin side. Så kan Styring af robotten opnås, ved at bede om forskellig hastighed på de 2 hjul, lige som en kampvogn.
Motorerne fra Lego Technic er ikke så kraftige, så det er en god idé at lave en gearing ud af lego. Jeg har kun fået lavet en 2:1 gearing ved hjælp et medium legotandhjul på motoren og et stort legotandhjul på hjulets aksel. Mere gearing ville være en god idé, et af de helt små legotandhjul og et af de store ville nok have passet godt.
PWM står for Pulse Width Modulation, og er en metode som kan bruges til at styre strømmen til fx en motor. Modstanden i en motor er rimelig konstant og kan ikke styres, derfor bruger man PWM til at regulere hvor meget strøm motoren skal trække. Et PWM signal ligger mellem 0 og 255, og får Arduinoen til at tænde og slukke for strømmen på den valgte PWM-pin. Ved at pulsere strømmen kan man teoretisk set opnå mellem 0 og 100% motorkræft, uden at ændre på strøm styrke, spænding eller modstand. Imellem at motoren modtager impulser vil motoren bare trille lidt af sig selv. Der kan opstå timings problemer i forhold til motorens faser, så især ved lave hastigheder kan man ikke regne med at få præcis den procent motorkræft man beder om.
Robottens fysiske udformning
Robottens lego karrosseri, kun med motorer
Den færdige robot
Styrings overvejelser
Robotten skal følge en streg. Det kan man bruge lyssensoren til, fordi stregen vil reflektere lys anderledes end underlaget. Fx vil en sort streg reflektere mindre lys end en hvid baggrund.
Da vi kun har 1 lyssensor at arbejde med vil robotten ikke kunne følge stregen direkte. Hvis lyssensoren måler at nu er den ikke over stregen længere, så ved robotten ikke hvilken side af stregen den er kørt ud over. Her vil jeg gennemgå nogle mulige løsninger på dette problem.
Dreje søger – flipflop
Når stregen ikke længere er under lyssensoren, drejer robotten fx mod venstre. Fanger den stregen hurtigt kører den videre lige ud. Går der lang tid fra den drejer til stregen nås, så skal den fortsætte forbi stregen til den finder stregen igen, før den kører lige ud. Hvis den bare kørte når den rammer stregen efter at have drejet i lang tid, vil den følge stregen i den forkerte retning. Det kan også udnyttes, ved at lade robotten køre baglæns i stedet for at vende rundt igen, en metode som jeg vil kalde flipflop.
Myre søger
I stedet for at dreje rundt om sig selv for at finde stregen, kan man lave et lignende søge system hvor robotten i stedet skifter søge retning hvis der går for lang tid med at finde stregen. For så ikke at afsøge det samme område igen og igen kan man få robotten til at køre lidt fremad for hver retningsskift. Det resulterer i en søge mekanisme som ligner den myrer bruger til at finde et spor som er blevet brudt.
Bounce søger
For at slippe for problemerne med ikke at vide hvilken side man skal dreje til for at finde stregen, fik jeg den idé at lave en modsat søger. En robot som forsøger at holde sin sensor over den hvide baggrund i stedet for over stregen. Den kan så laves med en bias mod at dreje ind mod stregen mens den kører. Når den så rammer stregen ændres kørselen midlertidigt til at dreje væk fra stregen.
Metode valg
Da robotten er under tidspres har jeg valgt at forsøge mig med Bounce-søgeren. Fordi jeg tror den vil kunne følge stregen hurtigst. Flipflop er ikke en mulighed fordi min robot fysisk kun har ét baghjul, som ikke egner sig til at køre baglæns. Bounce-søgeren er nok også den mest simple løsning at implementere.
Kasse undvigelse
For at kunne undvige kassen har min robot en hjemmelavet lego knap ude foran. Så når den kører ind i kassen bliver knappen trykket ind, og robotten kan aktivere en undvigelses manøvre. Undvigelses manøvren er preprogrammeret og er ikke afhængig af sensorer, før robotten igen skal finde stregen.
Knappen virker ved at jeg gennem en resistor har sat strøm til en ledning. Når knappen så trykkes ind forbindes en ledning fra en analogpin med den strømførende ledning, og strømmen kan aflæses af den analoge pin.
Styrings implemetering
Jeg har valgt at lave styrings implementeringen ved hjælp af modes. Som basis har jeg 3 simple modes i min kode, kun 1 er aktiv af gangen.
mode 0 – Sæt motorer til 0 i fart – venter på mode ændring over bluetooth
mode 1 – hvidt underlag – skifter til mode 2 hvis lyssensoren måler sort underlag.
mode 2 – sort underlag – skifter den til mode 1 hvis lyssensoren måler hvidt underlag.
Disse 3 modes er nok til at få robotten til at følge en streg. De kræver dog noget fin justering i forhold til motorhastighed.
Når robotten så kan følge en streg aktiveres mode 1 eller 2 over bluetooth, og robotten går igang. Robotten “vrikker” sig fremad ved skiftevis at køre på hver motor og lade en anden trille imens, så den både opnår retnings skift og fremdrift.
For at kunne komme uden om en forhindring har jeg preprogrammeret en sekvens i mode 5, som aktiveres af mode 1 og 2 når knappen bliver trykket ind. Robotten kører ind i kassen, knappen bliver trykket ind, også startes sekvensen til at køre uden om.
Efter sekvensen i mode 5 er færdig, skiftes til modes 6-8 som bruges til at finde stregen igen. Fordi robotten kommer med fart på, så overskyder den stregen første gang den ser den, og kommer derfor på den forkerte side. Mode 7-8 bruges til at få robotten tilbage på den rigtige side.
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);
// mode sættes til en værdi
char mode = 'H';
// ledpin som digital pin 6
const int ledpin = 6;
// deklarering globale variabler
int lightlvl;
int touch;
int lightTres = 260;
// sæt analog input pins
const int lightsense = A3;
const int touchpin = A4;
// 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;
void setup() {
// sæt ledpin som output
pinMode(ledpin, OUTPUT);
// 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);
// sæt lego sensors analog pin til input
// og tænd for arduinos indbyggede pull-up resistor
pinMode(lightsense, INPUT);
digitalWrite(lightsense, HIGH);
// samme for touchpin
pinMode(touchpin, INPUT);
digitalWrite(touchpin, HIGH);
// Definer pins til bluetooth som input/output pins
pinMode(RXPIN, INPUT);
pinMode(TXPIN, OUTPUT);
// Start kommunikation med Bluetooth
BTSerial.begin(115200);
//Serial.begin(115200);
analogWrite(ledpin, 200); // tænd LED
}
void loop() {
// hvis der er data tilgængelig på bluetooth serial
// så læs den, og gem i variabel mode
if( BTSerial.available() )
{
mode = BTSerial.read();
}
// læs sensorer
lightlvl = analogRead(lightsense);
touch = analogRead(touchpin);
if (touch < 900) { mode = '5'; }
// udskriv værdier til bluetooth serial forbindelse
BTSerial.print(lightlvl);
BTSerial.print(" | ");
BTSerial.print(touch);
BTSerial.print(" | ");
BTSerial.println(mode);
if( mode == '0' ) // mode 0 - stop motorer, sluk LED
{
digitalWrite(ledpin, LOW); // sluk LED
// Stop
analogWrite(speedA, 0);
analogWrite(speedB, 0);
} else if( mode == '1' ) // kør mode 2, drej venstre, hen mod streg
{
analogWrite(ledpin, 200); // tænd LED
analogWrite(speedA, 0);
analogWrite(speedB, 80);
if (touch < 900) { mode = '5'; } // hvis robotten rammer noget, skift til mode 5
else if (lightlvl > lightTres) { mode = '2'; }
} else if ( mode == '2' ) // kør mode 2, drej højre, væk fra streg
{
analogWrite(speedA, 80);
analogWrite(speedB, 0);
if (touch < 900) { mode = '5'; }
else if (lightlvl < lightTres) { mode = '1'; }
} else if ( mode == '3' ) { // mode til testing
analogWrite(speedA, 0);
analogWrite(speedB, 60);
if (lightlvl > lightTres) { mode = '4'; }
} else if ( mode == '4' ) { // test mode
analogWrite(speedA, 60);
analogWrite(speedB, 0);
if (lightlvl < lightTres) { mode = '3'; }
} else if ( mode == '5' ) { // undgå kasse
// motor pause
digitalWrite(brakeA, HIGH);
digitalWrite(brakeB, HIGH);
delay(100);
digitalWrite(brakeA, LOW);
digitalWrite(brakeB, LOW);
// bak væk fra kasse
digitalWrite(dirA, HIGH); // motor a baglæns
digitalWrite(dirB, LOW); // motor b baglæns
analogWrite(speedA, 100);
analogWrite(speedB, 0);
delay(300);
analogWrite(speedA, 0);
analogWrite(speedB, 90);
delay(300);
// motor pause
digitalWrite(brakeA, HIGH);
digitalWrite(brakeB, HIGH);
delay(100);
digitalWrite(brakeA, LOW);
digitalWrite(brakeB, LOW);
// drej uden om kasse, med bremse på den ene hjul
digitalWrite(dirA, LOW); // motor A fremad
digitalWrite(dirB, HIGH); // motor B fremad
analogWrite(speedA, 120);
analogWrite(speedB, 0);
digitalWrite(brakeB, HIGH); // brems B
delay(600);
digitalWrite(brakeB, LOW); // brems B fra
// kør fremad
analogWrite(speedA, 70);
analogWrite(speedB, 70);
delay(300);
// pause
analogWrite(speedA, 0);
analogWrite(speedB, 0);
delay(200);
// fang stregen igen
analogWrite(speedA, 30);
analogWrite(speedB, 60);
if (lightlvl > lightTres) { mode = '7'; }
} else if ( mode == '7' ) { // tilbage på stregen
analogWrite(speedA, 100);
analogWrite(speedB, 0);
if (lightlvl < lightTres) { mode = '8'; }
} else if ( mode == '8' ) { // tilbage til den rigtige side af stregen
analogWrite(speedA, 70);
analogWrite(speedB, 0);
if (lightlvl > lightTres) { mode = '1'; }
}
delay(5);
}
Konkurrence
Konklusion
Al Zheimer virker! Det er lykkedes at lave en robot som kan klare opgaven.