Teknisk dokumentation

Linköpings tekniska högskola 2003-12-09 

Institutionen för systemteknik (ISY) 

Teknisk dokumentation 

Platon – en härligt gul sexbent robot 

Projektgrupp 15 – Jessica Heyman 

Version 1.1 

Granskad 

Godkänd 

Status 

Konstruktion med mikrodatorer Projektgrupp 15 - Platon LIPs 

Dokumentansvarig: David Landén martenson+stalin@lysator.liu.se Sida 1

Platon 

2003-12-09 

PROJEKTIDENTITET 

Projektgrupp 15 – Platon 

Linköpings tekniska högskola, ISY 

Gruppdeltagare 

Namn Ansvar Telefon E-post 

Johan Rees Projektledare 070 – 249 91 52 johre469@student.liu.se 

David Landén Dokumentansvarig 013 – 473 18 18 davla185@student.liu.se 

Pia Fåk Designansvarig, hårdvara 073 – 698 68 78 piafa456@student.liu.se 

Jessica Heyman Leveransansvarig 070 – 391 83 14 jeshe852@student.liu.se 

Mattias Lennartsson Testansvarig 073 – 767 12 15 matle017@student.liu.se 

Johan Mårtenson Designansvarig, mjukvara 073 – 931 98 96 johma196@student.liu.se 

E-postlista för hela gruppen martenson+stalin@lysator.liu.se 

Kund Tomas Svensson 

Kursansvarig Tomas Svensson, Olle Seger 

Handledare Anders Nilsson 



Platon 

2003-12-09 

Innehåll 

1 INLEDNING .............................................................................................................................................. 5 

1.1 Tidigare dokument............................................................................................................................ 5 

1.2 Definitioner ...................................................................................................................................... 5 

2 ÖVERSIKT AV SYSTEMET................................................................................................................... 6 

2.1 Fysisk uppbyggnad........................................................................................................................... 6 

2.2 Ingående moduler ............................................................................................................................. 6 

2.2.1 Kommunikation mellan moduler ........................................................................................... 7 

3 FJÄRRSTYRNINGSENHET ................................................................................................................... 7 

3.1 Inledande beskrivning av fjärrstyrningsenheten ............................................................................... 7 

3.2 Mjukvara .......................................................................................................................................... 8 

3.2.1 Signal från IR-detektor........................................................................................................ 10 

3.2.2 Signal från tryckknapp ........................................................................................................ 11 

3.3 Kommunikation.............................................................................................................................. 11 

3.3.1 Informationsöverföring inom fjärrstyrningsenheten ........................................................... 11 

3.3.2 Gränssnitt mot andra enheter.............................................................................................. 12 

3.4 Störningar ....................................................................................................................................... 12 

4 STYRENHET........................................................................................................................................... 12 

4.1 Inledande beskrivning av styrenheten............................................................................................. 12 

4.2 Mjukvara ........................................................................................................................................ 13 

4.2.1 Styrlogik .............................................................................................................................. 13 

4.2.2 Steglogik.............................................................................................................................. 16 

4.3 Kommunikation.............................................................................................................................. 17 

4.3.1 Informationsöverföring inom styrenheten ........................................................................... 17 

4.3.2 Gränssnitt mot andra enheter.............................................................................................. 18 

5 SENSORENHET ..................................................................................................................................... 19 

5.1 Inledande beskrivning av sensorenheten ........................................................................................ 19 

5.2 Mjukvara ........................................................................................................................................ 19 

5.2.1 Sensorer............................................................................................................................... 20 

5.3 Gränssnitt mot andra enheter.......................................................................................................... 20 

5.3.1 Insignaler ............................................................................................................................ 20 

5.3.2 Utsignaler............................................................................................................................ 21 

5.3.3 Signalprioritet ..................................................................................................................... 22 

5.3.4 Display ................................................................................................................................ 22 

5.4 Sensorer.......................................................................................................................................... 22 

5.4.1 Hårdvara............................................................................................................................. 22 

5.4.2 Namngivning ....................................................................................................................... 22 

6 ANVÄNDARHANDLEDNING.............................................................................................................. 23 

7 REFERENSER ........................................................................................................................................ 24 

Bilagor 

1 Hårdvarulista 

A1 Kretsschema för fjärrstyrningsenheten 

B1 Kretsschema för styrenheten 

C1 Kretsschema för sensorenheten 

A2 Assemblerkod för fjärrstyrningsenheten 

B2 Assemblerkod för styrenheten 

C2 Assemblerkod för sensorenheten 



Platon 

2003-12-09 

Dokumenthistorik 

Version Datum Förändringar från föregående version Utförda av Granskad 

0.1 2003-12-01 Första utkast Jessica Heyman Pia Fåk, 

Johan Rees, 

David Landén 

1.0 2003-12-05 

Ny programkod för styr- och sensorenhet 

Förtydliganden och layoutkorrigeringar 

Jessica Heyman 

Pia Fåk 

1.1 2003-12-09 

Tillägg av bild på framsida, i inledning och i 

avsnitt 2.2.1 

Jessica Heyman 

Rättning av småfel 



Platon 

2003-12-09 

1 Inledning 

På uppdrag av beställaren Tomas Svensson har vi i projektgrupp 15, Platon, konstruerat en 

liten sexbenig robot som återfinns i figur 1.1. Denna kan styras manuellt med fjärrkontroll, 

men kan också autonomt ta sig genom en labyrint hjälp av ultraljudssensorer. 

Detta dokument beskriver i detalj hur vår robot är konstruerad och hur den ska användas. I 

avsnitt två ges en översiktlig bild av roboten, som följs av detaljerna i de tre ingående 

modulerna fjärrstyrning, styrning och sensorer i avsnitt tre, fyra respektive fem. I anslutning 

till modulbeskrivningarna förklaras också hur de olika modulerna kommunicerar med 

varandra. Slutligen finns en användarmanual till konstruktionen i avsnitt sex. 

Figur 1.1 Fotografi på Platon – en härligt gul sexbent robot. 

1.1 Tidigare dokument 

Inom ramen för projekt Platon har vi tidigare tagit fram en kravspecifikation, en 

systemskiss och en designspecifikation. Detaljer om samtliga återfinns i referenslistan 

i slutet av denna dokumentation. 

1.2 Definitioner 

Följande begrepp används i detta dokument: 

Platon, roboten 

autonomt läge 

den sexbeniga robot vi har konstruerat 

det läge då Platon utan hjälp från användaren hittar rätt 

väg genom en labyrint. 



Platon 

2003-12-09 

2 Översikt av systemet 

Detta avsnitt behandlar roboten som helhet: dess huvudsakliga delar och grovt deras 

gränssnitt mot varandra. 

2.1 Fysisk uppbyggnad 

Robotens grundbyggsten är ett robotskelett som har sex styrbara ben och rörligt 

huvud. På robotens kropp sitter ett virkort, där alla komponenter som behövs för 

manövrering och navigation har fästs. Virkortet matas med 7.5 V spänning från sex 

uppladdningsbara batterier som ligger mellan robotkroppen och virkortet. 

Figur 2.1 Blockschema: översiktlig vy av systemet. 

2.2 Ingående moduler 

Roboten Platon är uppbyggd av tre moduler: 

• Fjärrstyrningsenhet som gör det möjligt för användaren att ge styrkommandon 

till roboten med hjälp av en fjärrkontroll. 

• Styrenhet som med hjälp av kommandon från de övriga enheterna styr benen så 

att roboten rör sig åt rätt håll. 



Platon 

2003-12-09 

• Sensorenhet som känner av robotens omgivning och skickar information om 

denna till styrenheten. 

Modulerna beskrivs mer ingående i avsnitt tre, fyra respektive fem. 

2.2.1 Kommunikation mellan moduler 

Informationsöverföring sker enbart mellan sensor- och styrenhet samt mellan 

fjärrstyrnings- och styrenhet. 

Mellan sensor- och styrenhet sker dubbelriktad kommunikation med fyra bitar 

till styrenheten från sensorenheten och två bitar från styr- till sensorenhet enligt 

figur 2.2. Med hjälp av dessa bitar skickar sensorenheten information till 

styrenheten om nödvändiga riktningskorrigeringar eller om en korsning hittats, 

och i så fall även vilken typ av korsning det rör sig om. Utförligare beskrivning 

finns i avsnitt 4.3. 

styrenhet 

sensorenhet 

Figur 2.2 Mellan styr- och sensorenhet skickas två bitar från styrenheten till 

sensorenheten och fyra bitar åt andra hållet. 

Mellan fjärrstyrningsenheten och styrenheten sker enbart enkelriktad 

informationsöverföring. Med virade kopplingstrådar skickar 

fjärrstyrningsenheten fyra bitar till styrenheten, där tre bitar kodar för ett 

kommando och den fjärde är en strobe som anger att fjärrstyrningsenheten 

skickar en ny signal. Vidare beskrivning av de tre informationsbitarna och när 

stroben läggs på finns i avsnitt 3.4.2. 

Sammanfattningsvis har alla tre moduler mycket enkla gränssnitt, vilket gör att 

de enkelt kan bytas ut. 

3 Fjärrstyrningsenhet 

3.1 Inledande beskrivning av fjärrstyrningsenheten 

Då roboten inte är i autonomt läge, kan den styras manuellt med en enkel fjärrkontroll. 

En användare kan då ge följande kommandon: 

• gå framåt 

• gå bakåt 



Platon 

2003-12-09 

• sväng höger 

• sväng vänster 

• stanna 

• autonomt läge på/av 

• ljudsignal 

Fjärrkontrollen sänder ut IR-signaler till roboten enligt fjärrkontrollens i förhand 

inställda protokoll. IR-detektorn tar emot dessa signaler och omvandlar dem till 

elektriska signaler, som skickas till signalbehandlingsenheten. Till denna enhet 

kommer också en signal från en tryckknapp som används för i- och urkoppling av 

autonomt läge. De inkomna signalerna tolkas i signalbehandlingsenheten och då 

roboten inte är i autonomt läge skickas de vidare till styrenheten i form av 

styrkommandon i samtliga fall utom då den mottagna signalen beskriver 

användarkommandot ”ljudsignal”. Då användaren ger kommandot ”ljudsignal” tutar 

signalbehandlingsenheten nämligen själv med summern, utan att styrenheten är 

inblandad. 

När signalbehandlingsenheten tar emot kommandot ”autonomt läge på/av”, antingen 

via tryckknapp eller fjärrkontroll, kommer även en lysdiod att tändas eller släckas 

(beroende på om läget slås av eller på) samtidigt som ett kommando skickas till 

styrenheten. 

Alla komponenter finns närmare beskrivna i bilaga 1 och hur de är anslutna till 

varandra illustreras i bilaga A1. 

Figur 3.1 Översiktlig vy över fjärrstyrningsenheten. 

3.2 Mjukvara 

För att kunna ta emot signaler, tolka dessa, sända ut styrkommandon samt styra 

lysdiod och summer, krävs förstås mjukvara i mikrokontrollern. Dess funktion är 

schematiskt illustrerad i figur 3.1 och beskriven nedan, medan den fullständiga 

assemblerkoden återfinns i bilaga A2. 



Platon 

2003-12-09 

Figur 3.2 Flödesschema för fjärrstyrningsenheten. 

Efter en kort initieringssekvens som konfigurerar portar och olika avbrott, väntar 

mikrokontrollern i en loop. Denna loop kan avbrytas av två saker: en fallande flank på 

insignalen från IR-detektorn eller en ändring av insignalen från tryckknappen. Nästan 

all signalbehandling sker således i avbrottsrutiner, men då mikrokontrollern är så pass 

snabb i förhållande till hur mycket data den ska behandla är detta inget problem. 



Platon 

2003-12-09 

3.2.1 Signal från IR-detektor 

Då användaren trycker på en knapp på fjärrkontrollen, kommer ett pulståg från 

IR-detektorn in till signalbehandlingsenheten. Beroende på vilken knapp 

användaren har tryckt på har detta pulståg olika utseende enligt tabell 3.1. 

Knapp 

Bitmönster 

gå framåt 00T11111 001010 

gå bakåt 00T11111 001111 

sväng höger 00T11111 001011 

sväng vänster 00T11111 001101 

stanna/filosofera 00T11111 001001 

autonomt läge på/av 00T11111 110011 

ljudsignal 00T11111 001000 

T står för toggle-bit och ett mellanrum mellan meta- och signaldata har lagts till 

för tydlighet 

Tabell 3.1 Bitmönster för IR-signaler sedan de konverterats till digitala signaler. 

De elektriska signalerna genererar ett avbrott, som i sin tur startar en timer (i 

subrutinen IRKOM) och konfigurerar mikrokontrollern att lyssna efter denna 

timers avbrott. Ungefär en åttondel in i perioden för nästkommande bit hos den 

inkommande signalen ger den satta timern ett avbrott, och dess avbrottsrutin 

(TMRAVB) börjar exekvera. Denna bestämmer först med hjälp av två inre 

räknare, en för metadata och en för signaldata, vilket av dessa den inkommande 

biten hör till och lagrar signalens värde i korresponderande register. Därefter 

kollar den om det var den sista biten i signalen som just mottogs, i vilket fall 

den anropar en subrutin som kollar om signalen är giltig (SIGKOLL) och i så 

fall skickar den om roboten inte är i autonomt läge med hjälp av en annan 

subrutin (SKICKA). I annat fall sätter den om timern för att generera ett nytt 

avbrott då nästa bit ligger på ingången. 

En signal bedöms som giltig om dess metadata är korrekt och dess signaldata 

motsvarar koden för en knapp som mikrokontrollern lyssnar efter. Metadata är 

korrekt om det består av – i tur och ordning – två nollor som är synkbitar, en 

togglebit med godtyckligt värde och till sist fem ettor, allt enligt 

fjärrkontrollens inställda protokoll (se bilaga 1). De signaldata som 

mikrokontrollern anser giltiga utgörs av de sex sista bitarna i tabell 3.1, det vill 

säga de knappspecifika koderna för de knappar som används för att ange ”gå 

framåt”, ”gå bakåt”, ”sväng höger”, ”sväng vänster”, ”stanna”, ”autonomt läge 

