Implementering av ett inbyggt system för automatisk styrning av en robotbil.

(1)

School of Mathematics and Systems Engineering

Reports from MSI - Rapporter från MSI

Implementering av ett inbyggt system

för automatisk styrning av en robotbil.

Martin Aldrin

Jun

2007

MSI Report 07067

Växjö University ISSN 1650-2647

(2)

Organisation/ Organization

Författare/Author(s)

VÄXJÖ UNIVERSITET

Matematiska och systemtekniska institutionen

Martin

Aldrin

Växjö University/

School of Mathematics and Systems Engineering

Dokumenttyp/Type of document

Examensarbete/ Diplomawork

Titel och undertitel/Title and subtitle

Implementering av ett inbyggt system för automatisk styrning av en robotbil.

Implementation of an embedded system for automatic guidance of a robot car.

Sammanfattning (på svenska)

Denna rapport beskriver ett examensarbete för högskoleingenjörsexamen i elektroteknik vid Växjö

Universitet. Syftet är att konstruera ett styrsystem till en robotbil som hindrar bilen från att krocka

med objekt i omgivningen när bilen rör sig utan extern kontroll. Roboten ska kunna styras direkt från

en dator via ett virtuellt instrument, ett grafiskt gränssnitt implementerat i Labview. Nödvändig

hårdvara för styrning och kommunikation har konstruerats. Mjukvaran har implementerats i C, Perl

och Labview.

Nyckelord

Labview, styrsystem, robotbil, robot. inbyggda system

Abstract (in English)

This thesis describes a project for the bachelor degree in electrical engineering at Växjö University.

The purpose with this project is to construct a guidance system for a robot car, a program that

prevents the car from colliding with objects when moving without external control. The robot could

also be controlled from the computer through a virtual instrument implemented in Labview. The

necessary hardware for steering and communicating has been constructed. The software is

implemented using three different programming languages, C, Perl and Labview.

Key Words

Labview, guidance system, robot car, robot. embedded system

Utgivningsår/Year of issue

Språk/Language

Antal sidor/Number of pages

2007 Swedish 43

(3)

Sammanfattning

Denna rapport beskriver ett examensarbete för högskoleingenjörsexamen i elektroteknik

vid Växjö Universitet. Syftet är att konstruera ett styrsystem till en robotbil, ett program

som hindrar bilen från att krocka med omgivningen. Roboten ska även kunna styras från

en dator via ett grafiskt gränssnitt implementerat i Labview. Nödvändig hårdvara för

styrning och kommunikation har konstruerats.

Det har behövts tre olika programmeringsspråk för att nå de krav som har ställts på

uppgiften, C, Perl och Labview. Microprocessorn i robotbilen har programmerats i C

och gör bilen helt autonom, endast beroende av signaler från avståndssensorer.

Avlusningsprogrammet skrevs i Perl och styrningen från datorn har implementerats i

Labview. Avlusningsprogrammet togs fram på grund av att det blev svårt att hålla koll

på allt som skedde med värden och beräkningar i den automatiska styrningen av

robotbilen.

(4)

Innehållsförteckning

1 Bakgrund ... 1

2 Problemformulering ... 2

3 Hårdvara ... 3

3.1 IR-sensorer (Sharp GP2D12) ... 3

3.2 Ultraljud (SRF04) ... 3

3.3 LCD-Display ... 3

3.4 Servo (HS-311) ... 4

3.5 Motorstyrkort (DHB-01) ... 4

4 Resultat ... 5

4.1 Styrkortet ... 5

4.2 Programmeringsuppbyggnad - det automatiska styrsystemet. ... 6

4.2.1 IR-sensorerna ... 6

4.2.2 Servot ... 6

4.2.3 Ultraljudet ... 7

4.2.4 Motorstyrning ... 7

4.2.5 Styrning och hastighetsreglering ... 8

4.2.6 Avlusningsprogram (Debugging-program) ... 10

4.3 Styrning via Labview ... 11

5 Resultatdiskussion ... 13

6 Slutdiskussion ... 13

7 Referenser ... 14

8 Bilagor ... 15

8.1 Automatisk styrning av robotbil. ... 15

8.1.1 Upp1.h ... 15

8.1.2 Upg1.c ... 17

8.1.3 Peripheral.c ... 19

8.1.4 LCD4.C ... 24

8.2 Avlusningsprogram ... 26

8.3 Mjukvara Robotbil för styrning från Labview. ... 35

8.3.1 Uppgift3.h ... 35

8.3.2 Uppgift3.c ... 35

8.4 Programkod Labview ... 42

(5)

8.4.2 Motorkontroller ... 43

8.4.3 RS232-funktion ... 43

8.4.4.Ultraljudsfunktion ... 44

1.1.5 IR-funktion ... 44

8.5 Styrkortsschema och Layout. ... 45

Figurförteckning

Figur 1 – Robotbil. ... 3

Figur 2 – IR-sensor. ... 3

Figur 3 - Ultraljudssignaler ... 3

Figur 4 – Ultraljudssensor. ... 3

Figur 5 – Servopulslängd. ... 4

Figur 6 – Motorstyrningskort ... 4

Figur 7 - Anslutningar till PIC-processorn. ... 5

Figur 8 - Styrkort i 3D perspektiv. ... 5

Figur 9 - Ultraljudspositioner. ... 7

Figur 10 – Flödesschema. ... 9

Figur 11 - Avlusningsprogram ... 10

Figur 12 - Labview Frontpanel. ... 12

Tabellförteckning

Tabell 1: Labview - kommandon... 11

(6)

1 1 Bakgrund

Examensarbetet har tagits fram av Växjö Universitet för att kunna användas som

utbildningsmaterial i eventuella laborationer för Labveiw och andra

programmeringskurser, t.ex. Realtidssystem och C programmering i inbyggda system.

Uppgiften har varit att se över möjligheterna med robotbilen och se vad som är möjligt

att göra med den typen av utrustning. Rapporten ska också kunna fungera som en

manual.

Rapporten är upplagd enligt följande. I kapitel 2 ges en problemformulering där de

olika delarna och kraven i projektet beskrivs. Vidare, i kapitel 3 beskrivs den hårdvara

som är central och specifik för robotbilen. Kapitel 4 beskriver hur de olika

komponenterna har satts samman och programmerats. Kapitel 5 och 6 förklarar problem

som uppstått under arbetets gång och möjliga förbättringar som vore lämpliga att

(7)

2 2 Problemformulering

Komponenter att tillgå är en bil med fyra motorer, tre IR-sensorer, en ultraljudssensor,

ett servo, ett motorstyrningskort, en LCD samt ett utvecklingskort för Microchips

processor PIC16F877A. I uppgiften ingår även att tillverka ett styrkort till robotbilen.

Valet av programmeringsspråk och processor från Microchip är fritt, men även hur

styrkortet ska bestyckas och hur informationen ska skickas till datorn, dvs. via USB

eller RS232. Roboten ska kunna programmeras ”in circuit”, vilket betyder att

processorn ej behövs tas bort vid programmeringen.

Mjukvara för robotbil. Robotbilen ska köra av sig själv utan att stöta emot hinder och

all utrustning ska gå att använda samtidigt som bilen körs. Vid hinder så ska lämplig

åtgärd utföras, så som att svänga eller backa för att undvika att robotbilen kör in i något.

Sensorerna får placeras fritt efter vad som anses vara lämpligt. Funktioner som bilen ska

vara utrustad med är hastighetsreglering som är beroende på avståndet till hinder,

utskrift av värden från sensorer och beräkningar till LCD eller utskrift till dator.

Labview. Robotbilen ska kunna styras från Labview. Kommandon ska skickas till

robotbilen för att läsa av sensorernas värden, hastighet och för att styra den. Det är

valfritt att implementera denna del i samma program som det automatiska eller om det

görs i ett separat program. Viktigt är att Labview endast ska kunna komma åt de

grundläggande funktionerna, dvs. det som behövs för att kunna styra roboten, då

huvudfunktionerna för robotbilens drift ska göras i Labview. I Labview implementeras

en grundläggande mjukvara för att visa att alla funktioner fungerar.

(8)

3 3 Hårdvara

Hårdvaran består av en fyrhjulsdriven bil (4WD1

Robot) [1] som är bestyckad med fyra DC-motorer,

tre IR-sensorer, en ultraljudssensor, ett servo, en

fyra bitars LCD med 16x2 rader, ett motorstyrkort

och ett egentillverkat styrkort som är baserat på en

PIC16F877A.

Figur 1 – Robotbil.

3.1 IR-sensorer (Sharp GP2D12)

Två av IR-sensorerna har placerats i fronten på sådant sätt

att de korsar varandra, denna placering gjordes för att

roboten ska kunna se hinder på båda sidor. Den tredje har

placerats baktill på roboten. Sensorerna klarar avstånd som

är mellan 10 och 80 cm. De fungerar så att ju närmare de är

föremålet, desto högre spänning utvecklas.

