ROBOTICS CCFZ 27/3/2010
Totaalplaatje robotbesturing De ingrediënten voor het programmeren van de racerobot hebben we nu behandeld. We kunnen een PICprocessor programmeren. Het aansturen van een motor (gebruikmakend van de Hbrug) hebben we behandeld en we kunnen de sensors aansturen en uitlezen. Alles komt nu bij elkaar om de racerobot te programmeren. Twee stuursystemen kunnen we onderscheiden. Die vragen elk een apart programma. Het eerste systeem heeft twee apart aangedreven wielen en een soort zwenkwiel om het evenwicht te verkrijgen. Door het ene aangedreven wiel een andere snelheid te geven dan het andere wordt een bocht genomen. Draaien de wielen even snel rond, dan gaat de robot recht voor of achteruit. De BoEbot heeft dit systeem, dat voor veel robots populair is. Hieronder een foto van een dergelijke robot: de BeObot, gepimpt en wel, tijdens de Robotrace 2007.
Het andere systeem werkt met een motor op het ene wielpaar, en een andere motor, die de wielen laat zwenken. Een stuurmechanisme zoals voor auto’s plaatsvindt. De eerste motor kan een vaste as aandrijven, dan wel met een differentieel werken. Dat laatste is natuurlijk veel mooier. Zelf heel moeilijk zelf te maken, maar wel beschikbaar bij bijvoorbeeld niet meer functionerende op afstand bestuurbare auto’s.
www.ccfz.nl 42
ROBOTICS CCFZ 27/3/2010
Hierboven is er een foto van gemaakt. Haal je de carrosserie weg, dan blijft er een racechassis over. Beide systemen zullen we beschrijven. Opvalt, dat voor beide systemen telkens twee motors nodig zijn! Voor beide gevallen zullen we in deze beschrijving uitgaan van gelijkstroommotors, aangestuurd door een Hbrug. Met twee motors kunnen we onze Rommtech CCFZHbrug gebruiken. Beide systemen hebben als sensors links en rechts de IRdetector, die tot op 60 cm vooruit het terrein verkennen. Twee apart aangedreven wielen Het eerst, dat we moeten doen om structuur te maken, om het overzicht te behouden is een stroomdiagram maken, want het wordt wel complex en omvangrijk. We kunnen meer pagina’s code verwachten en daardoor kunnen we snel het overzicht kwijtraken. Het geknoei op het kladblaadje bespaar ik de lezer maar.
www.ccfz.nl 43
ROBOTICS CCFZ 27/3/2010
start
beide motors vooruit
B obstakel links ?
nee
ja set bitlinks ol
obstakel rechts ? ja set bitrechts or
A
www.ccfz.nl 44
nee
ROBOTICS CCFZ 27/3/2010
A
ja ol&or = 1? nee
B
P ja ol of or = 0? nee ja or = 1? nee
L
R
Ik begin met de motors beide op volle snelheid te laten draaien, want ik neem aan, dat de robot bij de start meteen in de goede richting is gezet. Een vrije baan van 10 à 15 cm is dan nog aanwezig. Terwijl de robot deze afstand aflegt moet de processor snel de beide sensors voor links en rechts aansturen en uitlezen om te beoordelen of de baan vrij van obstakels is. Wordt een obstakel gedetecteerd, dan wordt een vlag geplaatst in een daartoe gereserveerd deel van het geheugen. Is geen vlag geplaatst, dan hoeven we niets aan de motoraandrijvingen te doen: met volle kracht vooruit blijven rijden: koppeling B verwijst naar het begin om opnieuw te testen. Is er een obstakel gedetecteerd, dan moet de rijrichting af gaan wijken en wel in de richting, afhankelijk van de plaats waar het obstakel zich bevindt. Signaleert de linker sensor een obstakel (vlag ol wordt dan geset), dan moet de rijrichting naar rechts afwijken (R van naar rechts afbuigen) en omgekeerd, als er rechts een obstakel wordt gedetecteerd (vlag or setten), dan moet de robot naar links afbuigen (subroutine L). Dat zal duidelijk zijn. Het kan ook voorkomen, dat beide sensors tegelijk een obstakel detecteren (or en ol zijn tegelijk geset en subroutine P van panieksituatie wordt dan aangestuurd). Ik kies ervoor om dan de robot ca. 10 cm terug te laten rijden en daarna te kijken of er een bepaalde richting gekozen kan worden (één der sensors ‘ziet’ het obstakel, dat kan worden ontweken). Achtereenvolgend wordt gekeken: www.ccfz.nl 45
ROBOTICS CCFZ 27/3/2010
Zijn beide vlaggen geset: ga naar subroutine paniek. Is geen der vlaggen geset: begin opnieuw met rechtdoor rijden Is vlag 1 geset van obstakel rechts geset, dan naar subroutine L voor naar links afbuigen. Als dat laatste niet het geval is, dan moet vlag 2 geset zijn en dient subroutine R voor naar rechts afbuigen te worden bereikt. Voor de drie subroutines L, R en P moet een apart stroomschema worden opgesteld, die eindigt met een verwijzing naar B, begin, om opnieuw de meting uit te voeren. De Proutine kan er als volgt uitzien. P
B
rij 10 cm achteruit
De routines voor het naar links en naar rechts afbuigen lijken natuurlijk sterk op elkaar. Ik zal die voor afbuigen naar rechts weergeven. De andere kun je zelf bedenken. Voor later is dat natuurlijk wel gemakkelijk. Je kunt de code voor de een maken en kopiëren voor de andere. Alleen de data voor rechtss moet je dan veranderen voor die van links.
R
B
Buig af naar rechts
En dit moeten we dus programmeren! Waar in het stroomschema een rechthoek staat (dat geeft een zogenaamd proces aan) kan ook weer een ingewikkelde situatie aanwezig zijn. Aparte, in detail tredende stroomschema’s kunnen ook dan vaak een vereenvoudiging van het programmeren betekenen. Zullen we verderop zeker tegen komen!
www.ccfz.nl 46
ROBOTICS CCFZ 27/3/2010
Eerst de hardware. Naast de ‘huishoudelijke aansluitingen’ zoals de spannings aansluitingen, de reset en de kristalaansluitingen zijn er de specifieke aansluitingen voor de sensors en de motor. In de tabel worden ze weergegeven. Poort ra is als ingang gebruikt, poort rb als uitgang. Er zijn zes uitgangen nodig, 2 ingangen.
PICaansluiting rb0 rb1 rb2 rb3 rb4 rb5 ra0 ra1
Functie motor links richting motor links snelheid motor rechts richting motor rechts snelheid IRLED links IRLED rechts IRdetector links IRdetector rechts
Het totale schema van de aansluitingen van de PIC16F82A voor de sensors en de H brugzijn hieronder getekend. De aansluitingen zijn alleen als naam genoemd. Dit schema is de aanvulling op het schema op bladzijde 3 van deze handleiding.
Alles is nu bekend om aan het programmeerwerk te beginnen. Het belangrijkste zal zijn om ingeval beide IRdetectors een obstakel vinden, dat dan een goede, snelle reactie volgt. Beide IR’ogen’ geven een signaal en dat moet snel www.ccfz.nl 47
ROBOTICS CCFZ 27/3/2010
worden verwerkt. Laten we daarom maar beginnen met dat stukje programma te ontwikkelen, die routine te maken. We reserveren een geheugenplaats, die we IRvlaggen noemen. Het 0de (nulde) en het eerste bit daarvan gaan we gebruiken om ingeval van een obstakel te seten als vlaggetje, waar we rekening mee moeten gaan houden. Vandaar de naam van deze routine: Vlaggentest. Het resultaat van ons programmeren volgen we in het Special Function register en het Fileregister. Die moeten we zeker nu dus gereed hebben staan. We stappen door de routine heen met de F6 en de F7toetsen, ook als gebruikelijk dus. Na het standaardbegin wordt het interessant. ; ; ; Vlaggentest ; ; Juni 2008 ; Hans Dorst ; ; list p=16f84 errorlevel 302 __config 3ffd porta portb trisa trisb
equ equ equ equ
0x05 0x06 0x85 0x86
rp0 status temp loop1 loop2 IRvlaggen
equ equ equ equ equ equ
5 0x03 0x34 0x30 0x31 0x40
bsf movlw movwf bcf
status,rp0 0xff trisa status,rp0
bsf movlw movwf bcf
status,rp0 0x00 trisb status,rp0
bsf bsf
IRvlaggen,0 IRvlaggen,1
movlw subwf
0x03 IRvlaggen,0
btfss goto nop goto
status,2 paniek
goto
start
; van poort a alles als ingang instellen
; van poort b alles als uitgang instellen
; hierboven dus de standaardinstellingen. ; +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ start clrf IRvlaggen ; alle vlaggen op 0 zetten
paniek
; linker kant obstakel ; rechter kant obstakel
start
www.ccfz.nl 48
ROBOTICS CCFZ 27/3/2010
end
We beginnen met de door ons gemaakte ruimte van de vlaggen schoon te maken: clrf IRvlaggen. Daarvan setten we in de twee volgende regels zowel bit 0 als bit 1. Na doorstappen zine we in het File register geheugenruimte 40 hierna op 03 staan. Klopt: de Hex waarde van binair 11 is 03. Nu plaatsen we de waarde 0x03 in het Wregister, WREG. Door subwf trekken we de waarde uit het werkregister WREG af van de waarde in het IRvlaggengeheugen. Als beide bits geset zijn, dan komt dus 0, nul als resultaat in het Statusregister op plaats Z, dat wil zeggen het bit 2, te staan. De rest van de routine bevat elementen, die we al eerder tegenkwamen: Kijk of de waarde van bit 2 in het statusregister geset is. Sla dan de volgende regel over en na een regel NOP (no operations) ga dan naar start om nog eens het geheel goed te kunnen nagaan. Maar als je één van de regels bsf verandert in bcf (dat wil zeggen dat bit nul maakt in plaats van een) dan wordt er door btfss geen regel overgeslagen en wordt de volgende regel uitgevoerd: paniek, want beide IRdetectors signalen tegelijk een obstakel. Al met al een korte, belangrijke routine, die we goed kunnen gebruiken. ; ; ; Racerobotbesturing ; ; Juli 2008 ; Hans Dorst en Joop van Schaik ; ; voor ; Robotics CCFZ ; list p=16f84 errorlevel 302 __config 3ffd porta portb trisa trisb
equ equ equ equ
0x05 0x06 0x85 0x86
rp0 status temp loop1 loop2 loop3 loop4 IRvlaggen
equ equ equ equ equ equ equ equ
5 0x03 0x34 0x30 0x31 0x32 0x33
bsf movlw movwf bcf
status,rp0 0xff trisa statusr,rp0
bsf movlw movwf bcf
status,rp0 0x00 trisb status,rp0
0x40
; van poort a alles als ingang instellen
; van poort b alles als uitgang instellen
; hierboven dus de standaardinstellingen. +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
www.ccfz.nl 49
ROBOTICS CCFZ 27/3/2010
end
Dit is het bekende begin. Een viertal registerplaatsen zijn gereserveerd voor verschillende loops, en een voor de vlaggen van de IRafstandsmeting. De racerobot zal vanaf het begin op volle snelheid rechtdoor moeten rijden. Dat verdient een aparte routine, die we vanaf het begin aanroepen: call
rechtdoor
Achteraan het programma zetten we deze routine, waarbij we ons herinneren hoe de Hbrug is aangesloten: rb0: motor links, draairichting rb1: motor links, snelheid rb2: motor rechts, draairichting rb3: motor rechts, snelheid. En dit allemaal zoals voorheen beschreven. We nemen aan, dat de motors zo zijn aangesloten, dat als ze zo draaien, dat als er een 1 op de draairichting aan de Hbrug wordt aangeboden, de robot vooruit gaat. Eventueel kunnen de aansluitdraden van de gelijkstroommotor worden omgewisseld om dit te bereiken. Volle vaart vooruit betekent dus aan alle vier de rbaansluitingen een 1. Volgens bladzijde 6 is de hexadecimale waarde daarvoor f. De routine is dan ook: ; rechtdoor movlw 0x0f ; maak de vier rbpoorten alle 1 movwf portb
rdaftellen
movlw movwf
0xff loop1
; <====== deze in de praktijk fijnregelen ; gedurende deze loop wordt rechtdoor gereden
decfsz loop1 goto rdaftellen goto start return ;
Te verwachten valt, dat om een rustige rit te verkrijgen de robot enige tijd ongestoord verder kan. Binnen deze afgelegde afstand wordt er verder niets gedaan. Achteruit rijden gebeurt door rb0 en rb2 0, nul te maken. De routine daarvoor is: ; achteruit movlw 0x0a ; maak poorten 1 en 3 gelijk 1 en 0 en 2 gelijk 0 mowf portb
auaftellen
movlw movwf
0xff loop1
decfsz
loop1
; <====== deze in de praktijk fijnregelen ; gedurende deze loop wordt achteruit gereden
www.ccfz.nl 50
ROBOTICS CCFZ 27/3/2010
goto goto return
auaftellenrd start
return ;
Ook hier eerst even een bepaalde afstand afleggen. De grootte van de afgelegde afstanden wordt bepaald door de lus. In de praktijk kunen de waarden voor nu ff worden ingesteld. Deze hebben we ook meteen maar klaar. Voor 0x0a zijn volgens de tabel op bladzij 6 de juiste poorten 0 en 1. Bochten maken moeten we natuurlijk ook. Bocht rechts, wil zeggen, maak een bocht naar rechts, buiglinks is maak een bocht naar links. De routines” buigrechts movlw 0xff ; <====== deze in de praktijk fijnregelen movwf loop1 braftellen movlw 0x0a ; even rechtdoor movwf portb movlw 0x0d ; rechter wiel stopt even, poort rb1 even 0 movwf portb decfsz loop1,f goto braftellen goto start ; en tenslotte buiglinks blaftellen
movlw movwf
0xff loop1
; <====== deze in de praktijk fijnregelen
movlw 0x0a ; even rechtdoor movwf portb movlw 0x07 ; rechter wiel stopt even, poort rb1 even 0 movwf portb decfsz loop1,f goto blaftellen goto start ;
Bij deze laatste twee routines zal de spanning op 50 % uitkomen. Een kortere bocht wordt bereikt als we duur van het stoppen verlengen, bijvoorbeeld door de regel movlw 0x07 meer keren herhalen. Een langere bocht ontstaat door de regel movlw 0x0a te herhalen. In de praktijk moet dat voor elke robot, met andere motors en met andere wielen apart worden uitgeregeld. Dus ook met de afstanden, die worden ingesteld voor al deze rijrichtingen. Nu de obstakeldetectie. Hieronder worden de routines voor zowel links als rechts gegeven. Voor de pulsduur en de pulspauze kan van dezelfde subroutine gebruik worden gemaakt. Dat betekent wel , dat de twee IRLEDlampjes en de twee IR detectors van dezelfde soort moeten zijn. Dat zal meestal het geval zijn. ; IRmetingenL startIRL ; voor de linkerkant call pulsuitL
www.ccfz.nl 51
ROBOTICS CCFZ 27/3/2010
call
obstakeltestL
goto
startIRL
return ; pulsuitL movlw 0x0a movwf loop2 tellerIRL decfsz goto return
loop2,f pulsvorm
; pulsvormL bsf portb,4 ; IRLEDlamp is aangesloten op rb4 bsf portb,4 call pulsduur bcf portb,4 call pulspauze goto tellerIRL ; obstakeltestL btfsc porta,0 ; controleer de uitgang van de IRdetector return ; als die 0 is, dan is er obstakel en regel bsf IRvlaggen,0 ; overslaan, anders terug gaan naar hoofdprogramma return ; IRmetingenR startIRR ; voor de rechterkant call pulsuitL call obstakeltestL goto
startIRR
return ; pulsuitR movlw 0x0a movwf loop2 tellerIRR decfsz goto return
loop2 pulsvorm
; pulsvormR bsf portb,5 ; IRLEDlamp is aangesloten op rb4 bsf portb,5 call pulsduur bcf portb,5 call pulspauze goto tellerIRR ; obstakeltestR btfsc porta,1 ; controleer de uitgang van de IRdetector return ; als die 0 is, dan is er obstakel en regel bsf IRvlaggen,1 ; overslaan, anders terug gaan naar hoofdprogramma return ; pulsduur movlw 0x02 movwf loop3 opn1 decfsz loop3,f goto opn1
www.ccfz.nl 52
ROBOTICS CCFZ 27/3/2010
return ; pulspauze movlw 0x02 movwf loop4 opn2 decfsz loop4,f goto opn2 return
Dat schiet aardig op. De resultaten van de IRmeting kunnen we nu gebruiken vanaf IRvlaggen, de eerste twee bitjes. We gebruiken daarvoor de routine van de vlaggentest en plaatsen die onderaan de programmacode. Het totaal komt er dan zo uit te zien: ; ; ; Racerobotbesturing ; ; Juli 2008 ; Hans Dorst en Joop van Schaik ; ; voor ; Robotics CCFZ ; list p=16f84 errorlevel 302 __config 3ffd porta portb trisa trisb
equ equ equ equ
0x05 0x06 0x85 0x86
rp0 status temp loop1 loop2 loop3 loop4 IRvlaggen
equ equ equ equ equ equ equ equ
5 0x03 0x34 0x30 0x31 0x32 0x33 0x40
bsf movlw movwf bcf
0xff trisa
bsf movlw movwf bcf
0x00 trisb
call
pulsuitL
status,rp0
; van poort a alles als ingang instellen
status,rp0 status,rp0
; van poort b alles als uitgang instellen
status,rp0
; hierboven dus de standaardinstellingen. ; ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ start call rechtdoor clrf Irvlaggen ; beide vlaggen op 0, nul zetten call IRmetingenL call IRmetingenR call paniek call bijsturen goto start ; IRmetingenL startIRL
; voor de linkerkant
www.ccfz.nl 53
ROBOTICS CCFZ 27/3/2010
call
obstakeltestL
return ; pulsuitL movlw 0x0a movwf loop2 tellerIRL
decfsz goto
loop2,f pulsvormL
return ; pulsvormL bsf portb,4 bsf portb,4 call pulsduur bcf portb,4 call pulspauze goto tellerIRL ; obstakeltestL btfsc porta,0 return bsf IRvlaggen,0 return ; IRmetingenR startIRR call pulsuitL call obstakeltestL goto
decfsz goto
return ; pulsduur movlw 0x02 movwf loop3 decfsz goto
; controleer de uitgang van de IRdetector ; als die 0 is, dan is er obstakel en regel ; overslaan, anders terug gaan naar hoofdprogramma ; bij obstakel links bit 0 van IRvlaggen setten
; voor de rechterkant hetzelfde
; tot a tellen
loop2,f pulsvormR
return ; pulsvormR bsf portb,5 bsf portb,5 call pulsduur bcf portb,5 call pulspauze goto tellerIRR ; obstakeltestR btfsc porta,1 return bsf IRvlaggen,1
opn1
; IRLEDlamp is aangesloten op rb4
startIRR
return ; pulsuitR movlw 0x0a movwf loop2 tellerIRR
; hier tot 11 (a) tellen voor 11 pulsen
; IRLEDlamp is aangesloten op rb4
; controleer de uitgang van de IRdetector ; als die 0 is, dan is er obstakel en regel ; overslaan, anders terug gaan naar hoofdprogramma ; hier wordt bit 1 geset ingeval obstakel rechts
loop3,f opn1
return ; pulspauze movlw 0x02 movwf loop4 opn2
decfsz
loop4,f
www.ccfz.nl 54
ROBOTICS CCFZ 27/3/2010
goto
opn2
return ; rechtdoor movlw 0x0f movwf portb
rdaftellen
movlw movwf
0xff loop1
decfsz goto goto
loop1,f rdaftellen start
; <====== deze in de praktijk fijnregelen ; gedurende deze loop wordt rechtdoor gereden
return ; achteruit movlw 0x0a movwf portb
auaftellen
movlw movwf
0xff loop1
decfsz goto nop goto
loop1,f auaftellen
; maak poorten rb1 en rb3 gelijk 1 enr 0 en rb2 gelijk 0 ; zie ook bladzij 6 voor binaire informatie ; <====== deze in de praktijk fijnregelen ; gedurende deze loop wordt achteruit gereden
start
return ; buigrechts movlw 0xff movwf loop1 braftellen movlw 0x0a movwf portb movlw 0x0d movwf portb decfsz loop1,f goto braftellen nop goto start ; buiglinks movlw 0xff movwf loop1 blaftellen movlw 0x0a movwf portb movlw 0x07 movwf portb decfsz loop1,f goto blaftellen goto start ; paniek movlw 0x03 subwf IRvlaggen,0 btfss status,2 goto bijsturen goto achteruit ; bijsturen movlw 0x00 subwf IRvlaggen,0 btfss status,2 goto afbuigen goto start afbuigen movlw
; maak de vier rbpoorten alle 1
; <====== deze in de praktijk fijnregelen ; even rechtdoor ; rechter wiel stopt even, poort rb1 even 0
; <====== deze in de praktijk fijnregelen ; even rechtdoor ; rechter wiel stopt even, poort rb1 even 0
; zet de waarde 03 in het werkgeheugen ; trek deze af van IRvlaggen ; als nul het resultaat is, dan een regel overslaan
;controleren of IRvlaggen wel een geset bitje hebben ; als dat niet het geval is opnieuw meten. ; is er wel een geset bitje, dan naar afbuigen
0x01
www.ccfz.nl 55
ROBOTICS CCFZ 27/3/2010
subwf IRvlaggen,0 btfss status,2 goto buigrechts goto buiglinks ; end
; na buiglinks en buigrechts wordt teruggegaan ; naar start om alles opnieuw in gang te zetten
Wat gebeurt er? Nou, als beide bitjes van IRvlaggen geset zijn, dan is de waarde van IRvlaggen dus 03. Die 03 trekken we af van de waarde in IRvlaggen en als er dan 0, nul uitkomt, dan hebben beide IRdetectors, zowel links als rechts een obstakel geconstateerd en moet de robot in de achteruit. Anders naar de volgende controle gaan. Daar checken we of er soms geen obstakel is gedetecteerd. Is dat het geval, dan weer van voren af aan beginnen. En anders controleren of er links, dan wel rechts iets te zien valt. En daar weer de juiste reactie op maken. Let wel even op. Dit is droogzwemmen!!! We zullen nu eerst de hardware van de racerobot moeten bouwen en daarna dit programma moeten uitoeren om ter kunnen nagaan of er echt geen vergissingen zijn gemaakt. Daarom gaan we daar nu eerst maar eens verder mee. Wat blijkt:: voor onze racerobot met twee aangedreven wielen (twee motors) werkt het. We moesten wel de IRdetector door veel te proberen gevoeliger maken. Voor een racerobot met normale stuurinrichting, ik noem dat maar met zwenkwielen, is het programma grotendeels hetzelfde: alleen na het setten van de vlaggen moet de aansturing van de Hbrug worden aangepast. Ik presenteer hieronder maar het hele programma. ; ; ; Racerobotbesturing Zwenkwielen ; ; Maart 2010 ; Hans Dorst en Joop van Schaik ; ; voor ; Robotics CCFZ ; list p=16f84A errorlevel 302 __config 3ffd porta portb trisa trisb option_reg
equ equ equ equ equ
0x05 0x06 0x85 0x86 0x81
rp0 equ 5 status equ 0x03 temp equ 0x34 loop1 equ 0x30 loop2 equ 0x31 loop3 equ 0x32 loop4 equ 0x33 IRvlaggen equ 0x40 ; hierboven de koptekst bsf status,rp0 movlw 0xff movwf trisa bcf status,rp0
; van poort a alles als ingang instellen
www.ccfz.nl 56
ROBOTICS CCFZ 27/3/2010
bsf movlw movwf bcf
status,rp0 0x00 trisb status,rp0
bcf bsf
portb,0 portb,1
; van poort b alles als uitgang instellen ; hierboven de standaardinstellingen. ; vooruit starten: ; poort b0 clear M1, ri vooruit ; poort b1 setten M1, snelheid
; ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ start clrf IRvlaggen bcf IRvlaggen,0 bcf IRvlaggen,1 ; call rechtdoor call IRmetingenL call IRmetingenR call paniek call bijsturen goto start ; IRmetingenL ; voor de linkerkant call pulsuitL call obstakeltestL return ; pulsuitL movlw 0x0f ; f pulsen aftellen movwf loop2 tellerIRL
decfsz goto
loop2,f pulsvormL
; na f pulsen volgende regel overslaan
return ; terug naar startIRL ; pulsvormL bsf portb,4 ; IRLEDlamp is aangesloten op rb4 bsf portb,4 call pulsduur bcf portb,4 call pulspauze goto tellerIRL ; naar pulsteller terug ; obstakeltestL btfsc porta,0 ; controleer de uitgang van de IRdetector return ; als die 0 is, dan is er obstakel en regel bsf IRvlaggen,0 ; overslaan, anders terug gaan naar hoofdprogramma return ; IRmetingenR ; voor de rechterkant call pulsuitR call obstakeltestR return ; pulsuitR movlw 0x0f ; f pulsen tellen movwf loop2 tellerIRR decfsz
loop2,f goto return
pulsvormR
; pulsvormR bsf portb,5 ; IRLEDlamp is aangesloten op rb5 bsf portb,5 call pulsduur bcf portb,5 call pulspauze goto tellerIRR
www.ccfz.nl 57
ROBOTICS CCFZ 27/3/2010
; obstakeltestR btfsc porta,1 ; controleer de uitgang van de IRdetector return ; als die 0 is, dan is er obstakel en regel bsf IRvlaggen,1 ; overslaan, anders terug gaan naar hoofdprogramma return ; pulsduur movlw 0x02 movwf loop3 opn1 decfsz loop3,f goto opn1 return ; pulspauze movlw 0x01 movwf loop4 opn2 decfsz loop4,f goto opn2 return ; rechtdoor ; WERKT ZÓ !!!!!!!!!!!!!!!!!!!!!!!!!! bcf portb,3 ; afbuigen uitzetten bcf portb,0 ; poort b0 clear M1, ri vooruit bsf portb,1 ; poort b1 setten M1, snelheid bcf portb,2 ; poort b2 clear M2 linksaf; set M2 rechtsaf movlw movwf
0xff loop1
; <====== deze in de praktijk fijnregelen ; gedurende deze loop wordt rechtdoor gereden
rdaftellen decfsz loop1,f goto rdaftellen goto start return ; achteruit bcf portb,3 ; afbuigen uit zetten bsf portb,0 ; rb0 bepaalt rijrichting: 0 = vooruit, 1 = achteruit bsf portb,1 ; volle snelheid movlw 0xff ; <====== deze in de praktijk fijnregelen movwf loop1 ; gedurende deze loop wordt achteruit gereden auaftellen decfsz loop1,f goto auaftellen goto start return ; buigrechts bcf portb,0 ; vooruit bsf portb,2 ; poort b2 richting links/rechts M2 bsf portb,3 ; poort b3 set = sturen movlw 0xff ; <====== deze in de praktijk fijnregelen movwf loop1 ; gedurende deze loop wordt rechtdoor gereden braftellen decfsz loop1,f goto braftellen goto start return ; buiglinks bcf portb,0 ; vooruit bcf portb,2 ; portb2 richting links/rechts M2 bsf portb,3 ; portb3 set is sturen M2 movlw 0xff ; <====== deze in de praktijk fijnregelen movwf loop1
www.ccfz.nl 58
ROBOTICS CCFZ 27/3/2010
blaftellen
decfsz goto goto
loop1,f blaftellen start
return ; paniek clrf status clrw movlw 0x03 ; zet de waarde 03 in het werkgeheugen subwf IRvlaggen,0 ; trek deze af van IRvlaggen en sla op in f btfss status,2 ; als nul het resultaat is, dan een regel overslaan goto bijsturen ; als het resultaat geen nul is dan doorgaan goto achteruit return ; bijsturen clrf status clrw movlw 0x00 ; controleren of IRvlaggen wel een geset bitje hebben subwf IRvlaggen,0 ; als dat niet het geval is opnieuw meten. btfss status,2 ; is er wel een geset bitje, dan naar afbuigen goto afbuigen goto rechtdoor ; afbuigen clrf status clrw movlw 0x01 subwf IRvlaggen,0 btfss status,2 goto buiglinks ; na buiglinks en buigrechts wordt teruggegaan goto buigrechts ; naar start om alles opnieuw in gang te zetten ; end
Het verschil van deze listing met de vorige betreft dus de aansturing van de motors via de Hbrug. Wederom gebeurt dat op basis van het al dan niet geset zijn van de vlaggen. Bij de realisatie kan het zijn, dat er toch een onjuiste reactie optreedt. Controleer dan de aansluitingen van de Hbrug. Motor 1 is voor de aandrijving van de achterwielen. Hiervoor is de polariteit belangrijk: plus en min verwisselen helpt tegen het achteruit rijden. En voor de stuurmotor geldt: is de reactie de verkeerde kant op: polariteit van de draadjes verwisselen door de aansluitingen om de draaien. Voor alle duidelijkheid nog een keertje: poort b0: set = achteruit clear = vooruit poort b1: set = rijden clear = stoppen poort b2: set = rechtsaf clear = linksaf poort b3: set = afbuigen clear = afbuigen uit. Het moeilijkst van dit alles is de IRdetectie. De juiste frequentie vinden, waar de de tector het meest gevoelig is. Experimenteren! We wensen je een groot doorzettingsvermogen en veel succes toe!
www.ccfz.nl 59