på/av” och ”ljudsignal”. 

Då en signal som mottagits från IR-detektorn bedömts som giltig, skickar 

fjärrstyrningenheten signaler till styrenheten, dioden och summern enligt 

tabell 3.2. 



Platon 

2003-12-09 

Användarkommando 

Autonomt läge på 

Åtgärd 

Autonomt läge av 

gå framåt ingen skicka 001 till styrenheten 

gå bakåt ingen skicka 010 till styrenheten 

sväng höger ingen skicka 011 till styrenheten 

sväng vänster ingen skicka 100 till styrenheten 

stanna/filosofera ingen skicka 101 till styrenheten 

autonomt läge på/av 

byt till tillstånd ”autonomt läge av” 

släck lysdiod 

skicka 000 till styrenheten 

byt till tillstånd ”autonomt läge på” 

tänd lysdiod 

skicka 000 till styrenheten 

ljudsignal tuta! (signal till summern) tuta! (signal till summern) 

Tabell 3.2 Kommandon med bitmönster från fjärrstyrningsenheten. 

De åtgärder som mikrokontrollern utför kommer enligt ovanstående tabell att 

vara olika beroende på vilket tillstånd roboten befinner sig i (autonomt läge på 

eller av) och vilket kommando det är frågan om. Eftersom autonomt läge 

enbart kan slås på och av via fjärrstyrningsenheten har denna en inre 

tillståndsvariabel, onoff i registret statreg, som alltid berättar om roboten 

befinner sig i autonomt läge eller inte. Denna variabel används då 

signalbehandlingsenheten ska bestämma vad som ska skickas: om roboten är 

autonom ska enbart kommandona autonomt läge på/av samt tuta göra att ett 

kommando skickas, medan alla giltiga signaler gör att ett kommando skickas 

till antingen styrenheten eller tutan då autonomt läge är avslaget. 

3.2.2 Signal från tryckknapp 

Den tryckknapp som finns monterad på virkortet har samma funktion som 

knappen ”autonomt läge på/av” på fjärrkontrollen. Robotens autonoma läge 

kan således stängas av via fjärrkontrollen för att sedan sättas på med 

tryckknappen, och vice versa. 

Då tryckknappen trycks ner genereras ett avbrott, som tas om hand i en 

avbrottsrutin (TRYCKAVB). Denna gör precis samma sak som rutinen SIGKOLL 

då insignalen från IR-detektorn kodat för ”autonomt läge på/av”, det vill säga 

släcker eller tänder lysdioden, togglar det inre tillståndet och skickar 

kommandot ”autonomt läge på/av” till styrenheten. 

3.3 Kommunikation 

3.3.1 Informationsöverföring inom fjärrstyrningsenheten 

Mellan fjärrkontroll och IR-mottagare sker överföring av data med hjälp av IRsignaler 

och mellan IR-mottagare och signalbehandlingsenhet finns enkla 

kopplingstrådar. De IR-signaler som, sedan de konverterats till elektriska 



Platon 

2003-12-09 

signaler av IR-detektorn, bedöms som giltiga av signalbehandlingsenheten 

finns i tabell 3.1. 

Också tryckknapp, diod, samt summer är anslutna till signalbehandlingsenheten 

på enklast möjliga sätt; direkt till ett ben på signalbehandlingsenheten. 

3.3.2 Gränssnitt mot andra enheter 

Fjärrstyrningsenheten kommunicerar enkelriktat med styrenheten genom virade 

kopplingstrådar. Informationsöverföringen består av fyra bitar, där tre av dessa 

utgör en binär kodning för sex olika kommandon som användaren kan ge 

roboten via fjärrkontrollen enligt tabell 3.2. 

Då fjärrenheten mottagit en signal från användaren, läggs aktuellt kommando 

på tre trådar till styrenheten, och precis därefter ges en strobe på den fjärde 

tråden. Stroben genererar ett avbrott i styrenheten, som då läser av de tre andra 

trådarna. Dessa tre trådar behåller sitt värde mellan det att två kommandon 

skickas, men då ingen ny strobe ges kommer inte styrenheten att läsa av dem 

och deras värde är således ointressant. Pinnummer för de tre 

informationsbitarna och stroben återfinns i bilaga A1. 

3.4 Störningar 

I vanliga miljöer finns IR-ljus som ibland genererar en puls från IR-detektorn. Denna 

puls sätter igång signalavkodningen i fjärrstyrningsenheten precis som en giltig signal 

från fjärrkontrollen, men sannolikheten är mycket låg att en störning ska ha samma 

värden som en giltig signal vid samtliga tider då signalen avläses. Sannolikheten finns 

dock, och därför skickar aldrig fjärrstyrningsenheten iväg något kommando till 

styrenheten såvida den inte tagit emot samma signal från IR-detektorn två gånger i rad. 

Då varje knapptryckning på fjärrkontrollen oftast genererar fler än tre upprepningar av 

knappens IR-signal, påverkar inte detta handhavandet av fjärrkontrollen utan är bara 

en extra felkontroll. 

Även om mikrokontrollerna inte kommer att skicka några signaler till styrenheten, sin 

diod eller sin tuta på grund av störningar, leder dessa dock till att mikrokontrollern 

sätter igång inläsningen av en signal och läser in tretton bitar. Detta gör att användaren 

ibland måste upprepa sin knapptryckning för att roboten ska reagera i en miljö med 

mycket störningar. 

4 Styrenhet 

4.1 Inledande beskrivning av styrenheten 

Styrenhetens uppgift är att ta emot signaler från fjärrstyrningsenheten samt 

sensorenheten och därefter bestämma hur roboten ska gå och röra benen på ett sådant 

sätt att roboten förflyttar sig på önskat sätt. Enheten kan befinna sig i fjärrstyrt eller 



Platon 

2003-12-09 

autonomt läge och läser då information från fjärrstyrningsenheten respektive 

sensorenheten. 

För att få en enkel och tydlig uppdelning inom modulen används två kretsar. En, 

kallad steglogik, som genom tre servon kontrollerar benens rörelser och en, kallad 

styrlogik, som hanterar vägval samt kommunikation med de andra modulerna. 

Uppdelningen bör framgå tydligt ur figur 4.1. 

Fjärrstyr Styrlogik Sensor 

Steglogik 

Servo 1 Servo 2 Servo 3 

Ben 

VF, VB 

Ben 

VM, HM 

Ben 

HF, HB 

Figur 4.1 Interface mellan de olika delarna av styrenheten. VF, HM… står för Vänster Fram, 

Höger Mitten och så vidare. 

4.2 Mjukvara 

4.2.1 Styrlogik 

Styrenheten kan befinna sig i två olika lägen; autonomt eller fjärrstyrt. 

Mjukvaran är uppbyggd av en loop enligt figur 4.2. En gång i varje varv i 

denna loop kontrolleras vilket av lägena enheten befinner sig i. 



Platon 

2003-12-09 

Figur 4.2 Flödesschema över styrenheten (se tabell 4.4 för förklaring av signalerna A1 

och A2). 

Då styrenheten befinner sig i fjärrstyrt läge skickas det kommando som läses in 

från fjärrstyrningsenheten vidare till steglogiken som sedan gör att roboten går 

på motsvarande sätt. 

Då styrenheten befinner sig i autonomt läge läses signaler från sensorenheten. 

Dessa signaler kan vara av tre typer: 

• ingen korrigering behövs 

• korrigering behövs 

• korsning hittad 

Då ingen korrigering behövs fortsätter roboten rakt fram. Om korrigering 

behövs utförs denna. Korrigeringen kan vara av två olika typer enligt figur 4.3 

och enligt 

• sväng höger 

• sväng vänster 



Platon 

2003-12-09 

Figur 4.3 Förklaring av riktningskorrigeringarna: höger/vänster. 

Då korsning hittats kommer sensorenheten senare även skicka signaler om 

vilken typ av korsning det är. Mer om detta i avsnitt 4.3.2.2. De olika 

korsningstyperna återfinns i figur 4.4. 

Figur 4.4 Sju korsningstyper. 

Den algoritm med vilken roboten autonomt ska ta sig genom labyrinten är 

mycket enkel. Roboten följer alltid labyrintens högerkant, vilket betyder att 

den förr eller senare alltid kommer till målet. Tabellen nedan visar vilken 

åtgärd som vidtas vid de olika typerna av korsningar för att åstadkomma denna 

algoritm. 

Korsningstyp 

vänstersväng 

högersväng 

T-korsning 

framåt-höger korsning 

framåt-vänster korsning 

återvändsgränd 

fyrvägskorsning 

mål 

Åtgärd 

sväng vänster 

sväng höger 

sväng höger 

sväng höger 

gå rakt fram 

vänd 180 o 

sväng höger 

stanna 

Tabell 4.1 Åtgärder vid de åtta olika korsningstyperna. 



Platon 

2003-12-09 

4.2.2 Steglogik 

Steglogiken tar emot signaler från styrlogiken och skickar sedan ut 

pulsbreddsmodulerande signaler, PWM-signaler, till de tre servona som i sin 

tur får benen att röra på sig på motsvarande sätt. Steglogiken är programmerad 

att utföra 11 olika rörelser enligt tabell 4.2. 

Rörelse Används i läge Kodning 

stanna fjärr/auto 0000 

gå framåt fjärr/auto 0001 

gå bakåt fjärr 0010 

sväng vänster fjärr 0011 

sväng höger fjärr 0100 

snurra vänster 0101 

parallellförflyttning höger auto 0110 

parallellförflyttning vänster auto 0111 

snurra höger 1000 

snurra 90 o höger auto 1001 

snurra 90 o vänster auto 1010 

Tabell 4.2 Steglogikens olika rörelser. De rörelser som inte används av någon modul 

är endera till för testsyfte eller används för att utföra de andra rörelserna. 

Dessa förflyttningar använder sig i sin tur av följande sex subrutiner: 

Subrutin 

höger upp 

vänster upp 

höger fram 

vänster fram 

höger bak 

vänster bak 

Beskrivning 

höger sida av roboten pressas upp från marken 

vänster sida av roboten pressas upp från marken 

både det främre och det bakre benet på höger sida flyttas till sitt 

främre läge. 

både det främre och det bakre benet på vänster sida flyttas till sitt 

främre läge 

båda benen på höger sida flyttas bak 

båda benen på vänster sida flyttas bak 

Tabell 4.3 Steglogikens sex subrutiner. 

Tack vare denna abstraktion fås lätt en bra överblick över koden och det är lätt 

att lägga till nya rörelsemönster. 

Servona fungerar på så sätt att en pulsbreddsmodulerad signal med perioden 

20 ms och pulslängd mellan 0,5 och 2,5 ms anger servots läge. Förflyttning 

mellan ändlägena motsvarar en vridning av servot på ungefär 180 o . För att 



Platon 

2003-12-09 

ställa servot i ena ändläget måste alltså en 0,5 ms lång puls skickas upprepade 

gånger med periodtiden 20 ms, enligt figur 4.5. 

Figur 4.5 Servots läge beroende på den pulsbreddsmodulerade signalen. 

Då det räcker att roboten kan flytta benen mellan de båda ändlägena kodar vi 

PWM-signalen så att den endast kan ha två diskreta pulslängder, motsvarande 

de båda ändlägena. I kretsen använder vi två räknare som räknar upp från 0 till 

255. De båda räknarna genererar avbrott vid något värde X, samt vid 

maxvärdet 255. För räknare nummer 1 är X lika med 243 och för räknare 2 är 

det 253. På så sätt skapar vi 2 PWM-signaler, som figur 4.6 visar. Dessa 

signaler har de båda önskade pulslängderna och kan därmed ställa servona i de 

båda ändlägena. 

Figur 4.6 Den pulsbreddsmodulerade signalens utseende beroende på räknarens 

X-värde. 

Nu behöver vi alltså bara ange vilken av de båda PWM-signalerna som skall 

skickas till vilket servo. Detta gör vi genom att sätta eller nollställa tre register 

som svarar mot de tre servona. Dessa register ändras sedan av subrutinerna i 

tabell 4.3. På så sätt rör roboten på benen och kan gå. 

4.3 Kommunikation 

4.3.1 Informationsöverföring inom styrenheten 

Det inre gränssnittet i styrenheten är en enkelriktad trebitars kommunikation 

från styrlogiken till steglogiken. Styrlogiken skickar ständigt någon av de elva 

signalerna i tabell 4.2. För varje varv i steglogikens huvudloop läses signalerna 

in och motsvarande rörelse utförs. 



Platon 

2003-12-09 

4.3.2 Gränssnitt mot andra enheter 

4.3.2.1 Fjärrstyrningsenhet–styrenhet 

Fjärrstyrningsenheten skickar signaler till styrenheten enligt tabell 3.2. Dessa 

signaler läses sedan in av styrenheten på varje nedåtgående flank på den strobe 

som finns mellan fjärrstyrning- och styrenheten. När signalen från 

fjärrstyrningsenheten till styrenheten är 000 byter styrenheten mellan fjärrstyrt 

och autonomt läge, enligt avsnitt 4.2.1. I fjärrstyrt läge lyssnar styrenheten av 

alla fjärrstyrningsenhetens signaler och agerar utifrån dessa. I autonomt läge 

lyssnar enheten dock endast efter signalen 000 – autonomt läge på/av. 

4.3.2.2 Sensorenhet–styrenhet 

Kommunikationen mellan sensorenheten och styrenheten består av två bitar 

från styrenheten till sensorenheten samt fyra bitar åt det motsatta hållet. Tabell 

4.4 förklarar betydelsen av de olika bitarna. 

Bit Betydelse Riktning 

A1 leta efter korsning och behov av korrigering från styrenhet 

A2 roboten befinner sig i en korsning som ska undersökas från styrenhet 

B1 

typ av signal som skickas 

1 betyder korrigeringstyp 

0 betyder korsningstyp 

till styrenhet 

B2 – B5 kod för korrigeringstyp eller korsningstyp till styrenhet 

Tabell 4.4 Beskrivning av signalerna mellan sensorenhet och styrenhet. 