Figur 2 – IR-sensor.

3.2 Ultraljud (SRF04)

Ultraljudssensorn har placerats i fronten av robotbilen på ett servo. Sensorn klarar av

mätningar på upp till två meters avstånd. Sensorn sänder ut en akustiskt pulsformad

signal med en frekvens på 40 KHz. Tiden mellan signalens sändning och mottagning

motsvaras av två gånger sträckan till objektet som reflekterar signalen.

Figur 3 - Ultraljudssignaler

1. Trigger som ultraljudet startas av 2. Ljudet som skickas ut.

3. Ekopulsen som fås tillbaka.

Figur 4 – Ultraljudssensor.

3.3 LCD-Display

Displayen är en XIAMEN OCULAR GDM1602A. Displayen använder sig av en

styrkrets av typen Samsung KS0066U [9]. Displayen har placerats på ett egentillverkat

kort som anpassats för att köras med fyra bitar. Orsaken att fyra bitar har valts är för att

inte blockera in-/utgångarna på PIC processorn. En nackdel med att bara använda sig av

fyra bitar är att displayen blir långsammare. Displayen har ytterligare tre anslutningar,

Enable, RS och R/W. Den sistnämnde har anslutits till jord, därför står displayen alltid i

(9)

4 3.4 Servo (HS-311)

Servot är ett standard RC-servo. Det styrs av elektroniska pulser som skickas till servot;

pulslängden är mellan 1-2 ms och periodlängden är 20 ms där 1,5 ms är centrum. Se

bild nedan. Pulsen som skickas behöver upprepas ett flertal gånger för att servot ska nå

den önskade positionen. Om antalet pulser överskrider det maximala antalet pulser som

krävs för att ta sig till den önskade positionen bibehåller servot samma position tills

pulsbredden ändrats.

Figur 5 – Servopulslängd.

Figuren visar exempel på hur en puls kan se ut.

3.5 Motorstyrkort (DHB-01)

Kortet består av en dubbel H-brygga, som kan leverera en maxström på 1 ampere på

bägge kanalerna. Kortet har två ingångar för PWM-signaler samt fyra ingångar så att

motor och riktning ska kunna väljas.

Figur 6 – Motorstyrningskort

(10)

5 4 Resultat

4.1 Styrkortet

Styrkortet är baserat på en PIC16F877A från Microchip [2] som arbetar med en

klockfrekvens på 4 MHz. I övrigt består kortet av programmerbara knappar, en

reset-knapp, en strömbrytare samt en RS232-anslutning via en MAX232-krets [10]. Kortet

har även en anslutning för ”in circuit”-programmering.

Kretsen stödjer endast att programmeras med 5 Volt och där igenom har en

in-/utgång på port B sparats. Under programmeringen får ingen extern utrustning vara

ansluten till de specifika ben på processorn som används vid programmering. Skulle

någon extern utrustning vara ansluten till programmeringsingångarna så kan inte

processorn hittas på korrekt sätt av programmeringsprogrammet, i detta fall Melabs

Programmer V4.1.

Kretsschema och layouten för mönsterkortet har gjorts i EDWinXP 1.5.

Figur 7 - Anslutningar till PIC-processorn.

Bilden visar vad som har anslutits till de olika in/-utgångarna på PIC processorn.

(11)

6 4.2 Programmeringsuppbyggnad - det automatiska styrsystemet.

Programmet som har använts för att skriva koden heter PCWH Compiler version 4,013

[3]. Valet föll på detta program eftersom viss erfarenhet av tidigare versioner fanns.

Till en början testades all extern utrustning på det utvecklingskort som fanns att

tillgå. Detta gjordes för att se att allt fungerade samt för att veta hur all extern

utrustningen skulle kopplas på det senare egentillverkade styrkortet. Se bilaga 8.1.

4.2.1 IR-sensorerna

IR-sensorerna anslöts till de analoga ingångarna på PIC-processorn. Genom det här

tillvägagångssättet fås ett referensvärde mellan 0 och 127. En nolla som referensvärde

motsvarar att inget hinder finns inom räckhåll. Ju högre referensvärde som fås desto

närmare befinner sig hindret.

4.2.2 Servot

Servot [4] styrs med pulser som varierar mellan en och två ms, med en periodtid på 20

ms. För att åstadkomma dessa pulser används en timer med tillhörande

avbrottsfunktion. Servot behöver ha 1200 upprepningar av pulsen som skickas för att

göra en total vridning motsvarande 180 grader..

Timern är en 16-bitars realtidstimer (RTCC). Servot har programmerats så att det

ligger och sveper fram och tillbaka under tiden robotbilen kör. I avbrottsfunktionen som

styr servot finns det även flera villkor när ultraljudet får läsas av beroende på positionen

servot befinner sig i. Positionerna då ultraljudet läses av har bestämts genom att dela

upp antalet upprepningar det tar att gå från ena sidan till den andra i intervall.

Positionerna då servot tillåts läsa kan variera, detta har gjorts på detta sätt för att annars

skulle inte ultraljudet alltid hinna med att läsa av värdena för den aktuella positionen.

(12)

7 4.2.3 Ultraljudet

Ultraljudet aktiveras genom att en signal på 5 volt skickas till ingången på sensorn

under 10 µs för att sedan direkt ändras till 0 volt igen. Ultraljudssensorn skickar ut en

akustisk ultraljudsignal, ett ljud med hög frekvens som sedan reflekteras tillbaka om det

finns föremål inom räckhåll. Tiden mellan skickad och mottagen signal varierar

beroende på avståndet till det reflektiva föremålet, denna ekopuls mäts med hjälp av en

timer, i detta fall timer1.

Utifrån tiden som fås från timern, beräknas en skalfaktor för att få avståndet i

centimeter. Detta görs genom att mäta upp avståndet från robotbilen till föremålet som

ultraljudet är riktat mot. Skalfaktorn beräknades till 58, tiden dividerades sedan med

denna faktor varje gång ultraljudet läses av.

Avståndet till föremål framför ultraljudssensorn avläses på sju olika servopositioner.

Tre värden på vardera sida om roboten samt ett i centrum. Positionerna kan variera lite

beroende på när de läses av. Anledningen till varför inläsningen av ultraljudet har valts

att göras på detta sätt beror på att servot ska röra sig kontinuerligt och aldrig stanna upp.

Det är av mindre betydelse för den här uppgiften att servopositionen är exakt, det

viktiga är att inläsningen görs på rätt sida.

Figur 9 - Ultraljudspositioner.

Bilden visar de olika positionerna i fronten av bilen som ultraljudet får sina värden.

4.2.4 Motorstyrning

Samtliga motorer styrs via ett motorstyrningskort. Det måste styras på det här sättet då

processorn inte klarar av att leverera den ström som krävs samt för att det smidigt ska

gå att ändra riktning på roboten. Från processorn skickas det kontrollsignaler som talar

om riktning och hastighet som motorerna ska ha.

Riktningen styrs genom att ändra spänningen på respektive ingång till 5 volt på

motorstyrningskortet. Hastigheten styrs med hjälp av PWM-pulser. Processorn har två

färdiga funktioner för hantering av PWM-pulser. Genom att aktivera dessa två utgångar

samt ställa in timer2 kan hastigheten regleras genom att ändra på ”duty cycle”.

Värdena på timer2 har angetts så att ”duty cycle” kan ställas mellan 0 och 50. De

lämpligaste värdena att använda är från 34-50 där 50 motsvarar full hastighet. Anges

värden under 34 orkar inte motorerna driva bilen då PWM-pulsernas längd blir för

korta.

(13)

8 4.2.5 Styrning och hastighetsreglering

Styrningen baseras på de värden som inhämtas från IR-sensorerna och ultraljudet,

medan hastigheten enbart regleras via IR-sensorerna. Styrningen är konstruerad så att

om spänningen från någon IR-sensor hamnar under minimivärdet som har fördefinierats

i programmet, så backar ena motorn medan den andra kör framåt ända tills värdena har

gått över minvärdet igen. Skulle båda sensorerna i fronten hamna under minvärdet så

backar roboten ända tills sensorsignalens värde är över minvärdet. Bilen backar även

om ultraljudet får ett minvärde från centrumpositionen. En nackdel som uppkommer när

robotbilen styrs med hjälp av ultraljudet i fronten är servot. Detta på grund av tiden det

tar för servot att göra en svepning. Det medför att robotbilen svänger mycket kraftigare

om minvärdet kommer från ultraljudssensorerna än om det skulle komma från

IR-sensorerna.

(14)

9

Figur 10 – Flödesschema.

(15)

10 4.2.6 Avlusningsprogram (Debugging-program)

Det insågs ganska snart att det blev näst intill omöjligt att hålla koll på vad som skedde

med alla beräkningar som hanterade styrning och hastighet. Ett försök gjordes att avlusa

programmet via displayen, men det räckte inte då den bara kunde visa 16x2 tecken.