Kommunikationen går till så att då roboten befinner sig i en korridor sätts bit 

A1 hög och sensorenheten börjar leta efter korsningar eller 

riktningskorrigeringar. Allt enligt flödesschemat i figur 4.2. 

Behövs en korrigering sätts A1 låg så att sensorenheten slutar leta efter 

korrigeringar och korsningar. Därefter utför styrenheten den nödvändiga 

korrigeringen och A1 sätts åter hög. På detta sätt uppstår inte problem med att 

sensorenheten skickar korrigeringssignaler som beror på korrigeringen. 

Om sensorenheten hittar en korsning sätts även där A1 låg. Roboten går sedan 

ungefär 15 cm in i korsningen så att sensorenheten lättare kan avgöra vilken 

typ av korsning roboten står i. A2 sätts hög så att sensorenheten vet att den 

skall bestämma korsningstyp. När den gjort detta och styrenheten tagit emot 

signalen sätts A2 låg. Styrenheten förflyttar sedan roboten på det sätt som 

beskrivs av algoritmen i avsnitt 4.2.1. Styrenheten förflyttar även roboten 

ungefär 20 cm in i den korridor som ligger efter korsningen. Detta för att 

sensorenheten inte ska bli störd av den tidigare korsningen. Slutligen sätts A1 

hög och sensorenheten börjar återigen söka efter riktningskorrigeringar och 

korsningar. 



Platon 

2003-12-09 

5 Sensorenhet 

5.1 Inledande beskrivning av sensorenheten 

Sensorenhetens uppgifter är att meddela styrenheten om en ny korsning upptäckts, 

vilken typ av korsning det är samt om kurskorrigering behövs. Styrenheten meddelar 

sensorenheten vad den är intresserad av just nu, antingen korrigera och finna korsning, 

eller avgöra korsningstyp. Korrigeringssignaler för korrigering åt vänster och höger 

beräknas och skickas till styrenheten då roboten befinner sig alltför nära en vägg och 

signaler för korsningar beräknas då en korsning upptäckts. Utsignaler från 

sensorenheten är uppdelade i två grupper. Den första gruppen är typ av korsning, den 

andra är hitta korsning eller korrigera. Sensorenheten har tre sensorer till hjälp och en 

mikrodator för att samordna dem. 

5.2 Mjukvara 

Då signalen A1 eller A2 är hög läses sensorerna av en efter en. Detta görs i en 

avbrottsrutin till timer1 som räknar upp sexton bitar med åtta MHz. I detta läge är 

mätningen på vilket markeras med att variabeln MATNING_PA har värdet 256. När alla 

sensorer är avlästa körs en rutin UPPD_UTV (uppdatera utvärde); denna körs också 

kontinuerligt av timer1 då mätningen är avstängd. Varje gång UPPD_UTV körs 

undersöks invärden från styrenheten och det avgörs om mätningen ska fortsätta vara 

på, stängas av eller kanske sättas på. Exakt hur sensorerna läses av återfinns i 

avsnitt 5.2.1. 

Varje gång rutinen UPPD_UTV körs finns alltså tre korrekta mätvärden till hands, 

alternativt att de inte finns om mätningen är avstängd. I det sista fallet görs bara den 

tidigare nämnda undersökningen sedan avslutas rutinen. 

Dessa tre utvärden behandlas på följande sätt. 

Om A1 är hög: 

De tre sista bitarna i en variabel (MAT_BYTE) sätts till höga 

respektive låga beroende på om sensorn har en vägg eller inte 

framför sig. Framåt betraktas ett eko med avståndet < 30 cm 

som en vägg, åt sidorna mätvärde < 50 cm. 

Sedan mappas dessa bitmönster om till en signal (korsningstyp). 

Denna signal skrivs sedan till utporten. 

Om A2 är hög: 

Mätvärdena undersöks efter lämpliga utsignaler efter signalernas 

prioritet (se avsnitt 5.3.3). Signalerna bestäms av följande krav och 

gränser: 

Korsning höger/vänster: sidorna > 50 cm åt något håll 

Korrigering: 

sidorna

Platon 

2003-12-09 

5.2.1 Sensorer 

För att läsa av sensorerna används en timer (timer0) som räknar upp åtta bitar 

med klockans sexton MHz. När sensorn ska läsas av skickas först en triggersignal 

till den aktuella sensorn som ska mätas av. Medan signalen skickas 

loopar processorn i en loop som inte gör någonting en liten tid. Detta eftersom 

pulsen inte behöver vara så speciellt lång. Sedan startas timern och tidigare 

mätvärden nollställs. Även ett värde TID_START nollställs. 

När timerrutinen sedan körs kontrollerar den om signalen echo-out (se avsnitt 

5.4.1) är hög. Är den hög räknas mätvärdet upp med ett steg. Variabeln till 

TID_START sätts också till 255 om den inte redan är det. Är signalen däremot 

låg antas dessa två fall vara möjliga: 

1. Pulsen har inte kommit ännu. 

2. Pulsen har passerat och mätningen är klar. 

För att särskilja dessa värden undersöks värdet av TID_START. Om 

TID_START har värdet 255 antas mätningen vara klar och timern stängs samt 

om alla mätningar från sensorerna är klara uppdateras utvärdet till styrenheten. 

Har TID_START värdet 0 återgår datorn till att vänta på en puls från 

sensorerna. Kommer ingen puls återgår datorn till att vänta. 

Ett problem med våra ultraljudssensorer är att de inte ger något stabilt värde. 

Det verkar som att de har en tendens att ge ett felaktigt mätvärde lite då och då. 

Detta är inte bra i vårt fall eftersom om en korsning undersöks felaktigt 

kommer roboten kanske inte att kunna hitta ut ur labyrinten. För att undvika 

detta gör vi tre mätningar per sensor i rad och beräknar medelvärdet av de två 

mätningar som ger närmast mätvärde. Differensen mellan mätvärdena 

beräknas, det finns alltså tre möjliga differenser. De två mätvärden som har 

lägst differens väljs och medelvärdet av dem beräknas. Denna behandling ger 

ett mycket stabilt mätvärde. 

5.3 Gränssnitt mot andra enheter 

5.3.1 Insignaler 

Sensorenheten har två insignaler från styrenheterna, A1 och A2. När 

styrenheten vill ha information om vilken typ av korsning roboten befinner sig 

i så sätter den signalen A2 hög. Sensorenheten läser då av sensorerna och när 

den sedan har fått mätvärdet från alla sensorer beräknar den vilken korsning 

roboten befinner sig i och sätter det som utvärde. Utvärdet kommer sedan att 

uppdateras kontinuerligt ända tills att signalen A2 går låg igen. Den kommer då 

att sätta 0 0000 som utsignal efter en kortare tid. 

När roboten går i en av labyrintens korridorer och styrenheten vill veta om 

korrigering är nödvändig eller om roboten kommit till en korsning sätts 

signalen A1 till hög. Roboten kommer då kontinuerligt att uppdatera utvärdet 

och om den upptäcker att korrigering behövs, sätta signal för detta, samma sak 



Platon 

2003-12-09 

om roboten kommer till en korsning. Signalen för korrigering försvinner då 

roboten står rätt i korridoren igen. 

5.3.2 Utsignaler 

Styrenheten har en uppsättning signaler som kan skickas till styrenheten. Dessa 

representeras med ett bitmönster. Signalerna kan delas in i två grupper: 

• korsningstyp 

• korrigeringstyp eller hittad korsning. 

Den första biten beskriver vilken typ av signal som skickas, 0 betyder grupp 

ett, 1 betyder grupp två. Själva signalen representeras sedan med ett fyrabitars 

tal. Signalerna ur grupp ett är svar på signalen A2 från styrenheten och signaler 

ur grupp två svar på signalen A1. 

Grupp ett, som skickas då styrenheten vill undersöka vilken typ av korsning det 

är, finns i tabell 5.1, medan grupp två, som skickas då styrenheten vill leta efter 

korsning eller få signal för korrigering, åskådliggörs i tabell 5.2. 

Korsningstyp 

Bitmönster 

felmeddelande, okänd korsning. 0 0000 

återvändsgränd 0 0001 

vänsterkorsning 0 0010 

högerkorsning 0 0011 

T-korsning (höger-vänster korsning) 0 0100 

framåt-vänster korsning 0 0101 

framåt-höger korsning 0 0110 

fyrvägskorsning 0 0111 

Den första biten i bitmönstret betecknar grupp: 0 står för grupp ett 

Tabell 5.1 Korsningssignaler från sensorenheten till styrenheten. 

Korrigering/finn korsning 

Bitmönster 

ingen korrigering 1 0000 

korrigera vänster 1 0001 

korrigera höger 1 0010 

hittat en korsning 1 1111 

Den första biten i bitmönstret betecknar grupp: 1 står för grupp två 

Tabell 5.2 Korrigeringssignaler från sensorenheten till styrenheten. 



Platon 

2003-12-09 

5.3.3 Signalprioritet 

När sensorenheten går i en korridor och söker efter en korsning eller 

korrigering en situation där det både behöver skickas en korrigeringssignal och 

signal för korsningtyp uppstå. För att kunna hantera detta har de olika 

signalerna ur grupp två olika prioritet. Inträffar två situationer samtidigt väljs 

den signal som har högst prioritet. 

Prioritet: 

1. Korsning funnen 

2. Korrigering 

3. Återvändsgränd 

5.3.4 Display 

Eftersom sensorenheten inte ger ett lika konkret resultat som styrenheten eller 

fjärrstyrenheten har vi utrustat den med en display för att återge vad 

sensorenheten ger för utsignaler. Detta underlättar testning av roboten och 

kontroll av sensorenhetens resultat. 

Vi har därför valt att bygga in en display bestående av ett sjusegment. Detta är 

inkopplat med ett motstånd på 120 ohm för att begränsa strömmen samt en 

avkodare och tolv stycken inverterare. Signalen som avkodas är de fyra 

signalbitarna som kommer från sensorenheten. 

5.4 Sensorer 

5.4.1 Hårdvara 

Vårt sensorsystem består av tre ultraljudssensorer av typen SRF04, en placerad 

längst fram på roboten och en på vardera sida av roboten. Sensorerna har 

förutom en pinne för 0V/+5V även två pinnar för avläsning av sensorerna. Den 

ena kallas trigger-input och den andra för echo-out. För att läsa av sensorn 

skickar man först en kort puls till trigger-input. Sensorn skickar då i väg en 

puls av ultraljud som sedan reflekteras om det är något framför sensorn. Strax 

efter att pulsen skickas i väg sätts echo-out till hög av sensorn. Denna stannar 

sedan hög tills att sensorn får tillbaka ultraljudspulsen från något reflekterande 

objekt eller tills att mätningen gör en time-out. För att läsa av sensorerna 

används en åttabitars port på PIC:en. De lägsta bitarna är signalerna till triggeroutput 

och de höga är echo-out signalen från sensorerna. Denna port är alltså 

både en in- och utport. De höga bitarna input och de låga output. 

5.4.2 Namngivning 

Sensorerna har vi namngivit med siffror. Den högra sensorn har fått nummer 1, 

den vänstra nummer 2 och den främre sensorn har fått nummer 3, allt enligt 

figur 5.1. 



Platon 

2003-12-09 

3 

2 

1 

Figur 5.1 

Placering av sensorer på roboten. 

6 Användarhandledning 

Platon är en sexbent robot utförd i härligt gul plast. Roboten kan användas i två olika lägen; 

ett manuellt läge, där användaren kontrollerar dess beteende med hjälp av kommandon från en 

fjärrkontroll, och ett autonomt läge, i vilket roboten kan söka sig igenom en labyrint på egen 

hand. En översiktlig vy över den fjärrkontroll som används för att styra roboten visas i 

figur 6.1. I tabell 6.1 ges en förklaring av de kommandoknappar som behövs för att styra 

Platon. 

Figur 6.1 Fjärrkontrollen och dess knappar (se även tabell 6.1). 

Knapp nr 

Kommando 

1 autonomt läge på/av 

2 VCR (sätter fjärrkontrollen i rätt läge) 

3 sväng vänster 

4 gå framåt 

5 sväng höger 

6 stanna/filosofera 

7 gå bakåt 

8 Ljudsignal 

Tabell 6.1 Förklaring av knappar och kommandon (se även figur 6.1). 

Innan Platon startas, bör man förvissa sig om att fjärrkontrollen befinner sig i rätt läge genom 

att trycka på knappen VCR på fjärrkontrollen. Sedan startas roboten med hjälp av 

skjutomkopplaren längst fram på virkortet på robotens rygg. Om allting står rätt till tänds då 



Platon 

2003-12-09 

en liten röd diod strax intill omkopplaren. Skjutomkopplaren och dioden är markerade i 

figur 6.2 nedan. 

Figur 6.2 

Platon sedd uppifrån. 

Roboten styrs i sitt manuella läge genom fjärrkontrollen. Genom knapptryckningar kan man 

då styra roboten. När man tryckt ner till exempel ”gå framåt” en gång, kommer sedan roboten 

att fortsätta gå framåt tills ett annat kommando, till exempel ”stanna” eller ”sväng vänster” 

ges. Undantaget från denna regel är kommandot ”ljudsignal”. När Platon mottar detta 

kommando, ger han en kort ljudsignal. Detta kommando påverkar inte robotens rörelser på 

något sätt. I vissa miljöer kan det finnas mycket störningar från till exempel lysrör. Då kan det 

vara svårt att få Platon att reagera på kommandon. Det lönar sig då att vara envis och trycka in 

rätt knapp ett antal gånger tills kommandot går fram. Knappen kan även hålls in kontinuerligt. 

Vill man att Platon ska söka sig igenom en labyrint, bör man först se till att han står rakt 

framför labyrintens ingång. Därefter väljer man autonomt läge, antingen med hjälp av 

knappen autonomt läge på/av på fjärrkontrollen eller med den knapp på roboten som har 

motsvarande funktion. När autonomt läge är valt, lyser den stora röda dioden på robotens 

virkort. Knappen och dioden finns utmärkta i figur 6.2. Medan roboten befinner sig i 

autonomt läge reagerar den endast på kommandona ”ljudsignal” och ”autonomt läge av” från 

fjärrkontrollen. För att åter välja manuellt läge, trycker man bara ännu en gång på knappen på 

fjärrkontrollen eller roboten. Den stora dioden kommer då att släckas. 

7 Referenser 

Designspecifikation, Landén, David, 2003. 

Kravspecifikation, Rees, Johan, 2003. 

Systemskiss, Fåk, Pia, 2003. 



Bilaga 1 

Hårdvarulista 

Fjärrstyrningsenhet 

fjärr1. en generell fjärrkontroll av modell BW0368 Remotec inställd på det Sonyprotokoll 

som specificeras i den tekniska beskrivningen av fjärrkontrollen 

fjärr2. en IR-mottagardiod från Everlight av modell IRM-86901S 

fjärr3. en mikrokontroller från Microchip av modell PIC16F870 med tillhörande 

fördröjd reset som består av två kondensatorer och en resistor. 

fjärr4. en tryckknapp 

fjärr5. en röd lysdiod med tillhörande motstånd 

fjärr6. en summer 

fjärr7. en avstudsarkrets till tryckknappen 74LS00 

Sensorenhet 

sensor1. ultraljudssensor från Devantech av modell SRF04 

sensor2. 

sensor3. 

sensor4. en mikrokontroller från Microchip av modell PIC16F870 

sensor5. ett sjusegment från Everlight av typen ELS-515EWA 

sensor6. en BCD till sjusegment avkodare 74LS47 

sensor7. två 6x inverterare 74LS04 

sensor8. ett motstånd 150 ohm 

Styrenhet 

styr1. 

styr2. 

styr3. 

en AVR Atmel Mega16 till styrlogik, 

en AVR Atmel Mega16 till steglogik samt 

ett robotskelett med förmonterade servon och ben. 

Samtliga enheter kommunicerar med varandra med hjälp av rättframma tråddragningar. 

Samma sorts informationsöverföring används även inom enheterna då olika komponenter 

anslutits till varandra, så när som på fjärrkontrollen och IR-detektorn som naturligtvis skickar 

och tar emot signaler i form av IR-ljus. 

I bilagorna A1, B1 och C1 visas hur de olika enheternas komponenter är anslutna till 

varandra.

Bilaga A1 

Kretsschema för fjärrstyrningsenheten

Bilaga B1 

Kretsschema för styrenheten 

Styrkommando 

från fjärrenhet 

Styrkommando 

till sensorenhet 

A1 

A2 

Strobe från 

fjärrenhet 

Styr1: styrlogik 

15 

16 

17 

18 

40 

39 

38 

37 

36 

35 

34 

33 

32 

31 

30 

29 

28 

27 

26 

25 

24 

23 

19 22 

20 21 

Styrkommando 

från sensorenhet 

B5 

B4 

B3 

B2 

B1 

Styr2: steglogik 

15 

16 

17 

18 

40 

39 

38 

37 

36 

35 

34 

33 

32 

31 

30 

29 

28 

27 

26 

25 

24 

23 

19 22 

20 21 

Styr3: servon

Bilaga C1 

Kretsschema för sensorenheten

Bilaga A2 

Assemblerkod för fjärrstyrningsenheten 

;************************************************************** 

; 

; PROGRAM fjarr 

; 

; Version 1.0 

; 

; Skapare Pia Fåk och Jessica Heyman, grupp15 

; Skapad 2003-11-07 

; 

; Ändringar 

; 2003-11-10 CLRF PORTA får portA att fungera 

; 

; 2003-11-11 tutkod tillagd 

; 

; 2003-11-12 skickar 000 på portA även vid autonomt 

; läge av 

; 

; 2003-11-27 skickar stoppkod till styrenhet 

; vid uppstart 

; 

; 

;************************************************************** 

; 

; Programmet ska ta emot signaler från en IR-diod och 

; skicka data till styrmodulen, samt sköta om tändning 

; och släckning av diod, tutning, signaler från 

; tryckknapp m.m. 

; 

; Version 1.0: programvaran används i färdig robot 

; 

; Programmet fungerar under förutsättning att de IR-signaler 

; som inkommer till enheten till någorlunda stor del består 

; en viss delmängd av signalerna i RC5-protokollet 

TITLE "fjarr" 

LIST P=16F870, F=INHX8M 

#INCLUDE "P16F870.INC" 

__CONFIG _XT_OSC & _WDT_OFF & _PWRTE_ON 

;************************************************************** 

; Definition av konstanter 

;************************************************************** 

; Konstanter som motsvarar specifika bitar i registren 

; 

KNAPP EQU 7 ; bitar i ut- och ingångar 

IRDET EQU 0 

STYR0 EQU 0 

STYR1 EQU 1 

STYR2 EQU 2 

STROBE EQU 3 

TUTA EQU 1 

DIOD EQU 5 

ONOFF EQU 0 ;inre tillstånd 

SECOND EQU 1 ;vill vi ha en likadan signal eller ej 

TOGG EQU 5 ;förra togglebiten 

; Egendefinierade registernamn 

STATREG EQU 0x20 ;autonomt läge av/på lagras här 

COMMAND EQU 0x21 ;här bor det som ev. ska skickas 

COUNTM EQU 0x22 ;räknar hur många metabitar vi fått 

COUNTS EQU 0x23 ;räknar hur många signalbitar vi fått 

SIGNAL EQU 0x24 ;viktig insignal från IR-detektorn 

META EQU 0x25 ;register för intstoppning av metadata 

LASTSIG EQU 0x26 ;den förra signalen ligger här 

LJUD EQU 0x27 ;här lägger vi ett ljud 

COUNTL EQU 0x28 ;räknar hur många tut vi tutat 

; signalbitsmönster 

STANNA EQU b'00001001' 

FRAM EQU b'00001010' 

BAK EQU b'00001111' 

VANSTER EQU b'00001101' 

HOGER EQU b'00001011' 

TUT EQU b'00001000' 

AUTON EQU b'00110011'

;************************************************************** 

; Program-yta 

;************************************************************** 

; Start av program-yta 

; 

ORG 0x00 

GOTO MAIN ; Gå till huvudprogram 

ORG 0x04 

GOTO ISR ; Gå till avbrottsrutin 

;************************************************************** 

; Avbrottsrutiner 

; 

; IRKOM 

; TMRAVB 

; TMR1AVB 

; TRYCKAVB 

; 

; som använder SKICKA 

; och SIGKOLL 

;************************************************************** 

;-------------------------------------------------------------- 

; Subrutin IRKOM 

; anropas när en IR-signal först börjar komma in till processorn 

; Sätter igång en timer för att börja sampla signalen, stänger 

; av vidare avbrott pga av fallande flanker på RB0 

IRKOM BCF STATUS,RP0 ;byt till bank 0 

BCF STATUS,RP1 ;bank 0, ju. 

BCF INTCON,INTE ;INTE några fler RB0-avbrott 

MOVLW b'00000000' ;0 = lite mindre än 5T 

MOVWF TMR0 ;till TMR0! 

BCF INTCON,T0IF ;cleara TMR0-avbrottsflaggan så att 

;inget gammal skräp avbryter oss 

BSF INTCON,T0IE ;nu börjar vi lyssna efter TMR0-avbrott 

BCF INTCON,INTF ;cleara RB0-avbrottsflagga 

RETURN 

;-------------------------------------------------------------- 

;-------------------------------------------------------------- 

; Subrutin TMR1AVB 

; Sköter om tutningen. 

; ge 1 eller 0 till summer beroende på ljudregistrets LSB 

; stäng av vidare TMR1-avbrott om ljudsignal har getts färdigt 

TMR1AVB 

;bestäm ljud, LSB i LJUD ger 1=tut, 0=inget 

BCF 

PIR1,TMR1IF 

RRF LJUD,1 ;rotera ljudsignal 

BTFSS STATUS,C ;roterat ut en etta 

GOTO NOLLA ;ja, en etta, tuta 

;etta 

BSF PORTB,TUTA ;etta => tut! 

GOTO 

DONE 

NOLLA BCF PORTB,TUTA ;nolla, tuta inte 

DONE BCF STATUS,C ;nolla carryn 

RRF COUNTL,1 ;rotera räknaren, roterar in 0 

BTFSS STATUS,C ;färdigtutad 

RETURN 

;ej färdigtutad, bara att fortsätta 

;färdigtutat, stäng av timer1 

BCF PORTB,TUTA ;stäng av tutan, för nu är den färdig 

BCF T1CON,TMR1ON ;stäng av TMR1, färdiguttimrad! 

BSF STATUS,RP0 ;bank1 

BCF PIE1,TMR1IE ;stäng av TMR1-avbrott också, 

;för säkerhets skull 

BCF STATUS,RP1 ;bank0 

RETURN 

;-------------------------------------------------------------- 

;-------------------------------------------------------------- 

; Subrutin TMRAVB 

; Sköter det mesta av inläsningen av en IR-signal 

; Rutinen läser av RB0, och lagrar undan värdet och räknar upp 

; hur många gånger det skett. Är det metadata som kommer, läggs 

; det i registret META, annars lägger man det i DATA. 

; Har vi samplat signalen 14 ggr är vi färdiga. Då

; stänger vi av TMR0 och kollar om vi kanske har något 

; att skicka till styrenheten. 

; Annars sätts timern igen och förloppet kommer att upprepas. 

; IN: RB0 - signalen seriellt 

; UT: skickar via subrutin SKICKA kommando till styrenheten. 

TMRAVB BCF STATUS,RP0 ;Byt bank! 

BCF STATUS,RP1 ;Bank 0. 

BCF STATUS,C ;se till att carryn är 0. 

RRF COUNTM,0 ;låtsasuppdatera räknare 

BTFSS STATUS,C ;kolla vad carryn blev 

GOTO METADAT ;det är ju metadata som kommer nu! 

SDATA BTFSS PORTB,IRDET ;är det månne en etta vi har 

GOTO INTEOR2 ;nä, då ORAR vi inte. 

MOVF COUNTS,0 ;Flytta countervärdet till W 

IORWF SIGNAL,1 ;ORA W med SIGNAL 

INTEOR2 BCF STATUS,C ;carryn måste vara noll 

RRF COUNTS,1 ;ROTERA! 

BTFSS STATUS,C ;colla carryn! 

GOTO SCHLUT ;inte färdigmottagen signal än 

SIGREDY CALL SIGKOLL ;signal färdigmottagen, kolla om vi 

; tycker om den 

CLRF META ;nolla META för nu har den använts 

; färdigt 

CLRF SIGNAL ;nolla SIGNAL för den är lika 

; färdig som META 

MOVLW b'10000000' ;Metaräknaren ska förstås vara åtta, 

MOVWF COUNTM ; på ett mystiskt vis. 

MOVLW b'00100000' ;Signalräknaren ska förstås vara fem, 

MOVWF COUNTS ; på ett mystiskt vis, den med. 

BCF INTCON,T0IE ;stäng av vidare TMR0-interrupt 

BCF INTCON,INTF ;cleara RB0-avbrottsflaggan 

BSF INTCON,INTE ;sätt på vidare RB0-avbrott 

GOTO 

SCHLUT 

METADAT RRF COUNTM,1 ;uppdatera räknaren på riktigt 

BTFSS PORTB,IRDET ;kolla om vi har en etta att stoppa in 

GOTO 

SCHLUT 

MOVF COUNTM,0 ;flytta countervärdet till W 

IORWF META,1 ;ORA W med META. 

SCHLUT MOVLW d'36' ;36! 

MOVWF TMR0 ;till TMR0! 

BCF INTCON,T0IF ;cleara flaggan 

RETURN 

;-------------------------------------------------------------- 

;-------------------------------------------------------------- 

; Subrutin TRYCKAVB 

; Anropas vid tryck på tryckknappen. När man släpper upp knappen 

; kommer ett "extra" avbrott, som ska ignoreras 

; Om riktigt avbrott: toggla autonomt läge på/av i STATREG, 

; Om ON: tänd diod, skriv i COMMAND, skicka. 

; Om OFF: släck diod. 

TRYCKAVB BCF STATUS,RP0 ;byt till bank 0 

BCF 

STATUS,RP1 

BTFSC PORTB,KNAPP ;om biten är ett, är avbrottet falskt 

GOTO SLUT ;avbrottet falskt, gör inget mer! 

BTFSS STATREG,0 ;kolla hälsan 

GOTO ON ;Den var satt och tjock! 

;stäng av autonomt läge 

OFF BCF PORTA,DIOD ;släck dioden! 

BCF STATREG,ONOFF ;byt den mentala hälsan, 

;nu är vi oautonoma 

MOVLW b'00000000' ;skriv i command-reg! 

MOVWF COMMAND ;gör det på riktigt 

CALL SKICKA ;skicka. OBS! görs efter att 

;vi stängt av autonomt läge 

GOTO 

SLUT 

;sätt på autonomt läge 

ON BSF PORTA,DIOD ;tänd dioden! 

MOVLW b'00000000' ;skriv i command-reg! 

MOVWF COMMAND ;gör det på riktigt 

CALL SKICKA ;skicka. OBS! görs innan 

;vi sätter på autonomt läge 

BSF STATREG,ONOFF ;nu är vi autonoma 

SLUT BCF INTCON,RBIF ;cleara tryckavbrottsflaggan

BCF INTCON,T0IF ;cleara TMR0-avbrottsflaggan 

RETURN 

;-------------------------------------------------------------- 

;-------------------------------------------------------------- 

; Subrutin SIGKOLL 

; Tittar på mottaget signaldata och kontrollerar om det är 

; vettigt, dvs metadata stämmer och signalen är någon vi 

; lyssnar efter. 

; Är signalen vettig, läggs data i registret 

; COMMAND, varpå detta skickas mha SKICKA-rutinen. 

; Utom om det är tutkommandot som kommer in, då startas 

; tutningen. 

; Är signalen inte vettig, skrotas den bums. 

SIGKOLL MOVF META,0 ;lägg META i W 

ANDLW b'11011111' ;Cleara togglebiten 

SUBLW b'00011111' ;"jämför" 

BTFSS STATUS,Z ;var de lika 