Programmet ändrades då om för att avlusas via RS232-protokollet i

terminalprogrammet Hyper-Terminal. Det blev betydligt bättre då mer mätdata kunde

visas samtidigt, men ingen riktigt bra struktur gick att få. Det blev helt enkelt för

mycket information att hålla koll på. Ett avlusningsprogram togs sedermera i bruk som

skrevs i Perl, genom det här programmet kunde all mätdata från sensorerna och

beräkningarna visas mycket överskådligt i olika programfönster.

Programmeringsspråket Perl [5,6] valdes dels för sin smidiga stränghantering, men

även för att det är mycket lättanvänt utan större förkunskaper. Alla strängar som skickas

från robotbilen förses med två starttecken i början av strängen. Genom att göra på det

här viset kunde mätdata filtreras ut genom att granska de första tecknen som kom in.

Utan att ha tagit fram det här programmet hade det varit mycket svårt att hitta alla

logiska fel och buggar som fanns i koden. Hastigheten som används för att skicka data

över RS232 är 9600 bps. Se billaga 8.2.

(16)

11 4.3 Styrning via Labview

Styrningen via Labview har gjorts i ett separat program, det gjordes på grund av ett

minnesproblem som uppkom vid försök av implementering i det befintliga programmet.

Det är dock möjligt att implementera Labview-delen i det befintliga programmet.

Anledningen till varför detta inte har gjorts beror på att det blir svårläst för de som ska

läsa rapporten. För att kunna implementera koden för Labview skulle variablerna

behövts användas mycket flitligare och alla utskrifter av strängarna till

avlusningsprogrammet skulle ha blivit tvungna att tas bort. Mjukvaran i robotbilen

skiljer så att all automatiskkontroll har tagits bort då den styrs ifrån Labview.

Programmet i robotbilen för styrning ifrån Labview sköter all kommunikation till

sensorerna och motorerna på ett likadant sätt som för det automatiska styrsystemet. Det

som skiljer sig är att funktionerna utförs efter inkommande kommando.

Kommandon skickas i form av strängar till robotbilen som filtreras ut innan de

utförs. Efter det skickas värdens tillbaka, även dessa i form av strängar. I Labview [7]

visas mätdata i form av grafer och olika typer av mätare som visar sensorernas värde

och den hastighet som bilen har. Strängarna har anpassats så att de ska vara mycket lätta

att tyda i Labview. Alla strängar har ett kolon framför värdena. Labview har färdiga

funktioner för att leta upp och hämta värden ur strängar. Se tabeller nedan för exempel

på kommandon. Kommunikationen från Labview till robotbilen sker via

RS232-gränssnittet med en hastighet på 14400 bps. Se billaga 8.3 och 8.4.

Enheter Kommando Vänster/Höger/Back Riktning Hastighet Exempel

Motor M L/R 1/0 0-50 MR150

IR IR L/R/B - - IRL

Ultraljud US 9 - - US9

Servo US - 0-8 - US1

Tabell 1: Labview - kommandon.

Tabellen visar de olika kommandon som kan skickas till robotbilen. Det går bra att använda både versaler och gemener.

Enheter Kommando Exempel på strängar som skickas tillbaka

Motor MR150 Left_Dir:1,LeftSpeed:50

IR IRL IR_Left:45

Ultraljud US9 US9:20

Servo - -

Tabell 2: Labview – Exempel på strängar.

(17)

12

Figur 12 - Labview Frontpanel.

(18)

13 5 Resultatdiskussion

Robotbilen förhindrar kollision på egen hand genom styrning på sensorernas signaler

med hjälp av implementerad mjukvara för processorn. Inga yttre påverkande signaler

behövs. Problem kan inträffa då hinder är för smala som till exempel stolsben men

också om objektet i höjdled är över eller under sensorerna.

Anledningen till valet av processor var att ett utvecklingskort för PIC16F877A fanns

att tillgå. All utrustning kunde därmed testas innan tillverkningen av det egna styrkortet.

En krets som annars hade varit mycket lämplig för ändamålet hade varit PIC18F4550,

eftersom den har mer internt minne och stöd för USB 2.0-gränssnittet.

Några närmare beräkningar har inte behövts göras för att se om processorn klarar av

kraven som ställts, då leverantören av robotbilen även säljer styrkort som har bestyckats

med en OOPIC [8]. OOPIC är en krets som är baserad på en PIC16F877A och den har

många färdiga funktioner för att styra robotar. OOPIC-kretsen programmeras aldrig om

eftersom all egen programkod läggs in på en EEPROM-krets.

Problem som uppstått under arbetets gång är relaterat till servofunktionen. Det har

grundats på tankefel gällande inställningar för timern och avbrottsfunktionen. Ett antal

problem för avbrottsfunktionen inträffade också i Labview-delen, då en funktion för

inläsning av data från RS232-porten användes. Det sistnämnda problemet uppkom då

tre funktioner kördes samtidigt och styrdes från Labview, avbrottsrutinen, ultraljudet

och kommunikationen för RS232. Kommunikationsproblemen till robotbilen från

datorn uppkom bara när bilen styrdes från Labview och inte ifrån Hyper-Terminalen.

Servofunktionen fick därför ändras om och resulterade i att servopositionerna inte

riktigt fördelades jämt över 180 grader. En lösning på det här problemet skulle kunna

vara att höja klockfrekvensen på PIC-processorn till 20 MHz istället för 4 MHz som

processorn för närvarande jobbar med.

6 Slutdiskussion

Det är mycket lämpligt att komplettera roboten med fler sensorer, främst IR-sensorer då

avläsningen sker snabbare än med ultraljud, men också för att de är lättare att

implementera och utnyttja för hastighetsreglering. En annan funktion som skulle

underlätta är att bygga in stöd för trådlös kommunikation för att på så sätt kunna styra

robotbilen via Labview utan anslutningskabel men också för att kunna avlusa bilen med

avlusningsprogrammet när den är i drift på ett smidigare vis.

(19)

14 7 Referenser

[1] Lynxmotion (2007-05-26).

http://www.lynxmotion.com/

[2] PIC16F877A Datablad (2007-05-26).

http://ww1.microchip.com/downloads/en/DeviceDoc/39582b.pdf

[3] PCWH Compiler Maunal (2007-05-26).

http://www.ccsinfo.com/downloads/ccs_c_manual.pdf

[4] Martin Fred, Robotic Explorations: A Hands-on Introduction to Engineering,

Prentice Hall, New Jersey, 2001, ISBN 0-13-089568-7

[5] Perl: Exempel kod. (2007-05-26).

http://www.perl.com/pub/a/1999/10/perltk/index.html

[6] O´Relilly´s bookshelf: Referens Libary (2007-05-26).

http://hell.org.ua/Docs/oreilly/

[7] National Instruments – Labview (2007-05-26).

http://www.ni.com/labview/

[8] Totalrobots (2007-05-26).

http://www.totalrobots.com/

[9] Styrkrets Samsung KS0066U (2007-05-26).

http://www.ortodoxism.ro/datasheets/SamsungElectronic/mXuuzvr.pdf

[10] MAX232 Datablad (2007-05-26).

http://rocky.digikey.com/WebLib/Texas%20Instruments/Web%20data/MAX232,232I.p

df

(20)

15 8 Bilagor

8.1 Automatisk styrning av robotbil.

8.1.1 Upp1.h

#ifndef uppg1 //Defininerar Variabler och funktioner om de inte blivit definerade innan #define uppg1

#include <16F877A.h> #device adc=8

#use delay(clock=4000000)

#fuses NOWDT,XT, NOPUT, NOPROTECT, NODEBUG, BROWNOUT, NOLVP, NOCPD, NOWRT #use rs232(baud=9600,parity=N,xmit=PIN_C6,rcv=PIN_C7,bits=8)

/*Konstanter till Servot och Ultraljudet*/ #define INTS_PER_SECOND 3840 #define INTS_PER_MILLISECOND 2 #define SERVO_CYCLE 20 #define NO_OF_ACTIONS 1200 #define STATE0 0 #define STATE1 1 #define STATE2 2 #define STATE3 3 #define STATE4 4 #define STATE5 5 #define STATE6 6 #define STATE7 7 #define L_TO_R true #define R_TO_L false #define GO true /*Adressering av portar*/ #byte port_d = 8 #byte port_c = 7 #byte port_e = 4 /*Variabler för INTERRUPT*/

byte seconds=0,milli_seconds=0; // A running seconds counter

long int int_count=INTS_PER_SECOND; // Number of interrupts left before a second has elapsed byte int_millicount=INTS_PER_MILLISECOND; /*Sträng för utskrift till RS232*/ char strang[90]; /*Variabler för Hastighet*/ byte LeftSpeed=0,RightSpeed=0; byte IR_Left=0,IR_Right=0,IR_Back=0; byte IR_L_dir=1,IR_R_dir=1; byte Left_Dir,Right_Dir; byte last_left_dir,last_right_dir;