GOTO SKROT ;nä, då skrotar vi värdet 

BRA BTFSS META,TOGG ;TOGGLE tjock 

GOTO 

SMAL 

MOVLW b'10000000' ;Tal att ora med 

IORWF SIGNAL,1 ;ORA in den där ettan 

SMAL MOVF SIGNAL,0 ;stoppa SIGNAL i W 

SUBWF LASTSIG,0 ;"jämför" 

BTFSC STATUS,Z ;feeeeett 

GOTO KOLLIK ;Vi har just haft en sån! 

;Kolla om de ska vara lika 

MOVF SIGNAL,0 ;lägg den i W igen. 

MOVWF LASTSIG ;Stoppa W (SIGNAL) i LASTSIG 

;för framtida bruk 

BSF STATREG,SECOND ;Vi vill att nästa signal som kommer 

;ska se likadan ut 

GOTO SKROT ;Vi är inte intresserade av en 

;signal som inte har kommit en gång 

;redan 

KOLLIK BTFSS STATREG,SECOND ;är det så att signalerna ska vara lika 

GOTO SKROT ;det ska de inte, så vi slänger signalen 

BCF STATREG,SECOND ;nu ska signalerna inte vara lika längre! 

HITTA BCF SIGNAL,7 ;återställ signal (ta bort togglebiten) 

MOVLW STANNA ;Stoppa in testvärde i W 

SUBWF SIGNAL,0 ;"jämför" 

BTFSC STATUS,Z ;lika 

GOTO STOP ;men gör stop då! 

MOVLW FRAM ;Stoppa in testvärde i W 



GOTO FORWARD ;men gör forward då! 

MOVLW BAK ;Stoppa in testvärde i W 



GOTO BACK ;men gör back då! 

MOVLW VANSTER ;Stoppa in testvärde i W 



GOTO LEFT ;men gör left då! 

MOVLW HOGER ;Stoppa in testvärde i W 



GOTO RIGHT ;men gör right då! 

MOVLW TUT ;Stoppa in testvärde i W 



GOTO HORN ;men gör horn då! 

MOVLW AUTON ;Stoppa in testvärde i W 



GOTO INDEPEN ;men gör indepen då! 

GOTO 

SKROT 

STOP MOVLW b'00000101' ;stoppkommandot in i W 

MOVWF COMMAND ;in i COMMAND 

CALL 

SKICKA

RETURN 

FORWARD MOVLW b'00000001' ;stoppkommandot in i W 


CALL 

SKICKA 

RETURN 

BACK MOVLW b'00000010' ;stoppkommandot in i W 


CALL 

SKICKA 

RETURN 

LEFT MOVLW b'00000100' ;stoppkommandot in i W 


CALL 

SKICKA 

RETURN 

RIGHT MOVLW b'00000011' ;stoppkommandot in i W 


CALL 

SKICKA 

RETURN 

HORN MOVLW b'11111011' ;ljudsignal består av 6 tut 

MOVWF LJUD ;in i ljudregistret 

MOVLW b'10000000' ;räknare 

MOVWF COUNTL ;in i ljudräknaren 

;sätt timer att timra ut om 0,26 s 

CLRF TMR1L ;nollställ TMR1 

CLRF TMR1H ;nollställ hela TMR1 

MOVLW b'00000001' ;prescale=4, TMR1=on 

MOVWF T1CON ;in i kontrollregistret 

BCF PIR1,TMR1IF ;cleara TMR1-avbrottsflaggan 

BSF INTCON,PEIE ;enabla perifera avbrott (TMR1) 

BSF STATUS,RP0 ;bank1 

MOVLW b'00000001' ;enabla TMR1-avbrott på riktigt 

MOVWF PIE1 ;in i kontrollregistret, oxå. 

BCF STATUS,RP0 ;bank0 

RETURN 

INDEPEN BTFSC STATREG,ONOFF ;autonomt läge på 

GOTO PA ; japps, gå till PA för att stänga av 

;sätt på autonomt läge 

AV BSF PORTA,DIOD ;tänd dioden, för vi var av 

MOVLW b'00000000' ;autonomt läge på in i W 


CALL SKICKA ;skicka iväg också 

BSF STATREG,ONOFF ;sätt på. 

;OBS! skicka autonomt PÅ först! 

RETURN 

;stäng av autonomt läge 

PA BCF PORTA,DIOD ;släck dioden 

BCF STATREG,ONOFF ;stäng av autonomt läge, för vi var på! 

MOVLW b'00000000' ;autonomt läge på in i W 


CALL SKICKA ;skicka iväg också. 

;OBS! skicka autonomt av efteråt! 

RETURN 

SKROT RETURN ;nä, precis. Inget var det. 

;-------------------------------------------------------------- 

;-------------------------------------------------------------- 

; Subrutin SKICKA 

; Lägg ut det som finns i COMMAND på STYR0-2 

; för att inte ändra värdena som ligger i PORTA i 

; onödan, vidtas visst krångel 

SKICKA BTFSC STATREG,ONOFF ;om vi är autonoma, skicka ej 

GOTO NEJ ; vi var autonoma och struntar i 

; att lyssna 

JA MOVLW b'11111000' ;stoppa en AND-vektor i W 

ANDWF PORTA,1 ;se till att det bor nollor 

; på rätt postioner i PORTA 

MOVF COMMAND,0 ;stoppa värdet i COMMAND i W 

IORWF PORTA,1 ;skriv rätt värden till PORTA 

BCF PORTA,STROBE ;Stroba! 

NOP 

;här har vi en massa NOPar

NOP 

;för att få längre strobe 

NOP 

NOP 

NOP 

NOP 

NOP 

NOP 

NOP 

NOP 

BSF PORTA,STROBE ;sluta stroba! 

NEJ CLRF COMMAND 

RETURN 

;-------------------------------------------------------------- 

;************************************************************** 

; Avbrottsrutin 

;************************************************************** 

; Avbrottskällor: 

; IRDET insignal från IR-detektor på RB0 

; avbryter på fallande flank 

; TRYCKKNAPP insignal från tryckknapp på portA,knapp 

; avbryter på change 

; TMR1 avbrott från tut-timern, som ändrar tut- 

; signal när det behövs 

; TMR0 avbryter med 4Ts mellanrum för att läsa 

; av signal 

; 

; Kollar på avbrottsflaggor i en Bra Ordning. 

; 

;Om RB0-avbrott är på, ska de servas. Annars måste vi strunta i 

; dess avbrottsflagga 

ISR BTFSC INTCON,INTE ;är RB0-avbrott på 

GOTO MAYBERB ;ja, det är de! 

NOTRB0 BTFSC INTCON,RBIF ;är det avbrott från tryckknappen 

CALL TRYCKAVB ; då går vi till TRYCKAVB 

BTFSC PIR1,TMR1IF ;är det timer1-avbrott 

CALL TMR1AVB ; då går vi till TMR1AVB 

BTFSC INTCON,T0IF ;är det timer0-avbrott 

CALL TMRAVB ; då går vi till TMRAVB 

RETFIE 

MAYBERB BTFSC INTCON,RBIF ;är det avbrott från tryckknappen 

CALL TRYCKAVB ; då går vi till TRYCKAVB 

BTFSC INTCON,INTF ;är det avbrott från RB0 

CALL IRKOM ; då går vi till IRKOM 

BTFSC PIR1,TMR1IF ;är det timer1-avbrott 

CALL TMR1AVB ; då går vi till TMR1AVB 

RETFIE 

;************************************************************** 

; Huvudprogram 

;************************************************************** 

; MAIN körs först av allt när processorn startar. 

; den initierar alla register och sådär, och fastnar sedan i en 

; oändlig loop som bara väntar på att bli avbruten. 

MAIN BCF STATUS,RP0 ;välj bank 0 

BCF STATUS,RP1 ;välj mer bank 0 

CLRF 

PORTA 

MOVLW b'10011000' ;Stoppa 11110000 i INTCON 

MOVWF INTCON ;ger interrupt på, p-interrupt av, 

;TMR0-overflow interrupt av, 

;rb-interrupt på, RB change-interrupt på. 

CLRF STATREG ;Auto-tillstånd ska vara AV 

MOVLW b'10000000' ;Metaräknaren ska förstås vara åtta, 

MOVWF COUNTM ; på ett mystiskt vis. 

MOVLW b'00100000' ;Signalräknaren ska förstås vara fem, 

MOVWF COUNTS ; på ett mystiskt vis, den med. 

CLRF COMMAND ;vi vill inte ha några ettor här. 

CLRF SIGNAL ;inte här heller. 

CLRF 

META 

CLRF 

LASTSIG 

CLRF 

LJUD 

BCF STATUS, RP1 ;välj bank 1 

BSF STATUS, RP0 ;bank1 definitivt 

MOVLW b'00000110' ;Stoppa en sexa i ADCON1 

;(inga analoga inputs här inte)

MOVWF ADCON1 ;Stoppa mer sexa i ADCON1 

MOVLW b'00000000' ;trisa: output = 0 

MOVWF 

TRISA 

MOVLW b'11111101' ;trisb: inputs = 1, RB1=TUTA 

MOVWF 

TRISB 

MOVLW b'10010010' ;sätt falling edge på RB0-interrupt, 

;TIMER0 ska lyssna på internal clock 

;prescalern ska höra till TMR0 och 

; ha skalan 1:8. 

MOVWF OPTION_REG ;ladda in i option_reg också, ju. 

BCF STATUS, RP0 ;välj bank0 

MOVLW b'00000101' ;vi vill skicka "stopp" till 

MOVWF PORTA ;styrenheten till att börja med 

BSF PORTA,STROBE ;stroben är aktivt låg, så nu ska 

;den vara hög 

LOOP GOTO LOOP ;här är en oändlig loop. 

END

Bilaga B2 

Assemblerkod för styrenheten 

;************************************************************** 

; 

; PROGRAM styr 

; 

; Version 1.1 

; 

; Skapare Johan Mårtenson och Johan Rees, grupp15 

; Skapad 2003-11-25 

; 

; Ändringar 

; 2003-11-25 Kodhuvud tillagt 

; 

; 2003-11-27 Justering av korsningsdetektion 

; 

; 2003-11-30 Gå längre in i korsningar 

; 

; 2003-12-04 Återvändsgränder är ett specialfall. 

; 

; 

;************************************************************** 

; 

; Programmet hämtar indata från fjärr- och sensorenhet 

; och styr utifrån dessa roboten. Algoritmen som används är 

; att alltid svänga vänster vilket garanterat för den ut 

; ur labyrinten. 

; 


; 

; Programmet beror av att det är inkopplat till rätt moduler. 

; 

.include "m16def.inc" 

; Obs! Är KOMMANDO == 0x00 så är vi i autonomt! 

.def TEMP = R16 

.def LOOP_INDEX3 = R17 

.def KOMMANDO = R18 

.def AUTO_UT= R19 

.def LOOP_INDEX2 = R20 

.def SAVE_REGS = R21 

.def SENASTE_KORSNING = R22 

.def AUTONOM = R23 

.def LOOP_INDEX = R24 

.def APORTEN = R25 

rjmp INIT 

; ----- Definierar interruptadresser ----- 

.org INT0addr 

rjmp INT0_INTERRUPT 

; Låt INIT börja på 0x50 så blir det inte så trångt i minnet. 

.org 0x50 

; ----- Init-funktionen ställer i ordning chipet och hoppar sedan till huvudloopen ----- 

INIT: 

; Sätter stackpekaren 

ldi TEMP, HIGH(RAMEND) 

out SPH, TEMP 

ldi TEMP, LOW(RAMEND) 

out SPL, TEMP 

; Port a går till steg från styr 

ldi TEMP, 0xFF 

out DDRA, TEMP 

; Port b läser ifrån fjärren 

ldi TEMP, 0x00 

out DDRB, TEMP 

; Port d går till sensor 

ldi TEMP, 0b11110011 ; Ska vi använda int1 också 

out DDRD, TEMP 

; Port c går från sensor 

ldi TEMP, 0xE0 

out DDRC, TEMP

; Slår på externt interrupt på pinne int0. 

; Interruptet triggar på negativ flank. 

ldi TEMP, (1

out PORTA, TEMP 

call VANTA_MELLAN 

steg 



call VANTA_FJARDE 

ori AUTO_UT, 0x01 

out PORTD, AUTO_UT 

rjmp AUTO_SLUT 

; Vänta en stund så vi inte förvirrar steglogiken. Vanta2 måste vänta längre än ett 

; Sen så går vi rakt fram igen 

; Sätt A1 hög 

; Om vi ska justera höger så. 

AUTO_ADJ_RIGHT: 

; andi AUTO_UT, 0xFe ; A1 låg.. 

; out PORTD, AUTO_UT 

ldi TEMP, 0x04 ; Sväng höger 


call VANTA_HALV ; Vänta en stund, tidigare halv 

ldi TEMP, 0x01 ; Sen så går vi framåt igen 



ori AUTO_UT, 0x01 ; A1 sätts hög så vi får indata igen 



; Nu har en korsning hittats men vi vet inte vilken sort det är. 

AUTO_FOUND: 

andi AUTO_UT, 0xFE 





; call VANTA_HALV 

ori AUTO_UT, 0x02 


; Lägg A1 låg 

; Gå framåt 

; Gå framåt i en sekund. 

; Lägg A2 hög 

; Nu har vi hittat en korsning så därför måste vi se 

; till att nästa inte kommer för snabbt inpå. 

KORSNINGSTYP: 

in TEMP, PINC 

cpi TEMP, 0x00 

breq KORSNINGSTYP 

cpi TEMP, 0x1F 

breq KORSNINGSTYP 

; Läs in från sensor 

; Jämför med 0x00 

; Loopa ett varv till 

nop 

nop 

nop 

in TEMP, PINC ; Förhoppningsvis är signalen stabil nu. 

andi AUTO_UT, 0b11111101 ; Lägg A2 låg 


; Vilken sorts korsning är vi i Gå på lämpligt sätt! 


breq AUTO_LEFT_TURN 


breq AUTO_RIGHT_TURN 


breq AUTO_T_TURN 


breq AUTO_FORWARD_LEFT 


breq AUTO_FORWARD_RIGHT 


breq AUTO_FOUR_WAY 

rjmp HUVUDLOOP 

; Om vi råkar ut för en vänstersväng så snurrar vi vänster 90 grader. 

AUTO_LEFT_TURN: 

ldi TEMP, 0x0A ; Snurra 90 grader vänster. 


call VANTA_LANG ; Vänta så vi hinner svänga helt. 

rjmp AUTO_KORSNING_KLAR 

; Om vi råkar ut för en högersväng så snurrar vi höger 90 grader. 

AUTO_RIGHT_TURN: 

ldi TEMP, 0x09 ; Snurra 90 grader höger. 


call VANTA_LANG ; Vänta så vi hinner svänga helt. 

rjmp AUTO_KORSNING_KLAR

; Om vi hamnar i en t-korsning så svänger vi höger. 

AUTO_T_TURN: 

ldi SENASTE_KORSNING, 0xFF 



call VANTA_LANG 

; Vänta så vi hinner svänga helt. 


; Om vi hamnar i en korsning som svänger vänster och går framåt så går vi framåt. 

AUTO_FORWARD_LEFT: 

ldi TEMP, 0x01 ; Gå framåt. 




; Om vi hamnar i en korsning som går rakt fram eller höger så väljer vi höger. 

AUTO_FORWARD_RIGHT: 




; Vänta så vi hinner börja snurra. 


; Om vi kommer in i en fyrvägskorsning så svänger vi höger. 

AUTO_FOUR_WAY: 

ldi SENASTE_KORSNING, 0xFF 

rjmp AUTO_FORWARD_RIGHT 

; Om vi vet att vi är i mål är det slut! 

AUTO_GOAL: 

cpi SENASTE_KORSNING, 0x00 

breq AUTO_SLUT 

ldi KOMMANDO, 0xFF 




; Om inget behövde göras går vi framåt. 

AUTO_NOTHING: 




; Efter att vi gått i en korsning går vi framåt igen. 

AUTO_KORSNING_KLAR: 



ori AUTO_UT, 0x01 ; Sätt A1 hög. 








in TEMP, PINC 

andi TEMP, 0x1F 

cpi TEMP, 0x1F 

breq AUTO_GOAL 

; Vänta så vi hinner komma ut ur korsningen. 

; Vi läser in för att se om vi står i en korsning igen. 

; Stod vi i en korsning går vi till AUTO_GOAL 

ldi SENASTE_KORSNING, 0x00 

; Efter genomförd algoritm sätts A1 hög. 

AUTO_SLUT: 

ori AUTO_UT, 0x01 ; Sätt A1 hög. 



; Fjarrstyrt hanterar kommandon från fjärrkontrollen. 

FJARRSTYRT: 

; Hoppa till rätt ställe utifrån kommando 

cpi KOMMANDO, 0x00 

breq AUTO 


breq STANNA 


breq FRAM 


breq BAK 


breq SVANG_VANSTER 


breq SVANG_HOGER

; Var kommando inget av ovanstående går vi ur fjarrstyrt. 

rjmp LOOP_SLUT 

; Gick vi precis in i autonomt läge så gör vi det. 

AUTO: 

ldi KOMMANDO, 0x00 ; Nu gick vi in i autonomt läge. 


; Nu ska vi stå still. 

STANNA: 



; Nu ska vi gå framåt. 

FRAM: 



; Nu ska vi gå bakåt. 

BAK: 



; Säg åt steg att inte göra någonting. 

; Säg åt steg att gå framåt. 

; Gå bakåt. 

; Nu ska vi svänga vänster. 

SVANG_VANSTER: 

ldi TEMP, 0x03 ; Sväng vänster. 


; Nu ska vi svänga höger. 

SVANG_HOGER: 

ldi TEMP, 0x04 ; Sväng höger. 


; Nu ska vi snurra 180 grader. 

; SNURRA: 

; ldi TEMP, 0x05 

; rjmp LOOP_SLUT 

; Förflytta parallellt höger. 

PARALLELL_HOGER: 

ldi TEMP, 0x06 ; Parallellförflytta höger. 


; Förflytta parallellt vänster. 

PARALLELL_VANSTER: 

ldi TEMP, 0x07 ; Parallellförflytta vänster. 


; Sista anhalten innan funktionen är klar. 

LOOP_SLUT: 

out PORTA, TEMP ; Lägg ut temp på porta, dvs kommandot till steg. 


; ----- Vanta2 gör en busyloop i XX sekunder ----- 

VANTA2: 

; Vi nollställer ett loopindex innan loopen. 

ldi LOOP_INDEX, 0x00 

; Här kommer den yttre loopen 

VANTA2_VANTA: 

; Jämför loopindex med 0xFF för att se om vi är klara. 

cpi LOOP_INDEX, 0xFF 

breq VANTA2_KLART 

; Annars ökar vi loopindex med 1. 

inc LOOP_INDEX 

; Nollställ andra loopindex-variabeln. 

ldi LOOP_INDEX2, 0x00 

; Här kommer den inre loopen. 

VANTA2_VANTA_INTERN: 

; Jämför andra loopindex med 0xFF för att se om vi är klara. 

cpi LOOP_INDEX2, 0xFF 

breq VANTA2_KLART_INTERN 

; Annars ökar vi andra loopindex med 1 och lägger till några nop:ar för att 

; förlänga loopen. 

inc LOOP_INDEX2 

nop 

nop 

nop 

nop 

; Loopa ett varv till i inre loopen.

jmp VANTA2_VANTA_INTERN 

VANTA2_KLART_INTERN: 

; Loopa ett varv till i yttre loopen. 

rjmp VANTA2_VANTA 

VANTA2_KLART: 

ret 

; ----- Vanta kör en loop och anropar inne i den vanta2 ----- 

VANTA: 


VANTA_INTERN: 

cpi LOOP_INDEX3, 0x01 

breq VANTA_KLAR 


call VANTA2 

rjmp VANTA_INTERN 

VANTA_KLAR: 

ret 

; ----- Vanta_lang vantar en längre stund ----- 

VANTA_LANG: 


VANTA_LANG_INTERN: 


breq VANTA_LANG_KLAR 


call VANTA2 

rjmp VANTA_LANG_INTERN 

VANTA_LANG_KLAR: 

ret 

; ----- Vanta_halv väntar hälften så länge som vanta_lang ----- 

VANTA_HALV: 


VANTA_HALV_INTERN: 


breq VANTA_HALV_KLAR 


call VANTA2 

rjmp VANTA_HALV_INTERN 

VANTA_HALV_KLAR: 

ret 

; ----- Vanta_fjarde väntar hälften så mycket som vanta_fjarde ----- 

VANTA_FJARDE: 


VANTA_FJARDE_INTERN: 


breq VANTA_FJARDE_KLAR 


call VANTA2 

rjmp VANTA_FJARDE_INTERN 

VANTA_FJARDE_KLAR: 

ret 

; ----- Vanta_mellan väntar nästan lika länge som vanta_halv ----- 

VANTA_MELLAN: 


VANTA_MELLAN_INTERN: 


breq VANTA_MELLAN_KLAR 


call VANTA2 

rjmp VANTA_MELLAN_INTERN 

VANTA_MELLAN_KLAR: 

ret 

; ----- Interruptrutiner----- 

INT0_INTERRUPT: 

in SAVE_REGS, SREG 

mov R14, KOMMANDO 

in KOMMANDO, PINB 

andi KOMMANDO, 0x07 ;rees har ändrat från 03 till 07 


breq INT0_KANSKE_INT ; Autonomt av/på 

out SREG, SAVE_REGS 

reti 

INT0_KANSKE_INT: 

mov KOMMANDO, R14 ; Lägg in r14 i kommando 

cpi KOMMANDO, 0x00 ; är kommando 0x00 ... 

breq INT0_TILL_AUTO ; .. så går vi till int0_till_auto 

cpi KOMMANDO, 0xFF 

breq INT0_TILL_AUTO 

ldi KOMMANDO, 0x00 ; Dessutom sätts KOMMANDO till 0x05 så att vi står still 

andi AUTO_UT, 0b11111100



reti 

INT0_TILL_AUTO: 

ori AUTO_UT, 0b00000001 

ldi KOMMANDO, 0x05 


reti

Bilaga C2 

Assemblerkod för sensorenheten 

;************************************************************** 

; 

; PROGRAM sensor 

; 

; Version 1.0 

; 

; Skapare Mattias Lennartsson och David Landén, grupp15 

; Skapad ht2003 

; 

;************************************************************** 

; 

; Programmet hämtar mätvärden från tre sensorer (fram, 

; höger och vänster), beräknar medelvärdet av de två mätningar 

; som är bäst av tre och skickar utifrån dessa värden signaler 

; till styrenheten om hitta korsning, korrigering eller korsningstyp. 

; 


; ****************************************** 

; Definitioner 

; ****************************************** 

#INCLUDE 

"P16f870.INC" 

TITLE 

"sensor" 

LIST P=16f870, F=INHX8M 

__CONFIG _XT_OSC & _WDT_OFF & _PWRTE_ON 

;Signaler (inte variabler) 

SIGN_FEL equ 0x00 

SIGN_ATERV equ 0x01 

SIGN_VANSTER equ 0x02 

SIGN_HOGER equ 0x03 

SIGN_T equ 0x04 

SIGN_FV equ 0x05 

SIGN_FH equ 0x06 

SIGN_FYRV equ 0x07 

;Variabler för main etc. 

COUNT equ 0x20 ;Räknarvariabel loopar 

DEBUG equ 0x21 

;Variabler för avb 

ST_TEMP equ 0x40 ;Undanlagrade STATUS och W 

W_TEMP equ 0x41 

TID_START equ 0x43 ;0xff -> Mätning av sensorer har startat 

;0x00 -> Mätningen väntar på svar från sensor 

NU_SENSOR equ 0x44 ;Aktuell sensor/mätning 

NU_MATNING equ 0x45 

MV1 equ 0x46 ;Tillfälliga mätvärden för mätningarna 

MV2 equ 0x47 

MV3 equ 0x48 

TEMP equ 0x49 ;Generell variabel 

SENSOR1_MV equ 0x50 ;Sensorernas mätvärden 

SENSOR2_MV equ 0x51 



;Sensor 1 -> Höger 

;Sensor 2 -> Vänster 

;Sensor 3 -> Fram (fd Vänster bak) 

V_AB equ 0x54 ;används av VALJ_VARDE 

V_AC equ 0x55 

V_BC equ 0x56 

MATNING_PA equ 0x57 ;Stänger av timer1 (mätningen) fast uppdaterar utvärdet 

;på timer interrupt 

MAT_BYTE equ 0x58 ;Används av UPPD_UTV

OK_MATV equ 0x59 ;är 0xff om det finns ett mätvärde i minnet 

; annars har den värdet 0x00 

; Blir inaktuellt då det används eller 

;om timerrutinern körs och värdet ligger kvar 

SENSOR1_SLAP equ 0x60 ;Släp-värden för sensor 1 resp 2. 

SENSOR2_SLAP equ 0x61 ; - - 

SENSOR1_SLAP1 equ 0x62 ;Släp värden, nivå 2 

SENSOR2_SLAP1 equ 0x63 







SENSOR1_SLAP5 equ 0x6a ;Släp värden, nivå 6 

SENSOR2_SLAP5 equ 0x6b 

SENSOR1_SLAP6 equ 0x6c ;Släp värden, nivå 7 

SENSOR2_SLAP6 equ 0x6d 

SENSOR1_SLAP7 equ 0x6e ;Släp värden, nivå 8 

SENSOR2_SLAP7 equ 0x6f 





dMV equ 0x74 ;Diff mellan mv 1 och mv 2 

Hmin equ 0x75 

Vmin equ 0x76 

KORSNING_ALLERT equ 

0x77 

; *** OBS 0x7F sista adress för General Perpose Registers *** 

; ****************************************** 

; Programminnet 

; ****************************************** 

ORG 

GOTO 

ORG 

GOTO 

0x00 

MAIN 

0x04 

AVB 

; ****************************************** 

; Avbrottsrutin 

; ****************************************** 

AVB MOVWF W_TEMP ;Spara undan W 

MOVF STATUS,W ;Spara undan STATUS 

MOVWF ST_TEMP ; - - 

BTFSC INTCON,T0IF ;Avbrotts flagga för timer 0 

CALL TIMER0_AVB ;Hoppa till timer avbrott 

BTFSC PIR1,0 ;Avbrotts flagga för timer 0 

CALL TIMER1_AVB ;Hoppa till timer avbrott 

MOVLW 0xF8 ;Maska ut interrupt bitarna (0-3) 

ANDWF INTCON,F ;i INTCON registret (avbrottsflaggor) 

BCF INTCON,INTE ;Inge interrupts från RB0/INT 

BCF PIR1,0 ;Nollställ avbrottsflaggan för timer1 

BSF INTCON,7 ;GIE på 

MOVF ST_TEMP,w ;Återställ W och STATUS 

MOVWF 

STATUS 

MOVF 

W_TEMP,w 

RETFIE 

;Returnera från avbrott 

; ****************************************** 

; Subrutin Timer0 Avbrott

; ****************************************** 

TIMER0_AVB 

BCF STATUS,5 ;Bank 0 

BCF STATUS,6 ; - - 

MOVF PORTB,w ;Läs porten till W 

MOVWF TEMP ;W -> TEMP 

BCF STATUS,C ;Rotera temp 4bit höger 

RRF 

TEMP,f 

BCF 

STATUS,C 

RRF 

TEMP,f 


RRF 

TEMP,f 


RRF 

TEMP,f 

MOVF TEMP,W ;TEMP->W 

ANDWF NU_SENSOR,w ;Maska ut biten från sensorn 

BTFSC STATUS,Z ;Mät av sensorn 

;maskades alla bitar bort -> 0, annars -> 1 

GOTO KLAR ;om sensorns utvärde är 0, gå till klar 

MOVLW 

0xFF 

MOVWF TID_START ;Markera att vi tagit emot en puls 

;BTFSC NU_MATNING,0 ;Räkna upp rätt mätvärde 

;INCF 

MV1,f 

;BTFSC NU_MATNING,1 ; - - 

;INCF 

MV2,f 

;BTFSC NU_MATNING,2 ; - - 

;INCF 

MV3,f 

BTFSC NU_MATNING,0 ;Läs ned till TEMP 

MOVF 

MV1,w 

BTFSC NU_MATNING,1 ; - - 

MOVF 

MV2,w 


MOVF 

MV3,w 

MOVWF TEMP ;Läs W till TEMP 

INCF TEMP,f ;Öka TEMP 

MOVLW 0x7f ;Ladda W med 0xff 

BTFSC TEMP,7 ;Är högsta biten satt 

MOVWF TEMP ;Skriv 0x7f till MV (alltså öka inte mer) 

MOVF 

TEMP,w 

BTFSC NU_MATNING,0 ;Läs upp till mätvärdet 

MOVWF 

MV1 


MOVWF 

MV2 


MOVWF 

MV3 

;BTFSC STATUS,Z ;Är zero biten satt 

;GOTO FULL 

;Räkna tillbaka detta värde 

RETURN 

KLAR 

BTFSS TID_START,0 ;Har vi inte ännu tagit emot en puls så är 

RETURN 

;vi inte klara ännu. 

;Vänta tills det kommer en puls 

BCF INTCON,T0IE ;Stäng av timern (mätningen klar) 

RETURN 

; ****************************************** 

; Subrutin Timer1 Avbrott 

; ****************************************** 

TIMER1_AVB 

BTFSS MATNING_PA,0 ;Är matningen på eller avslagen 

GOTO MATNING_AR_AV ;Är den av -> MATNING_AR_AV

CLRF TID_START ;nollställ tidmätningen 

;aktiveras av att pulsen kommer från sensorn 

BCF STATUS,C ;Rotera in nolla 

RLF NU_MATNING,f ;Nästa mätning (roter vänster en bit) 

BTFSC NU_MATNING,3 ;Har nu_matning gjort overflow (sensor 5) 

GOTO OF_MATN ;I så fall -> OF_MATN (=OverFlow_Matning) 

FORTS_MATN 

;Forts, NU_MATN och NU_SENSOR ska vara uppdaterade 

;till nästa mätning, nästa sensor 

; *** Läs av rätt sensor *** 

BTFSC NU_SENSOR,0 ;Sätt sensorn trigger-signal till hög 

BSF 

PORTB,0 

BTFSC 

NU_SENSOR,1 

BSF 

PORTB,1 

BTFSC 

NU_SENSOR,2 

BSF 

PORTB,2 

BTFSC 

NU_SENSOR,3 

BSF 

PORTB,3 

CALL WAIT_LOOP ;Vänta ett litet tag 

BCF 

BCF 

BCF 

BCF 

PORTB,0 

PORTB,1 

PORTB,2 

PORTB,3 

; *** starta timern *** 

CLRF TMR0 ;Nollställ interrupt räknaren 

BSF INTCON,T0IE ;Slå på timer 0 

MOVLW 0xf8 ;Interruptbitarna av 

ANDWF 

INTCON,f 

RETURN ;*** Klar!! Återgå till avbrottsrutin *** 

OF_MATN 

;Overflow mätning (subrutin utan CALL) 

;Mätning av nästa sensor ska startas 

CALL VALJ_VARDE ;Välj ut vilket värde som ska sparas 

;(och spara detta) 

MOVLW 0x01 ;Börja med nästa mätning 

MOVWF NU_MATNING ;Mätning 1 

BCF STATUS,C ;Rotera in nolla 

RLF NU_SENSOR,f ;Nästa sensor 

CLRF MV1 ;Nollställ mätvärdet 

CLRF 

MV2 

CLRF 

MV3 

MOVLW 

0xff 

BTFSC NU_SENSOR,4 ;Har sensor gjort overflow 

MOVWF OK_MATV ;Ett OK mätvärde ligger i minnet 


CALL 

UPPD_UTV 

MOVLW 

0x01 


MOVWF NU_SENSOR ;Sätt sensor till sensor 1 

GOTO FORTS_MATN ;Fortsätt (NU_MATN, NU_SENSOR uppdaterade) 

WAIT_LOOP 

;Gör en torrloop i ett par operationer 

;(min 10us) 

MOVLW 0x30 ;i 0x30 x 2 operationer 

MOVWF 

COUNT 

LOOP0 decfsz COUNT,1 ; - - 

goto LOOP0 

MOVLW 0xfc ;Starta med högt värde i räknar reg. 

BTFSC 

DEBUG,0

MOVWF 

TMR1H 

RETURN 

MATNING_AR_AV ; *+*+*+* Mätningen är av, ingen mätning ska göras *+*+*+* 

;endast uppdatering av utvärden och nollställ mätvärden 

CALL UPPD_UTV 

;Uppdatera utvärden på portarna 

CLRF OK_MATV ;Det gamla mätvärdet är inaktuellt 

MOVLW 0x01 ;Nollställ sensor och matning 

MOVWF NU_MATNING ;(förbered för ny mätning) 

MOVWF NU_SENSOR ; - - 

CLRF MV1 ; - - 

CLRF 

MV2 

CLRF 

MV3 

CLRF SENSOR1_MV ; - - 

CLRF 

SENSOR2_MV 

CLRF 

SENSOR3_MV 

CLRF 

SENSOR4_MV 

MOVLW 

0xfc 

BTFSC DEBUG,0 ;Snabba upp timern i DEBUG mode 

MOVWF 

TMR1H 

RETURN 

;Returnera till avbrottsrutinen... 

;Denna rutinen anropas med GOTO 

; ****************************************** 

; Välj Mätvärde (och spara undan det) 

; ****************************************** 

VALJ_VARDE 

MOVF MV1,w ; * Beräkna differensen mellan mätvärdena * 

MOVWF V_AB ;MV1->AB 

MOVF MV2,w ;MV2->w 

SUBWF V_AB,f ;AB(mv1) - w(mv2) -> V_AB 

BTFSC V_AB,7 ;Är talet negativt 

COMF V_AB,f ;Invertera talet (så det blir positivt) 

MOVF MV1,w ; - - (med AC) 

MOVWF V_AC ; - - 

MOVF 

MV3,w 

SUBWF 

V_AC,f 

BTFSC 

V_AC,7 

COMF 

V_AC,f 

MOVF MV2,w ; - - (med BC) 

MOVWF V_BC ; - - 

MOVF 

MV3,w 

SUBWF 

V_BC,f 

BTFSC 

V_BC,7 

COMF 

V_BC,f 

;V_AB, V_AC, V_BC inehåller 

;(den positiva) differensen mellan mätvärdena 

MOVF V_AB,w ;** Vilken av differenserna är minst ** 

SUBWF V_AC,w ;Börja med att jämföra AB och AC 

MOVWF TEMP ;Skriv (AC-AB) till TEMP 

BTFSS TEMP,7 ;Om negativt AB > AC 

GOTO AB_MINST_AV ;Alltså skippa om negativt 

MOVF V_BC,w ;Vilken är minst av AC, BC 

SUBWF V_AC,w ; - - 

MOVWF TEMP ;TEMP = (AC-BC) 

BTFSS TEMP,7 ;Om negativt (BC > AC) 

GOTO BC_MINST ;Inte negativt BC är minst 

GOTO AC_MINST ;Negativt BC är störst -> AC är minst 

AB_MINST_AV 

; Differensen AB är minst (av AB och AC) 

MOVF V_BC,w ;Vilken är minst av AB, BC 

SUBWF V_AB,w ; - - 

MOVWF TEMP ;TEMP = (AB-BC) 

BTFSS TEMP,7 ;Om negativt (BC > AB) 

GOTO BC_MINST ;Inte negativt BC är minst 

GOTO AB_MINST ;Negativt BC är störst -> AB är minst 

; *** Beräkna medel ***

AB_MINST 

MOVF MV1,w ;mv1 -> w 

ADDWF MV2,w ;mv1 + mv2 -> w 

MOVWF 

TEMP 

BCF 

STATUS,C 

RRF TEMP,f ;Dividera talet med 2 (heltalsdivision) 

GOTO 

FORTS_VALJ0 

AC_MINST 

MOVF MV1,w ;mv1 -> w 

ADDWF MV3,w ;mv1 + mv3 -> w 

MOVWF 

TEMP 

BCF 

STATUS,C 


GOTO 

FORTS_VALJ0 

BC_MINST 

MOVF MV2,w ;MV2 -> w 

ADDWF MV3,w ;MV2 + MV3 -> w 

MOVWF 

TEMP 

BCF 

STATUS,C 


GOTO 

FORTS_VALJ0 

FORTS_VALJ0 ; *** Forts *** 

;Skriv TEMP till rätt sensorvärde 

MOVF TEMP,w ;Läs mätvärdet till W 

BTFSC NU_SENSOR,0 ;Skriv värdet till rätt adress 

MOVWF 

SENSOR1_MV 

BTFSC 

MOVWF 

BTFSC 

MOVWF 

BTFSC 

MOVWF 

NU_SENSOR,1 

SENSOR2_MV 

NU_SENSOR,2 

SENSOR3_MV 

NU_SENSOR,3 

SENSOR4_MV 

RETURN 

;Klar - Valt och sparat värdet 

; ****************************************** 

; Huvudprogram 

; ****************************************** 

MAIN 

MOVLW 0x00 ;DEBUG på eller av 0x00 av, 0xff på 

MOVWF 

DEBUG 

;---------------------- 

;Initiera portarna 

;Port A - Output (signal till styrenhet) 

;RA5 Strobe 

;RA4 Signaltyp 

;RA0-3 Signal 

;Port C - Input (signal från styrenhet) 

;RC4 Korridor (A1, korsning) 

;RC5 Korsning (A2, korridor) 

;Port B - Input/Output (signal till/från sensorer) 

;RB4-7 Input (mätvärde från sensorerna, echo-out) 

;RB0-3 Output (trigger-signal till sensorerna) 

BCF STATUS,5 ;Byt till bank 0 


CLRF 

PORTA 

CLRF PORTB ;sätt utvärden till 0x0 

CLRF 

PORTC 

BSF STATUS,5 ;Byt till bank 1 


MOVLW 0x00 ;Ladda W med 0x00

MOVWF TRISA ;Skriv 0x00 till TRISA 

;Port A output 

MOVLW 0xF0 ;Ladda W med 0xF0 

MOVWF TRISB ;Skriv 0xF0 till TRISB 

;Port B input/output 

MOVLW 0xFF ;Ladda W med 0xFF 

MOVWF TRISC ;Skriv 0xFF till TRISC 

;Port C input (tristate) 

BCF STATUS, RP1 ;Initiera port A som digital input 

BSF STATUS, RP0 ;Bank 1 

MOVLW 0x06 

MOVWF ADCON1 

;Configure all pins 

;as digital inputs 

;---------------------- 

;Initiera timer 0 


BCF 

STATUS,6 

CLRF TMR0 ;Nollställ timerns räknarregister 


BSF INTCON,GIE ;GIE på 

BCF INTCON,T0IE ;T0IE av, sätts på av T1 

BCF INTCON,T0IF ;Avbrottsflagga av 

MOVLW 0xf8 ;Interruptbitarna av 

ANDWF 

INTCON,f 

MOVLW 0x48 ;Ingen prescaler... 

MOVWF 

OPTION_REG 


;---------------------- 

;Initiera Timer 1 



CLRF TMR1L ;Nollställ avbrottsräknaren 

CLRF TMR1H ; - - 

;DEBUG - Snabba upp TMR1 lite så det går att debugga... 

MOVLW 0xfc ;Starta med högt värde i räknar reg. 

BTFSC 

DEBUG,0 

MOVWF 

TMR1H 

BSF INTCON,6 ;Periperal Interrupt Enable på 

BCF PIR1,0 ;Avbrottsflaggan av 

(för säkerhets skull) 

MOVLW 0x15 ;OSC av, inte ext sync, internal clock 

MOVWF T1CON ;delning med 1:2!! + Timer ON: 



BSF PIE1,0 ;Timer1 Interrupt enable 



;---------------------- 

;Initiera input (B4-B7) 

;Stäng av interupt från port 

BCF 

INTCON,RBIE 

BCF INTCON,INTE ;Inga interrupts från RB0/INT 

;BSF INTCON,GIE ;Antas redan vara satt, 

;behöver inte sättas 


BCF STATUS,6 ; - -

BCF OPTION_REG,INTEDG ;Trigga interrupt på positiv flank 



;---------------------- 

;Övrigt 

CLRF MV1 ;Nollställ mätvärdena 



CLRF OK_MATV ;Inget OK mätvärde ännu... 

MOVLW 

0x01 

MOVWF NU_SENSOR ;aktuell sensor 1 

MOVWF NU_MATNING ;aktuell mätning 1 

MOVLW 

0x00 

MOVWF SENSOR1_SLAP ;0 till släpvärdet (korrigering) 

MOVWF 

SENSOR2_SLAP 

MOVLW 

0x00 

MOVWF SENSOR1_SLAP1 ;0 till släpvärdet (korrigering) 

MOVWF 

SENSOR2_SLAP1 

MOVLW 

0x00 


MOVWF 

SENSOR2_SLAP2 

MOVLW 

0x00 


MOVWF 

SENSOR2_SLAP3 

MOVLW 0x7f ;ettställ minvärdet 

MOVWF Hmin ;från början 

MOVWF 

Vmin 

CLRF 

KORSNING_ALLERT 

CLRF MATNING_PA ;Mätningen av från början.... 

;DEBUG - slå på mätningen 

MOVLW 

BTFSC 

MOVWF 

0xff 

DEBUG,0 

MATNING_PA 

STOP NOP ;Stanna och vänta på avbrott... 

NOP ; - - 

NOP 

NOP 

NOP 

NOP 

NOP 

NOP 

NOP 

NOP 

NOP 

GOTO STOP 

; ****************************************** 

; Uppdatera utvärden 

; ****************************************** 

UPPD_UTV 

;Denna funktionen ska köras när (1) styrenheten ändrar invärde 

;eller (2) när sensor-mätvärdet är klart eller (3) 

;vid timerinterrupt (tmr1) och MATNING_PA är av. 

;(0x00) I fall att vi skulle råka missa 

;positiv flank... (annat avbrott) 



BTFSC PORTC,4 ;Eller A2 på -> A2 (korsning) 

GOTO 

A2 

;Eller ingen: 

BTFSC PORTC,5 ;Vilket invärde har vi 

GOTO A1 ;Är A1 på -> A1 (korridor) 

CLRF PORTA ;Ingen utsignal 

CLRF MATNING_PA ;Ingen mätning då ingen pinna är på 

CLRF OK_MATV ;Mätvärdet inaktuellt (gammalt)

RETURN 

;Från UPPD_UTV!! 

A1 ; *** A1 hög, vi befinner oss i en korsning *** 

BTFSS OK_MATV,0 ;Finns det ett OK mätvärde 

GOTO EJ_MATV ;om inte -> EJ_MATV 

;1. Bestäm vilka sensorer som är aktiva 

CLRF 

MAT_BYTE 

MOVLW 0x2d ;Ladda W med 0x2d = 45 =~ 50cm 

SUBWF SENSOR3_MV,w ;Ligger mätvärdet under fram(), 

;sätt biten till 1 

MOVWF TEMP ;(0x2d-w) = TEMP 

BTFSC TEMP,7 ;Om TEMP negativt, MV över 

BSF MAT_BYTE,2 ;Skippa ej negativt (sign-biten är satt) 

MOVLW 0x28 ;Ladda W med 0x14 = 40 =~ 45cm 

SUBWF SENSOR1_MV,w ;Ligger mätvärdet under, sätt biten till 1 

MOVWF TEMP ;(0x14-w) = TEMP 

BTFSC TEMP,7 ;Ar TEMP negativt, mätvärdet större 

BSF MAT_BYTE,0 ;allts[ skippa om negativt 

MOVLW 0x28 ; - - 

SUBWF SENSOR2_MV,w ; - - 

MOVWF 

TEMP 

BTFSC 

TEMP,7 

BSF 

MAT_BYTE,1 

;2. Bestäm vilket värde den sensoruppställningen ger 

;Denna koden för återvändsgränd skulle kunna tas bort... 

MOVLW 0x7 ; Återvändsgränd 

SUBWF MAT_BYTE,w ; - - 

BTFSC STATUS,Z ; - - 

MOVLW 

SIGN_ATERV 


MOVWF 

PORTA 

MOVLW 0x5 ; Vänsterkorsning 



MOVLW 

SIGN_VANSTER 


MOVWF 

PORTA 

MOVLW 0x6 ; Högerkorsning 



MOVLW 

SIGN_HOGER 


MOVWF 

PORTA 

MOVLW 0x4 ; T-Korsning 



MOVLW 

SIGN_T 


MOVWF 

PORTA 

MOVLW 0x1 ; Fram & Vänster 



MOVLW 

SIGN_FV 


MOVWF 

PORTA 

MOVLW 0x2 ; Fram & Höger 



MOVLW 

SIGN_FH 


MOVWF 

PORTA 

MOVLW 0x0 ; Fyrvägskorsning 



MOVLW 

SIGN_FYRV 


MOVWF 

PORTA 

MOVLW 0x3 ; Mätfel... 

SUBWF MAT_BYTE,w ; - -


GOTO 

SKRIV_FEL 

GOTO FORTS_KORSN ;Hoppa över skrivfel 

SKRIV_FEL 

MOVLW SIGN_FEL ;Annan korsning, skriv fel... 

MOVWF 

PORTA 

FORTS_KORSN 

BCF PORTA,5 ;Utsignalen är av typ korsning 


RETURN 

;Klar - Signal på utporten 

A2 ; *** A2 hög, vi befinner oss i en korridor *** 

BTFSS OK_MATV,0 ;Finns det ett OK matvärde 

GOTO EJ_MATV ;om inte -> EJ_MATV 

;Högerkorsning 

MOVLW 0x4b ;Ladda W med 0x4b = 75 =~ 80cm 

SUBWF SENSOR1_MV,w ;Ligger mätvärdet över, 

;sätt utvärdet till korsning 

MOVWF TEMP ;0x16 minus mv = temp 

BTFSS TEMP,7 ; - - 

GOTO 

KORSN_UT 

;Vänsterkorsning 

MOVLW 0x4b ; - - 


MOVWF TEMP ; - - 

BTFSS TEMP,7 ; - - 

GOTO 

KORSN_UT 

CLRF 

KORSNING_ALLERT ;Ingen korsning den här gången, 

;-> false alarm 

;Återvändsgränd (lägst prioritet = ej länge) 

MOVLW 0x0d ;Ladda W med 0x08 = 8 =~ 10cm 

SUBWF SENSOR3_MV,w ;Ligger mätvärdet under, 


MOVWF TEMP ;0x19 minus mv = temp 

BTFSC TEMP,7 ; - - 

GOTO 

KORSN_AT_UT 

;Hoppa till INGEN_KORR om värdet ökar, 

;i så fall ska ingen korrigering göras 

;DEN NYA KODEN... 

testa 

MOVLW 0x09 ;a till w ;ob förut 

SUBWF SENSOR2_MV,w ;sensor2-a 

MOVWF 

TEMP 

BTFSC 

TEMP,7 

GOTO KORR_H ;sensor2 - a0, alltså allt är OK 

SUBWF SENSOR1_MV,w ;sensor1-a istället för W 

MOVWF 

TEMP 

BTFSC 

TEMP,7 

GOTO KORR_V ;sensor1 - a

;Undersök om mätvärdet ligger under minvärdet 

MOVF TEMP,w ;Ladda W med minvärdet 

SUBWF SENSOR2_MV,w ;Substrahera.. ;Kommentarer fel igen... 

MOVWF TEMP ;(min - mv) -> TEMP 

BTFSS TEMP,7 ;Om TEMP negativt, ingen korrigering 

GOTO INGEN_KORR ;Om negativt, hoppa inte över denna 

MOVF SENSOR2_MV,w ;Sätt nytt minvärde 

MOVWF 

Vmin 

GOTO FORTS_NICKLAS ;Korrigera... 

S2_STORST 

MOVLW 0x7f ;Aktivera korrigering vänster (igen) 

MOVWF 

Vmin 

MOVF Hmin,w ;Minvärdet till TEMP 

MOVWF 

TEMP 

MOVLW 0x01 ;Lägg till 2, måste vara lite under.... 

SUBWF 

TEMP,f 

MOVLW 0x00 ;Ej TEMP 

BTFSS TEMP,7 ;Om TEMP negativt, ingen korrigering 

GOTO INGEN_KORR ;Om negativt, hoppa inte över denna 

MOVF SENSOR1_MV,w ;Sätt nytt minvärde 

MOVWF 

Hmin 

GOTO FORTS_NICKLAS ;Korrigera... 

FORTS_NICKLAS 

;Korrigering vänster 

MOVLW 0x14 ;Ladda W med 0x0a = 12 =~ 16cm: 

;0x0c. Tidigare 15 

SUBWF SENSOR1_MV,w ;Ligger mätvärdet under, 


MOVWF TEMP ;0x0a minus mv = temp 


GOTO 

KORR_V 

;Korrigering höger 

MOVLW 0x14 ; - - 


MOVWF TEMP ; - - 


GOTO 

KORR_H 

INGEN_KORR 

;Hoppa hit om det inte behövs någon korrigering 

;Här ska återvändsgränd stå om den ska ha lägre prioritet än 

;korrigering 

MOVLW 

0xe0 

ANDWF PORTA,f ;Ingen korsning, maska ut de 

;4 sista bitarna 

GOTO FORTS_KORR 

KORSN_UT 

;Vi är i en korsning, 

;skicka det till styrenheten 

BTFSS KORSNING_ALLERT,0 ;Om ingen tidigare varning, 

;ge varning 

GOTO 

VARNING_KORS 

CLRF 

KORSNING_ALLERT ;Ta bort varningen till nästa gång... 

MOVLW 0xef ;Skriv korsning 

MOVWF 

PORTA 

GOTO FORTS_KORR

VARNING_KORS 

MOVLW 

0xff 

;Ett ställ korsnings alert, ge en ;varning, korsning igen 

MOVWF 

KORSNING_ALLERT 

; -> skicka signal 

GOTO 

FORTS_KORR 

KORSN_AT_UT 

;Återvändsgränd 

;MOVLW 0x7f ;ettställ min värdet 

;MOVWF Hmin ;aktivera korrigering igen... 

;MOVWF 

Vmin 

MOVLW 0xee ;skriv återvändsgränd 

MOVWF 

PORTA 


KORR_V 

;MOVWL 0xe1 ;Maska ut den gamla signalen 

;ANDWF 

PORTA,f 

MOVLW 0xe1 ;Skriv korrigera vänster 

MOVWF 

PORTA 

;IORWF 

PORTA,f 


KORR_H 

;MOVWL 0xe2 ;Maska ut den gamla signalen 

;ANDWF 

PORTA,f 

MOVLW 0xe2 ;Skriv korrigera höger 

;(utan att cleara signaltyp) 

;IORWF 

PORTA,f 

MOVWF 

PORTA 


FORTS_KORR 

;CALL 

MIN_FIX korrigera om vi är i mitten och på väg åt något håll 

;Lagra undan tidigare värden 

MOVF 

MOVWF 

MOVF 

MOVWF 

MOVF 

MOVWF 

MOVF 

MOVWF 

MOVF 

MOVWF 

MOVF 

MOVWF 

MOVF 

MOVWF 

MOVF 

MOVWF 

MOVF 

MOVWF 

MOVF 

MOVWF 

MOVF 

MOVWF 

MOVF 

MOVWF 

MOVF 

MOVWF 

MOVF 

MOVWF 

MOVF 

MOVWF 

MOVF 

SENSOR1_SLAP8,w ;S8 -> S9 

SENSOR1_SLAP9 

SENSOR2_SLAP8,w 

SENSOR2_SLAP9 


SENSOR1_SLAP8 


SENSOR2_SLAP8 


SENSOR1_SLAP7 


SENSOR2_SLAP7 


SENSOR1_SLAP6 


SENSOR2_SLAP6 


SENSOR1_SLAP5 


SENSOR2_SLAP5 


SENSOR1_SLAP4 


SENSOR2_SLAP4 


SENSOR1_SLAP3 


SENSOR2_SLAP3 


SENSOR1_SLAP2 

SENSOR2_SLAP1,w

MOVWF 

MOVF 

MOVWF 

MOVF 

MOVWF 

SENSOR2_SLAP2 

SENSOR1_SLAP,w ;S0 -> S1 

SENSOR1_SLAP1 

SENSOR2_SLAP,w 

SENSOR2_SLAP1 

MOVF SENSOR1_MV,w ; MV -> 0 

MOVWF SENSOR1_SLAP ; - - 

MOVF SENSOR2_MV,w ; - - 

MOVWF 

SENSOR2_SLAP 

; ----- ÄNDA HIT ----- 

BSF 

CALL 

CALL 

CALL 

CALL 

BCF 

PORTA,4 

WAIT_LOOP 

WAIT_LOOP 

WAIT_LOOP 

WAIT_LOOP 

PORTA,4 

BSF PORTA,5 ;Utsignalen är av typ korrigering 


RETURN 

;Från UPPD_UTV!! 

EJ_MATV ;* Körs om det inte ännu ett ok mätvärde i minnet * 

MOVLW 0xff ;Slå på mätningen 

MOVWF 

MATNING_PA 

MOVLW 0x7f ;ettställ min värdet 

MOVWF Hmin ;aktivera korrigering igen... 

MOVWF Vmin ;Det här är original nollställningen... 

;då A2 (a1) varit låg 

CLRF PORTA ;Inget utvärde till styrenheten (ännu) 


RETURN 

;Returnera från UPPD_UTV 

; ****************************************** 

; Subrutiner för min fix-algoritm 

; ****************************************** 

MIN_FIX 

;Beräkna diffen 

MOVF 

SENSOR1_MV,w 

MOVWF 

dMV 

MOVF 

SENSOR2_MV,w 

SUBWF 

dMV,f 

BTFSC dMV,7 ;Invertera om talet är negativt 

COMF 

dMV,f 

MOVLW 

0x10 

MOVWF TEMP ;16 -> temp 

MOVF dMV,w ;dMV -> w 

SUBWF TEMP,f ;TEMP (10) - dMV 

BTFSC TEMP,7 ;Om negativt 

GOTO FIX_FIX ;om inte, få til fix-fix 

RETURN 

FIX_FIX ;Avståndet mellan mv1 och mv2 är mindre än 16 

;alltså roboten är i mitten 

MOVF 

SENSOR1_SLAP9,w ;Beräkna diffen mellan mv och släp 

MOVWF 

TEMP 

MOVF 

SENSOR1_MV,w 

SUBWF TEMP,f ;diffen till temp 

BTFSC TEMP,7 ;Invertera om talet är negativt 

COMF 

TEMP,f 

MOVLW 0x0a ;10 -> w 

SUBWF TEMP,f ;TEMP - 10

BTFSS TEMP,7 ;kolla om negativt, 

RETURN 

;om så fallet, return, 

;differensen ligger under 10 

MOVF 


MOVWF TEMP ;(SLAP9-NEW) = TEMP 

MOVF 

SENSOR1_MV,w 

SUBWF 

TEMP,f 

BTFSC TEMP,7 ;Om TEMP negativt, MV över 

GOTO FIX_KORR_V ;om negativt, fortsätt, annars 

GOTO 

FIX_KORR_H 

FIX_KORR_V 

MOVLW 0xe1 ;Skriv korrigering på porten 

MOVWF 

PORTA 

RETURN 

FIX_KORR_H 

MOVLW 0xe2 ;Skriv korrigering på porten 

MOVWF 

PORTA 

RETURN 

; ****************************************** 

; Slut på koden 

; ****************************************** 

END

Teknisk dokumentation

You also want an ePaper? Increase the reach of your titles

Delete template?

Save as template?