/*Variabler som tillhör Ultrasonic och Servo*/ byte US_State = STATE0,US_state2=STATE0; byte action = 0;

long actioncounter = 0; byte servo_array[] = {0,4}; byte servo_array_pointer = 0; byte Servo_High=0;

byte servo_state = GO;

long D1=0,D2=0,D3=0,D4=0,D5=0,D6=0,DC=0; boolean Servo_Home = true;

boolean Servo_direction = L_TO_R; char servodir = 'A';

(21)

16

/*Funktioner*/ void init();

byte read_analog(byte channel); int UltraSonic(Void);

void UltraSonic_Read_Pos();

void pwm(int Duty_Left,Duty_Right);

void Direction_Control();//boolean Direction_Control(Int IR_Left,IR_Right,IR_Back); void print(char *vector);

void Meny(void); long Speed_Control(); void Read_IR(void); void init(){

seconds = 0;

setup_timer_0( RTCC_INTERNAL | RTCC_DIV_1 ); setup_adc_ports(AN0_AN1_AN2_AN3_AN4); setup_adc(ADC_CLOCK_INTERNAL); setup_psp(PSP_DISABLED); setup_spi(FALSE); setup_timer_1(T1_INTERNAL); setup_ccp1(CCP_PWM); setup_ccp2(CCP_PWM); setup_comparator(NC_NC_NC_NC); setup_vref(FALSE);

set_tris_c(0x00); //Sätter PortC till utgångar för styrning av motor (C0-C5), C6 &C7 används till RS232.

set_tris_d(0x02); //Sätter portD till utgång för syrning av UltraSonic (D0-D1). //lcd_init(); //initierar LCD

//lcd_clear();

set_rtcc(0); //Startar realtidsklocka som används till utskrivt och Servot. enable_interrupts(INT_RTCC);

enable_interrupts(GLOBAL); }

(22)

17 8.1.2 Upg1.c

#include <upg1.h> #include <peripheral.c>

#int_rtcc // This function is called every time

void clock_isr() { // the RTCC (timer0) overflows (255->0). if(--int_millicount==0) { if (action == 1 || Servo_Home==true){ if(milli_seconds < Servo_High+1){ Output_High(PIN_E2); }else{ Output_Low(PIN_E2); }if(actioncounter>=1200){ Servo_Home=False; } } milli_seconds++; if (milli_seconds > SERVO_CYCLE){ milli_seconds = 0; }if(servo_state == GO ){ actioncounter++;

}if (actioncounter > NO_OF_ACTIONS){ action = 0;

actioncounter = NO_OF_ACTIONS+1;

}if((actioncounter>1200) && (Servo_Home !=True)){ US_state = STATE7; // Stay in STATE6 until next second }else if((actioncounter>1000) && (Servo_Home !=True)){ US_state = STATE6;

}else if((actioncounter>900) && (Servo_Home !=True)){ US_state = STATE5;

}else if((actioncounter>550) && (Servo_Home !=True) && (actioncounter<650)){ US_state = STATE3;

}else{

US_state = STATE0; }

int_millicount=INTS_PER_MILLISECOND; }if(--int_count==0) { // per second. ++seconds; int_count=INTS_PER_SECOND; if ((US_State==STATE7)){ US_state=STATE0; if (Servo_Direction == L_TO_R){ Servo_Direction = R_TO_L; }else{Servo_Direction = L_TO_R;} actioncounter = 0; Servo_High = servo_array[servo_array_pointer++]; if (servo_array_pointer > 1){ servo_array_pointer = 0; }

action = (action + 1)&1; } if (seconds >= 59){ seconds = 0; } } }

(23)

18

void main() { init(); port_d=0x00; while(1){ UltraSonic_Read_Pos(); Read_IR(); Direction_Control();//IR_Left,IR_Right,IR_Back,IR_L_dir,IR_R_dir Speed_Control();//IR_Left,IR_Right,IR_Back,IR_L_dir,IR_R_dir Set_Speed();//LSpeed,RSpeed,IR_L_dir,IR_R_dir } // end while(1) } // end main

(24)

19 8.1.3 Peripheral.c

#include <upg1.h> #define INT_LENGTH 10 #define FORWARD 1 #define BACKWARD 0

#define MIN_US_DISTANCE 18 //Avstånd i cm. #define Max_US_DISTANCE 30

#define MAX_IR_DISTANCE 65 //Ju högre värde dessto närmare roboten. #define MIN_IR_DISTANCE 45

#define MAX_SPEED 100 #define REAL_MAX_SPEED 50 #define MIN_SPEED 60

#define BACK_SPEED 100

void print(char *vector){ //Skriver strängar till RS232. int i = 0;

char vcpy[80];

strcpy(vcpy, vector); //Kopierar vår pekare till en variabel. while(vcpy[i]!= '\0' ){

putc(vector[i++]); }

}

//UltraSoic, Triggar på D0 och läser på D1 int UltraSonic(Void){

long Distance=0; output_high(PIN_D0);

delay_us(10); //Måste vara minst 10us. output_low(PIN_D0);

while(!input(PIN_D1)); //Stannar här tills det inte finns någon signal på US. set_timer1(0); //Nollställer timer1

while(input(PIN_D1)); //Stannar så länge som man har signal på ingången. Distance = get_timer1(); //läser av Timer värdet.

Distance=Distance/58; //Räknar om tiden till cm skalfator 58 vid 4MHz 300 vid 20MHz. Distance=(int)Distance; //Omvandlar long till Integer.

Return Distance; }

//Read AD-Channels and print it on LCD. byte read_analog(byte Channel){

int Analog_Value;

SET_ADC_CHANNEL(Channel); //Tar emot värde för vilken ingång som ska läsas av. analog_value = read_adc(ADC_START_AND_READ); //Läser av ingången.

return Analog_Value; }

void pwm(int Duty_Left, Duty_Right){ /*

if((Right_Dir==BACKWARD) && (Left_Dir==BACKWARD)){ Duty_Left=100; Duty_Right=100; }*/ if(Servo_Home==true){ Duty_Left=0; Duty_Right=0; }

setup_timer_2(T2_DIV_BY_1,51,2); //Sätter upp Timer 2 för få rätt PWM. set_pwm1_duty(Duty_Left); //PWM signal för Vänster motor

set_pwm2_duty(Duty_Right); //PWM signal för höger motor. }

(25)

20

/*Funktion som styr riktningen på motorena fram eller bak, är beroende av värden som fås från sensorena*/

void Direction_Control(){

//IR_Left,IR_Right,IR_Back,IR_L_dir,IR_R_dir

if ((Last_Left_Dir == FORWARD) && (Last_Right_Dir == FORWARD) ){ if((D1>MIN_US_DISTANCE)&&(D2>MIN_US_DISTANCE)&&(D5>MIN_US_DISTANCE)&&(D6 >MIN_US_DISTANCE)){ Left_Dir = FORWARD; Right_Dir = FORWARD; } else if ((D5<MIN_US_DISTANCE)||(D6<MIN_US_DISTANCE)){ Left_Dir = FORWARD; Right_Dir = BACKWARD; } else if ((D1<MIN_US_DISTANCE)||(D2<MIN_US_DISTANCE)){ Left_Dir = BACKWARD; Right_Dir = FORWARD; }

if(((IR_Left>MIN_IR_DISTANCE) && (IR_Right>MIN_IR_DISTANCE)) || (DC<10)){ Left_Dir = BACKWARD; Right_Dir = BACKWARD; } else if(IR_Left>MIN_IR_DISTANCE){ Left_Dir = FORWARD; Right_Dir = BACKWARD; } else if(IR_Right>MIN_IR_DISTANCE){ Left_Dir = BACKWARD; Right_Dir = FORWARD; }

}else if (Last_Left_Dir != Last_Right_Dir){

if((IR_Left>MAX_IR_DISTANCE) && (IR_Right>MAX_IR_DISTANCE)){ Left_Dir = BACKWARD; // Go backward

Right_Dir = BACKWARD; if(D1<MIN_US_DISTANCE){

Left_Dir = BACKWARD; // Go backward Right_Dir = FORWARD;

}

if(D6<MIN_US_DISTANCE){

Left_Dir = BACKWARD; // Go backward Right_Dir = FORWARD;

} }

else if((IR_Left<MIN_IR_DISTANCE) && (IR_Right<MIN_IR_DISTANCE)){ Left_Dir = FORWARD;

Right_Dir = FORWARD; } }

else{//Vi åker bakåt kollar om vi behöver åka framåt. if (IR_Back > MAX_IR_DISTANCE){

Left_Dir = FORWARD; // Go Forward Right_Dir = FORWARD; } } last_left_dir=Left_dir,last_right_dir=Right_dir; // return (Left_Dir*16)+Right_Dir; }

(26)

21

void Set_Speed(){ //LeftSpeed,RightSpeed,Left_dir,Right_dir byte LeftSpeed,RightSpeed,L_Dir,R_Dir pwm(LeftSpeed,RightSpeed); if (Left_Dir==1){ output_High(PIN_C0); output_Low(PIN_C3); }else{ output_Low(PIN_C0); output_High(PIN_C3); } if (Right_Dir==1){ output_High(PIN_C4); output_low(PIN_C5); }else{ output_Low(PIN_C4); output_High(PIN_C5); } }

long Speed_Control(){ //IR_Left,IR_Right,IR_Back,IR_L_dir,IR_R_dir static long LeftSpeedArray[10];

static long RightSpeedArray[10]; static byte arraypointer = 0; int i;

if (arraypointer >= INT_LENGTH ){ arraypointer = 0;

}

if (IR_L_Dir){ // Go Forward

LeftSpeedArray[arraypointer] = Abs(MAX_SPEED - IR_Left); }

else { // Go Backward;

LeftSpeedArray[arraypointer] = Abs(MAX_SPEED - IR_Back); }

if (IR_R_Dir){ // Go Forward

RightSpeedArray[arraypointer] = Abs(MAX_SPEED - IR_Right); }

else { // Go Backward;

RightSpeedArray[arraypointer] = Abs(MAX_SPEED - IR_Back); }

for (i = 0; i < INT_LENGTH; i++){ LeftSpeed += LeftSpeedArray[i]; RightSpeed += RightSpeedArray[i]; } RightSpeed = RightSpeed/INT_LENGTH; LeftSpeed = LeftSpeed/INT_LENGTH; if (LeftSpeed > MAX_SPEED){ LeftSpeed = MAX_SPEED; } if (RightSpeed > MAX_SPEED){ RightSpeed = MAX_SPEED; } if (LeftSpeed < MIN_SPEED){ LeftSpeed = MIN_SPEED; } if (RightSpeed < MIN_SPEED){ RightSpeed = MIN_SPEED; } arraypointer++; }

(27)

22

Void UltraSonic_Read_Pos(void){

if (US_state==STATE0 && (US_state2 != STATE0)){ US_state2 = STATE0;

if(Servo_Direction == L_TO_R){ D1=UltraSonic();

}else D6=UltraSonic();

// sprintf(strang, "I: State:%d S-Dir:%c AC:%ld %c%c",US_state,servodir,ActionCounter,13,10); // print(strang);

}else if (US_state==STATE1 && (US_state2 != STATE1)){ US_state2 = STATE1;

if(Servo_Direction == L_TO_R){ DC=UltraSonic();

}else DC=UltraSonic();

sprintf(strang, "I: State:STATE%d S-Dir:%c AC:%ld %c%c",US_state,servodir,ActionCounter,13,10);

print(strang);

//Skriver ut värden från ultraljudet vänster och höger för sig.

sprintf(strang, "L: US1:%ld US2:%ld US3:%ld %c %c",D1,D2,D3,13,10); print(strang);

sprintf(strang, "R: US4:%ld US5:%ld US6:%ld %c %c",D4,D5,D6,13,10); print(strang);

sprintf(strang, "C: US_Center: %ld MAXSPEED %d MINSPEED %d %c %c",DC,MAX_SPEED,MIN_SPEED,13,10);

(28)

23

print(strang);

//Skriver ut riktning och hastighet som är beroende på värden från IR.

sprintf(strang, "L: IR_Left:%d Left_Dir:%d LSpeed:%d%c%c",IR_Left,Left_Dir,LeftSpeed,13,10); print(strang);

sprintf(strang, "R: IR_Right:%d Right_Dir:%d RSpeed:%d%c%c",IR_Right,Right_Dir,RightSpeed,13,10); print(strang);

//Skriver ut värden som behövs för att backa.

sprintf(strang, "B: IR_Back:%d S-Dir:%c %c%c",IR_Back,servodir,13,10); print(strang);// Skriver sträng till RS232.

if (Servo_Direction == true){ servodir='L';} else {servodir='R';}

} }

void Read_IR(void){ IR_Left=Read_Analog('0');

IR_Right=Read_Analog('1')+4; //4 justerar skillnaden mellan IR-Sensorena i fronten. IR_Back=Read_Analog('2'); if (IR_Left>100){ IR_Left = 100; } if (IR_Right>100){ IR_Right = 100; } if (IR_Back>100){ IR_Back = 100; } }

(29)

24 8.1.4 LCD4.C

/*DISPLAY GDM1602A with chip from Samsung (KS0066U)*/ void LCD_enable(void);

void lcd_test();

void lcd_write_string(char *str); void lcd_line_pos(int line, int pos); struct lcd_map{

int Data : 4; //PIN B0-B3 boolean Enable; //PIN B4 boolean RS; //PIN B5 int unused : 2; }lcd; #byte LCD = 6 void lcd_init(void){ //LCD Power ON set_tris_b(0x00); LCD.Data=0; LCD.Enable=0; LCD.RS=0; delay_ms(39); //Function Set LCD.Data=0b0010;//MSB LCD_enable(); LCD.Data=0b1000;//LSB LCD_enable(); //Display Control delay_us(39); LCD.Data=0b0000;//MSB LCD_enable(); LCD.Data=0b1111;//LSB LCD_enable(); //Display Clear delay_us(39); LCD.Data=0b0000;//MSB LCD_enable(); LCD.Data=0b0001;//LSB LCD_enable();

//Entry Mode Set delay_us(39); LCD.Data=0b0000;//MSB LCD_enable(); LCD.Data=0b0110;//LSB LCD_enable(); }

void LCD_Enable(void){ // Toggle Enable LCD.Enable=1;

delay_cycles(1); LCD.Enable=0; }

(30)

25

void lcd_clear(){

//Tömmer LCD och sätter Cursoren på position 1 på första raden. LCD.Data = 0b0000; LCD_Enable(); LCD.Data = 0b0001; LCD_Enable(); delay_us(1530); } //Test AV LCD utskrivt. void lcd_write_char(char c){ delay_us(43); LCD.RS=1; LCD.Data=(c&0xf0)>>4;//MSB LCD_enable(); LCD.Data=c&0x0f;//LSB LCD_enable(); LCD.RS=0; } /*Skriver ut en sträng på displayen*/ void lcd_write_string(char *vector){ int i = 0;

char vcpy[16];

strcpy(vcpy, vector); //Kopierar vår pekare till en variabel. while(vcpy[i]!= '\0' ){

lcd_write_char(vector[i]); i ++;

if (i==16){ int j;

for (j=0;j<24;j++){ // Write 24 blanks to get to beginning of next line lcd_write_char('x');

} } } }

void lcd_line_pos(int line, int pos){ //Rad2 på Display delay_us(39); if (line == 2){ lcd.data = 0b1100; LCD_enable(); lcd.data = 0b0000+pos; LCD_enable(); } //Rad1 på Display else{ lcd.data = 0b0100; LCD_enable(); lcd.data = 0b0000+pos; LCD_enable(); } }

(31)

26 8.2 Avlusningsprogram

#! perl my $version = "1.0"; #use strict; use Win32::SerialPort; use Tk; use Tk::Dialog; use Tk::DialogBox; #use Tk::FileDialog; use Tk::Toolbar; use Tk::JComboBox; use Tk::Frame; use Tk::TextUndo; use Tk::Text; use Tk::TextUndo; use Tk::Scrollbar; use Tk::Menu; use Tk::Menubutton; use Tk::Adjuster; use Tk::Canvas; use Tk::NoteBook; use Tk::ProgressBar; my $PortName = "COM1"; my $quiet; my $Configuration_File_Name = 'testconf.cfg'; print "Trying to connect to $PortName....\n";

my $PortObj = new Win32::SerialPort ($PortName, $quiet) || die "Can't open $PortName: $^E\n"; # $quiet is optional

print "\n\nConnected to $PortName\n\n"; $PortObj->user_msg("ON"); $PortObj->databits(8); $PortObj->baudrate(9600); $PortObj->parity("none"); $PortObj->stopbits(1); $PortObj->handshake("none"); $PortObj->buffers(4096, 4096);

$PortObj->write_settings || undef $PortObj; $PortObj->save($Configuration_File_Name);

$PortObj->are_match('NL', 'OKW', 'OKH', 'START', 'NEWOK'); $mw_X_size = 800; $mw_Y_size = 600; my $MW = MainWindow->new; my ($window,$frame,$txt); my ($book,$raisetime,$tab1,$tab2,$tab8); my ($leftwindow_text,$leftwindow_graph,$rightwindow_text,$rightwindow_graph); my @color = ("blue","red","green","yellow"); my $rspeed,$lspeed; my $x1 = 1; $x2 = 2; my $r_y1=0;$r_y2=0;$l_y1=0;$l_y2=0; my $minspeed,$maxspeed;

my $left_dir=1,$right_dir=1; # 1 = FoRWARD, -1 = BACKWARD my $y_offset = 300;

my $do_once;

(32)

27

create_screen();

$PortObj->close || die "failed to close"; undef $PortObj; exit; sub Read_From_COM(){ my ($line,$oldline,$count,$result,$in); while (1){ $PortObj->read_interval(100); while (1){

($count, $result) = $PortObj->read(1); $line = $line . $result;

last if ($line =~ m/\n/); }

#$line = "R: IR_Left:16 IR_L_Dir1 RSpeed:64"; # if ($line !~ $oldline){

# if ($line =~ m/STATE:3|STATE:6/){

chomp $line;

$line =~ s/\x0a+//g; # Remove all CR in line 0x0A $line =~ s/\x0d+//g; # # # print "$line\n"; if ($line =~ m/^(L|R|C|B|I):/){ $side = $1; if ($side =~ m/L/){ $leftwindow_text->insert('end',"$line\n",'left');

$leftwindow_text->yview(moveto => 1.1); # Automatically scroll to bottom of text (1.1 = bottom, 0.0 = top)

if ($line =~ m/Speed:(\d+)/){ $speed = $1;

$x2 = $x1; $x1++;

$l_y2 = $l_y1;

$l_y1 = $y_offset - (($speed)*$scale*$left_dir);

$l_y2 = $l_y1 if (!$l_y2); #Make sure both y1 and y2 has a value $x2 = $x1 if (!$x2); #Make sure both x1 and x2 has a value

# print "y_offset=$y_offset l_y1=$l_y1, l_y2=$l_y2 left_dir=$left_dir\n"; $speedwindow_graph->createLine($x1,$l_y1,$x2,$l_y2,-fill => 'red',-tags => 'left_speed');

}

if ($line =~ m/Left_Dir:(\d+)/){

$left_dir = 1 if ($1 == 1); #Direction is forward $left_dir = -1 if ($1 == 0); #Direction is backward # print "left_dir=$left_dir\n";

$directionwindow_text->insert('end',"($left_dir,",'left');

$directionwindow_text->yview(moveto => 1.1); # Automatically scroll to bottom of text (1.1 = bottom, 0.0 = top)

draw_left_direction(); }

}elsif (($side =~ m/R/)){

$rightwindow_text->insert('end',"$line\n",'right');

$rightwindow_text->yview(moveto => 1.1); # Automatically scroll to bottom of text (1.1 = bottom, 0.0 = top)

if ($line =~ m/Speed:(\d+)/){ $speed = $1;

$x2 = $x1; $x1++;

$r_y2 = $r_y1;

$r_y1 = $y_offset - (($speed)*$scale*$right_dir);

$r_y2 = $r_y1 if (!$r_y2); #Make sure both y1 and y2 has a value $x2 = $x1 if (!$x2); #Make sure both x1 and x2 has a value

(33)

28

# print "y_offset=$y_offset r_y1=$r_y1, r_y2=$r_y2 right_dir=$right_dir\n"; $speedwindow_graph->createLine($x1,$r_y1,$x2,$r_y2,-fill => 'black',-tags => 'right_speed');

}

if ($line =~ m/Right_Dir:(\d+)/){

$right_dir = 1 if ($1 == 1); #Direction is forward $right_dir = -1 if ($1 == 0); #Direction is backward # print "right_dir=$right_dir\n";

$directionwindow_text->insert('end',"$right_dir)",'right'); if (($left_dir == 1) and ($right_dir == 1)){

$directionwindow_text->insert('end',"FORWARD",'right'); }elsif(($left_dir == -1) and ($right_dir == -1)){

$directionwindow_text->insert('end',"REVERSE",'right'); }elsif(($left_dir == 1) and ($right_dir == -1)){

$directionwindow_text->insert('end',"TURN RIGHT",'right'); }elsif(($left_dir == -1) and ($right_dir == 1)){

$directionwindow_text->insert('end',"TURN LEFT",'right'); }

$directionwindow_text->insert('end',"\n",'right');

$directionwindow_text->yview(moveto => 1.1); # Automatically scroll to bottom of text (1.1 = bottom, 0.0 = top)

draw_right_direction(); } } if (($side =~ m/C/)){ if ($line =~ m/MAXSPEED\s+(\d+)\s+MINSPEED\s+(\d+)/){ $maxspeed = $1; $minspeed = $2; if (!$do_once){

$speedwindow_graph->createLine(0,$y_offset-$maxspeed*$scale,$mw_X_size,$y_offset-$maxspeed*$scale,-fill => 'blue',-tags => 'max_speed');

$speedwindow_graph->createLine(0,$y_offset-$minspeed*$scale,$mw_X_size,$y_offset-$minspeed*$scale,-fill => 'blue',-tags => 'min_speed');

$speedwindow_graph->createLine(0,$y_offset+$maxspeed*$scale,$mw_X_size,$y_offset+$maxspeed*$scale,-fill => 'blue',-tags => 'max_speed'); # When going backwards

$speedwindow_graph->createLine(0,$y_offset+$minspeed*$scale,$mw_X_size,$y_offset+$minspeed*$scale,-fill => 'blue',-tags => 'min_speed'); #When going backwards

$speedwindow_graph>createText(50,$y_offset+$maxspeed*$scale10, -text=>"MAX REVERSE",-fill=>'blue'); $speedwindow_graph>createText(50,$y_offset$maxspeed*$scale10, -text=>"MAX FORWARD",-fill=>'blue'); $speedwindow_graph>createText(50,$y_offset+$minspeed*$scale10, -text=>"MIN REVERSE",-fill=>'blue'); $speedwindow_graph>createText(50,$y_offset$minspeed*$scale10, -text=>"MIN FORWARD",-fill=>'blue'); $speedwindow_graph->createLine(0,$y_offset,$mw_X_size,$y_offset,-fill => 'green',-tags => 'y-offset'); $do_once = 1; } } $centrewindow_text->insert('end',"$line\n",'centre');

$centrewindow_text->yview(moveto => 1.1); # Automatically scroll to bottom of text (1.1 = bottom, 0.0 = top)

}elsif (($side =~ m/B/)){

$backwindow_text->insert('end',"$line\n",'back');

$backwindow_text->yview(moveto => 1.1); # Automatically scroll to bottom of text (1.1 = bottom, 0.0 = top)

}elsif (($side =~ m/I/)){

(34)

29

$infowindow_text->yview(moveto => 1.1); # Automatically scroll to bottom of text (1.1 = bottom, 0.0 = top)

} } if ($x > $mw_X_size){ On_Clear(); $x = 0; } &update_gui; # $box1->insert('end', $line);box1->pack; # $mw->update; # } # } $oldline = $line; $line = (); } } sub update_gui{ $MW->update; } sub create_screen() {

$MW->title("Robot Debug tool $version by Martin Aldrin "); $MW->iconname('frog');

my $menubar = build_menubar;

my $toolbar = $MW->ToolBar(qw/-movable 1 -side top/); $toolbar->ToolButton (-text => 'Clear',

-tip => 'Clearing windows', -command => sub {OnClear()} );

$toolbar->ToolButton (-text => 'Open', -tip => 'Opens a new log file', -command => sub {Open_File()} );

$toolbar->ToolButton (-text => 'Save',

-tip => 'Save the result to a file', -command => sub {Save_File()} );

$toolbar->ToolButton (-text => 'Quit', -tip => 'Quit',

-command => sub {OnExit()} ); $toolbar->ToolButton (-text => 'Pause', -tip => 'Pause',

-command => sub {OnPause()} ); # $toolbar->ToolLabel (-text => 'A Label'); # $toolbar->Label (-text => 'Another Label'); # $toolbar->ToolLabEntry(-label => 'A LabEntry', # -labelPack => [-side => "left", # -anchor => "w"]);

my $book = $MW->NoteBook()->pack( -fill=>'both', -expand=>1 ); $tab1 = $book->add( "Sheet 1", -label=>"LEFT & RIGHT"); # $tab2 = $book->add( "Sheet 2", -label=>"RIGHT" );

$tab3 = $book->add( "Sheet 3", -label=>"CENTRE & BACK" ); # $tab4 = $book->add( "Sheet 4", -label=>"BACK" );

$tab5 = $book->add( "Sheet 5", -label=>"SPEED" ); $tab6 = $book->add( "Sheet 6", -label=>"DIRECTION" ); $tab7 = $book->add( "Sheet 7", -label=>"INFO" );

(35)

30

$tab1->Label( -textvariable=>\'Left & Right Side Data')->pack(-expand=>1); # $tab2->Label( -textvariable=>\'Right Side Data')->pack( -expand=>1); $tab3->Label( -textvariable=>\'Centre & Back Data')->pack( -expand=>1); # $tab4->Label( -textvariable=>\'Back Data')->pack( -expand=>1);

$tab5->Label( -textvariable=>\'Speed')->pack( -expand=>1); $tab6->Label( -textvariable=>\'Direction')->pack( -expand=>1); $tab1->pack; # $tab2->pack; $tab3->pack; # $tab4->pack; $tab5->pack; $tab6->pack; $tab7->pack;

$leftwindow_text = $tab1->Scrolled('Text', -relief=>'sunken', -borderwidth=>'2', -setgrid=>'true', -height=>'15', -scrollbars=>'e');

$leftwindow_text->pack(-expand=>'yes', -fill=>'both');

$rightwindow_text = $tab1->Scrolled('Text', -relief=>'sunken', -borderwidth=>'2', -setgrid=>'true', -height=>'15', -scrollbars=>'e');

$rightwindow_text->pack(-expand=>'yes', -fill=>'both');

$centrewindow_text = $tab3->Scrolled('Text', -relief=>'sunken', -borderwidth=>'2', -setgrid=>'true', -height=>'15', -scrollbars=>'e');

$centrewindow_text->pack(-expand=>'yes', -fill=>'both');

$backwindow_text = $tab3->Scrolled('Text', -relief=>'sunken', -borderwidth=>'2', -setgrid=>'true', -height=>'15', -scrollbars=>'e');

$backwindow_text->pack(-expand=>'yes', -fill=>'both');

$speedwindow_graph = $tab5->Canvas(-width => $mw_X_size, -height => $mw_Y_size); $speedwindow_graph->createText(20,10, -text=>"Left",-fill=>'red');

$speedwindow_graph->createText(60,10, -text=>"Right",-fill=>'black'); $speedwindow_graph->pack;

$directionwindow_text = $tab6->Scrolled('Text', -relief=>'sunken', -borderwidth=>'2', -setgrid=>'true', -height=>'10', -scrollbars=>'e');

$directionwindow_text->pack(-expand=>'yes', -fill=>'both');

$directionwindow_graph = $tab6->Canvas(-width => $mw_X_size, -height => $mw_Y_size); $directionwindow_graph->pack;

$infowindow_text = $tab7->Scrolled('Text', -relief=>'sunken', -borderwidth=>'2', -setgrid=>'true', -height=>'15', -scrollbars=>'e');

$infowindow_text->pack(-expand=>'yes', -fill=>'both');

# $preferenceswindow_text = $tab8->Scrolled('Text', -relief=>'sunken', -borderwidth=>'2', # -setgrid=>'true', -height=>'20', -scrollbars=>'e');

# $preferenceswindow_text->pack(-expand=>'yes', -fill=>'both'); # $tab8->Entry(-textvariable => \$minimumscheds)

# ->pack(-expand => 1, # -fill => 'x');

# $tab8->LabEntry(-label => "Minimum number of schedulings per user for average calculation", # -labelPack => [-side => "left", -anchor => "w"],

# -width => 5,

# -textvariable => \$minimumscheds)->pack(-side => "top", # -anchor => 'nw');

# $tab8->LabEntry(-label => "Number of SCCH-codes per TTI", # -labelPack => [-side => "left", -anchor => "w"],

# -width => 5,

# -textvariable => \$numberofscchcodes)->pack(-side => "top", # -anchor => 'nw');

(36)

31

$MW->Entry(-textvariable => \$PortName) ->pack(-expand => 1, -fill => 'x'); $book->pack; $book->update; # Open_File(); Read_From_COM(); } sub build_menubar {

# Create the menubar and File and Quit menubuttons. Note # that the cascade's menu widget is automatically created.

my $menubar = $MW->Menu(-menuitems => &menubar_menuitems() ); $MW->configure(-menu => $menubar); } # end build_menubar sub menubar_menuitems { return [ map ['cascade', $_->[0], -tearoff=> 0, -menuitems => $_->[1]],

# make sure you put the parens here because we want to # evaluate and not just store a reference

['~File', &file_menuitems()], ['~Help', &help_menuitems()], ]; } sub file_menuitems { return [

[qw/command ~Open -accelerator Ctrl-o/,-command=>[\&Open_File]], [qw/command ~Save -accelerator Ctrl-s/,-command=>[\&Save_File]], [qw/command E~xit -accelerator Ctrl-q/,-command=>[\&OnExit]], ]

}

sub Open_File() {

my @types = (["Log files", '.log'],["Text files", '.txt'],["All Files", "*"] ); $logfile=$MW->getOpenFile(-filetypes => \@types, -initialfile=> 'Logfile', -defaultextension => '.log'); if ($logfile){ close_logfile(); OnClear();

open (INFILE, $logfile) or die "Can't open $logfile\n"; $execute = 1;

# $number_of_lines = analyse_file($logfile); }

(37)

32

sub Save_File() {

my $exclusive_lock = 2; my $unlock_lock = 8;

my @types = (["Result files", '.res'],["Text files", '.txt'],["All Files", "*"] ); $resultfile = $MW->getSaveFile(-filetypes => \@types,

-initialfile=> 'Result', -defaultextension => '.res'); if ($resultfile){

print "Saving File....\n";

open (OUTFILE, ">$resultfile") or die "Can't create $resultfile\n"; flock(OUTFILE,$exclusive_lock);

print (OUTFILE "===============\n");

my $selected_text = $leftwindow_text->get('0.0', 'end'); print (OUTFILE "$selected_text");

print (OUTFILE "===================\n"); my $selected_text = $rightwindow_text->get('0.0', 'end'); print (OUTFILE "$selected_text");

print (OUTFILE "===================\n");

my $selected_text = $centrewindow_text->get('0.0', 'end'); print (OUTFILE "$selected_text");

print (OUTFILE "===================\n"); my $selected_text = $backwindow_text->get('0.0', 'end'); print (OUTFILE "$selected_text");

print (OUTFILE "===================\n");

my $selected_text = $directionwindow_text->get('0.0', 'end'); print (OUTFILE "$selected_text");

print (OUTFILE "===================\n"); my $selected_text = $infowindow_text->get('0.0', 'end'); print (OUTFILE "$selected_text");

close (OUTFILE); flock(OUTFILE,$unlock_lock); print "Done!\n" } } sub help_menuitems { return [

[qw/command ~about -accelerator Ctrl-a/,-command=>[\&OnAbout]], ];

}

sub OnExit {

print STDOUT "Bye bye\n" if (!$noprint); exit;

(38)

33

sub OnAbout {

my $about = $MW->DialogBox(-title=>"Robot Debug tool v$version",-buttons=>["OK"] );

$about->add('Label',-anchor => 'w', -justify => 'left',-text => qq(Robot Debug tool v$version \n by \n Martin Aldrin\n\n Please enjoy!!!))->pack;

$about->Show; }

sub OnPause {

my $pause = $MW->DialogBox(-title=>"Pause updating",-buttons=>["CONTINUE"] ); $pause->add('Label',-anchor => 'w', -justify => 'left',-text => qq(Press CONTINUE to continue execution))->pack;

$pause->Show; }

sub OnClear {

$leftwindow_text->delete('0.0','end'); # Radera allt från början till slut i fönstret $rightwindow_text->delete('0.0','end'); $centrewindow_text->delete('0.0','end'); $backwindow_text->delete('0.0','end'); $rightwindow_text->delete('0.0','end'); $directionwindow_text->delete('0.0','end'); $directionwindow_graph->delete('L-dir'); $directionwindow_graph->delete('R-dir'); $infowindow_text->delete('0.0','end'); # $preferenceswindow_text->delete('0.0','end');

$speedwindow_graph->delete('speed'); #Radera den graf som heter speed } sub close_logfile() { close INFILE; } sub analyse_file() { # my $logfile = @_[0]; # print "Analysing file...\n";

# open (ANFILE, "<$logfile") or die "Can't open $logfile\n"; # my $lines = 0; # while (<ANFILE>) { # my $line = $_; # if ($line =~ m/^\[/){ # $lines++; # } # } # close (ANFILE); # print "Done!\n"; # return $lines; } sub draw_left_direction() { $directionwindow_graph->delete('L-dir');

$directionwindow_graph->createLine(100,100,100,200,-fill => 'blue',-tags => 'L-dir'); if ($left_dir == 1){

$directionwindow_graph->createLine(70,130,100,100,-fill => 'blue',-tags => 'L-dir'); $directionwindow_graph->createLine(100,100,130,130,-fill => 'blue',-tags => 'L-dir'); }else{

(39)

34

$directionwindow_graph->createLine(100,200,130,170,-fill => 'blue',-tags => 'L-dir'); }

}

sub draw_right_direction() {

$directionwindow_graph->delete('R-dir');

$directionwindow_graph->createLine(300,100,300,200,-fill => 'blue',-tags => 'R-dir'); if ($right_dir == 1){

$directionwindow_graph->createLine(270,130,300,100,-fill => 'blue',-tags => 'R-dir'); $directionwindow_graph->createLine(300,100,330,130,-fill => 'blue',-tags => 'R-dir'); }else{

$directionwindow_graph->createLine(270,170,300,200,-fill => 'blue',-tags => 'R-dir'); $directionwindow_graph->createLine(300,200,330,170,-fill => 'blue',-tags => 'R-dir'); }

(40)

35 8.3 Mjukvara Robotbil för styrning från Labview.

8.3.1 Uppgift3.h

#include <16F877A.h> #device adc=8

#use delay(clock=4000000)

#fuses NOWDT,XT, NOPUT, NOPROTECT, NODEBUG, BROWNOUT, NOLVP, NOCPD, NOWRT #use rs232(baud=19200,parity=N,xmit=PIN_C6,rcv=PIN_C7,bits=8)

8.3.2 Uppgift3.c

#include <Uppgift3.h> /*Adressering av portar*/ #byte port_d = 8 #byte port_c = 7 #byte port_e = 4 #define MAX_SPEED 50 #define MIN_SPEED 34

#define INTS_PER_SECOND 3840 // (20000000/(4*256*256)) 3840 = 1 sekund när man kör modulo 32 på interrupten #define INTS_PER_MILLISECOND 1 #define SERVO_CYCLE 40 #define NO_OF_ACTIONS 1200 #define STATE0 0 #define STATE1 1 #define STATE2 2 #define STATE3 3 #define STATE4 4 #define STATE5 5 #define STATE6 6 #define STATE7 7 #define L_TO_R true #define R_TO_L false #define GO true

/*Variabler för INTERRUPT*/ byte seconds=0,milli_seconds=0; long int int_count=INTS_PER_SECOND; byte int_millicount=INTS_PER_MILLISECOND;

(41)

36

/*Variabler som tillhör Ultrasonic och servofunktionen*/ byte US_State = STATE0,US_state2=STATE0;

byte action = 0; long actioncounter = 0; byte servo_array[] = {1,5}; byte servo_array_pointer = 0; byte Servo_High=0;

byte servo_state = GO;

long D0=0,D1=0,D2=0,D3=0,D4=0,D5=0,D6=0,DC=0; boolean Servo_Home = true;

boolean Servo_direction = L_TO_R; char servodir = 'A';

int i=0,start=1; char sign; char laban[50]; byte Left_Dir=1,Right_Dir=1; byte LeftSpeed=0,RightSpeed=0; char Labview[10]; /*Funktioner*/

void Print_RS232(char *vector); void Read_RS232();

void Call_Unit(void);

void Pwm(int Duty_Left, Duty_Right); void Set_Speed(byte Left_Dir,Right_Dir); byte Read_Analog(byte Channel); void UltraSonic_Read_Pos(); int UltraSonic(Void); void main() { setup_adc_ports(AN0_AN1_AN2_AN3_AN4); setup_adc(ADC_CLOCK_INTERNAL); setup_psp(PSP_DISABLED); setup_spi(FALSE); setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1); setup_timer_1(T1_INTERNAL); setup_timer_2(T2_DISABLED,0,1); setup_comparator(NC_NC_NC_NC); setup_vref(FALSE); setup_ccp1(CCP_PWM); setup_ccp2(CCP_PWM);

set_tris_d(0x02); //Sätter portD till utgång för syrning av UltraSonic (D0-D1). set_rtcc(0); //Startar realtidsklocka som används till utskrivt och Servot. enable_interrupts(INT_RTCC); enable_interrupts(GLOBAL); port_d=0x00; while(1){ Read_RS232(); } }

(42)

37

//Utför olika åtgärder beroende på kommando som inkommit. void Call_Unit(void){ byte IR_Left=0,IR_Right=0,IR_Back=0; if(i >= 4){ if((labview[0]=='M') || (labview[0]=='m')){ if((labview[1]=='L') || (labview[1]=='l')){ Left_Dir=labview[2]-'0'; LeftSpeed=(labview[3]-'0')*10; LeftSpeed+=(labview[4]-'0'); if(LeftSpeed>50){ LeftSpeed=MAX_SPEED; }

if((LeftSpeed < 34) && (LeftSpeed !=0)){ LeftSpeed=MIN_SPEED;

}

sprintf(laban, " Left_Dir %d Left_Speed:%d",Left_Dir,LeftSpeed); Print_RS232(laban);

} // end if L

else if((labview[1]=='R') || (labview[1]=='r')){ Right_Dir=labview[2]-'0'; RightSpeed=(labview[3]-'0')*10; RightSpeed+=(labview[4]-'0'); if(RightSpeed>50){ RightSpeed=MAX_SPEED; }

if((RightSpeed < 34) && (RightSpeed !=0)){ RightSpeed=MIN_SPEED;

}

sprintf(laban, " Right_Dir %d Right_Speed:%d",Right_Dir,RightSpeed); Print_RS232(laban);

} // end if else if R

Set_Speed(Left_Dir,Right_Dir); } // end if labview[0]='M'

(43)

38

if(i >= 2){

if(((labview[0]=='I') || (labview[0]=='i')) && ((labview[1]=='R') || labview[1]=='r')){ if((labview[2]=='L') || (labview[2]=='l')){

IR_Left=Read_Analog('0');

sprintf(laban, " IR_Left:%d ",IR_Left); Print_RS232(laban);

} // end if L

else if((labview[2]=='R') || (labview[2]=='r')){

IR_Right=Read_Analog('1')+4; //4 justerar skillnaden mellan IR-Sensorena i fronten. sprintf(laban, " IR_Right:%d ",IR_Right);

Print_RS232(laban); } // end else if R

else if((labview[2]=='B') || (labview[2]=='b')){ IR_Back=Read_Analog('2');

sprintf(laban, " IR_Back:%d ",IR_Back); Print_RS232(laban);

} // end else if B

} //end if labview[0] 'I' and labview[1] 'R'

if(((labview[0]=='U') || (labview[0]=='u')) && ((labview[1]=='S') || labview[1]=='s')){ if(labview[2]=='1'){ Servo_High=2; }else if(labview[2]=='2'){ Servo_High=3; }else if(labview[2]=='3'){ Servo_High=4; }else if(labview[2]=='4'){ Servo_High=5; }else if(labview[2]=='5'){ Servo_High=6; }else if(labview[2]=='6'){ Servo_High=7; }else if(labview[2]=='7'){ Servo_High=8; }else if(labview[2]=='8'){ Servo_High=9; }else if(labview[2]=='9'){ D0=UltraSonic(); sprintf(laban, " US0: %ld ",D0); Print_RS232(laban); }else if(labview[2]=='0'){ Servo_High=0; }

}//end if labview[U] 'I' and labview[1] 'S' and labview[1] 'D' } // end if i>=3

(44)

39

//Läser av RS232. void Read_RS232(void){ if(kbhit()){ sign = getchar(); } if(sign != 0){ putc(sign); labview[i]=sign;

if((i>=4) && (Labview[0]=='M' || Labview[0]=='m')){ Call_Unit();

PUTC('&'); i=0;

}else if((i>=2) && (Labview[0]=='I' || Labview[0]=='i')){ Call_Unit();

PUTC('&'); i=0;

}else if((i>=2) && (Labview[0]=='U' || Labview[0]=='u')){ Call_Unit(); PUTC('&'); i=0; }else if(i>=4){ PUTC('&'); i=0; }else{ i++; } } sign = '\0'; }

void Print_RS232(char *vector){ //Skriver strängar till RS232. int i = 0;

while(vector[i]!= '\0' ){ putc(vector[i++]); }

}

void pwm(int Duty_Left, Duty_Right){ Duty_Left=Duty_Left;

Duty_Right=Duty_Right;

setup_timer_2(T2_DIV_BY_1,51,2); //Sätter upp Timer 2 för få rätt PWM. set_pwm1_duty(Duty_Left); //PWM signal för Vänster motor

set_pwm2_duty(Duty_Right); //PWM signal för höger motor. }

(45)

40

void Set_Speed(byte Left_Dir,Right_Dir){ pwm(LeftSpeed,RightSpeed); if (Right_Dir==1){ output_High(PIN_C0); output_Low(PIN_C3); }else{ output_Low(PIN_C0); output_High(PIN_C3); } if (Left_Dir==1){ output_High(PIN_C4); output_low(PIN_C5); }else{ output_Low(PIN_C4); output_High(PIN_C5); } }

//Read AD-Channels and print it on LCD. byte read_analog(byte Channel){

int Analog_Value;

SET_ADC_CHANNEL(Channel); //Tar emot värde för vilken ingång som ska läsas av. analog_value = read_adc(ADC_START_AND_READ); //Läser av ingången.

return Analog_Value; }

//UltraSoic, Triggar på D0 och läser på D1 int UltraSonic(Void){

long temp=0,temp2=1; long Distance=0; output_high(PIN_D0);

delay_us(10); //Måste vara minst 10us. output_low(PIN_D0);

start =0; temp2=1; temp=0; start=0;

while(!input(PIN_D1)); //Stannar här tills det inte finns någon signal på US. set_timer1(0); //Nollställer timer1

while(input(PIN_D1)); start=1;

Distance = get_timer1(); //läser av Timer värdet.

Distance=Distance/58; //Räknar om tiden till cm skalfator 58 vid 4MHz 300 vid 20MHz. Distance=(int)Distance; //Omvandlar long till Integer.