ROBOTICS CCFZ 27/3/2010

Transcriptie

1 Totaalplaatje robotbesturing De ingrediënten voor het programmeren van de racerobot hebben we nu behandeld. We kunnen een PIC processor programmeren. Het aansturen van een motor (gebruikmakend van de H brug) 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 BoE bot heeft dit systeem, dat voor veel robots populair is. Hieronder een foto van een dergelijke robot: de BeObot, gepimpt en wel, tijdens de Robotrace 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. 42

2 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 H brug. Met twee motors kunnen we onze Rommtech CCFZ H brug gebruiken. Beide systemen hebben als sensors links en rechts de IR detector, 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. 43

3 beide motors vooruit B obstakel links? nee ja set bitlinks ol obstakel rechts? ja set bitrechts or A 44 nee

4 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 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: 45

5 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 P routine 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! 46

6 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. PIC aansluiting rb0 rb1 rb2 rb3 rb4 rb5 ra0 ra1 Functie motor links richting motor links snelheid motor rechts richting motor rechts snelheid IR LED links IR LED rechts IR detector links IR detector 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 IR detectors een obstakel vinden, dat dan een goede, snelle reactie volgt. Beide IR ogen geven een signaal en dat moet snel 47

7 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 0 de (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 F7 toetsen, ook als gebruikelijk dus. Na het standaardbegin wordt het interessant. Vlaggen test Juni 2008 Hans Dorst list p=16f84 errorlevel 302 config 3ffd porta trisa trisb 0x05 0x06 0x85 0x86 rp0 status temp IRvlaggen 5 0x03 0x34 0x30 0x31 0x40 trisa 0x00 trisb IRvlaggen,1 subwf 0x03 btfss nop status,2 paniek van poort a alles als ingang instellen van poort b alles als uitgang instellen hierboven dus de standaardinstellingen clrf IRvlaggen alle vlaggen op 0 zetten paniek linker kant obstakel rechter kant obstakel 48

8 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 W register, WREG. Door subwf trekken we de waarde uit het werkregister WREG af van de waarde in het IRvlaggen geheugen. 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 om nog eens het geheel goed te kunnen nagaan. Maar als je één van de regels verandert in (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 IR detectors 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 trisa trisb 0x05 0x06 0x85 0x86 rp0 status temp loop3 loop4 IRvlaggen 5 0x03 0x34 0x30 0x31 0x32 0x33 trisa statusr,rp0 0x00 trisb 0x40 van poort a alles als ingang instellen van poort b alles als uitgang instellen hierboven dus de standaardinstellingen

9 end Dit is het bekende begin. Een viertal registerplaatsen zijn gereserveerd voor verschillende loops, en een voor de vlaggen van de IR afstandsmeting. De racerobot zal vanaf het begin op volle snelheid rechtdoor moeten rijden. Dat verdient een aparte routine, die we vanaf het begin aanroepen: rechtdoor Achteraan het programma zetten we deze routine, waarbij we ons herinneren hoe de H brug 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 H brug 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 rb aansluitingen een 1. Volgens bladzijde 6 is de hexadecimale waarde daarvoor f. De routine is dan ook: rechtdoor 0x0f maak de vier rb poorten alle 1 rdaftellen gedurende deze loop wordt rechtdoor gereden rdaftellen 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 maak poorten 1 en 3 gelijk 1 en 0 en 2 gelijk 0 mowf auaftellen gedurende deze loop wordt achteruit gereden 50

10 auaftellenrd 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 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 braftellen even rechtdoor 0x0d rechter wiel stopt even, poort rb1 even 0,f braftellen en tenslotte buiglinks blaftellen even rechtdoor 0x07 rechter wiel stopt even, poort rb1 even 0,f blaftellen 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 0x07 meer keren herhalen. Een langere bocht ontstaat door de regel 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 IR LED lampjes en de twee IR detectors van dezelfde soort moeten zijn. Dat zal meestal het geval zijn. IRmetingenL IRL voor de linkerkant pulsuitl 51

11 obstakeltestl IRL pulsuitl tellerirl,f pulsvorm pulsvorml,4 IR LED lamp is aangesloten op rb4,4 pulsduur,4 pulspauze tellerirl obstakeltestl btfsc porta,0 controleer de uitgang van de IR detector als die 0 is, dan is er obstakel en regel overslaan, anders terug gaan naar hoofdprogramma IRmetingenR IRR voor de rechterkant pulsuitl obstakeltestl IRR pulsuitr tellerirr pulsvorm pulsvormr,5 IR LED lamp is aangesloten op rb4,5 pulsduur,5 pulspauze tellerirr obstakeltestr btfsc porta,1 controleer de uitgang van de IR detector als die 0 is, dan is er obstakel en regel IRvlaggen,1 overslaan, anders terug gaan naar hoofdprogramma pulsduur 0x02 loop3 opn1 loop3,f opn1 52

12 pulspauze 0x02 loop4 opn2 loop4,f opn2 Dat schiet aardig op. De resultaten van de IR meting 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 trisa trisb 0x05 0x06 0x85 0x86 rp0 status temp loop3 loop4 IRvlaggen 5 0x03 0x34 0x30 0x31 0x32 0x33 0x40 trisa 0x00 trisb pulsuitl van poort a alles als ingang instellen van poort b alles als uitgang instellen hierboven dus de standaardinstellingen rechtdoor clrf Irvlaggen beide vlaggen op 0, nul zetten IRmetingenL IRmetingenR paniek bijsturen IRmetingenL IRL voor de linkerkant 53

13 obstakeltestl pulsuitl tellerirl,f pulsvorml pulsvorml,4,4 pulsduur,4 pulspauze tellerirl obstakeltestl btfsc porta,0 IRmetingenR IRR pulsuitl obstakeltestl pulsduur 0x02 loop3 controleer de uitgang van de IR detector 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,f pulsvormr pulsvormr,5,5 pulsduur,5 pulspauze tellerirr obstakeltestr btfsc porta,1 IRvlaggen,1 opn1 IR LED lamp is aangesloten op rb4 IRR pulsuitr tellerirr hier tot 11 (a) tellen voor 11 pulsen IR LED lamp is aangesloten op rb4 controleer de uitgang van de IR detector 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 pulspauze 0x02 loop4 opn2 loop4,f 54

14 opn2 rechtdoor 0x0f rdaftellen,f rdaftellen gedurende deze loop wordt rechtdoor gereden achteruit auaftellen nop,f auaftellen maak poorten rb1 en rb3 gelijk 1 enr 0 en rb2 gelijk 0 zie ook bladzij 6 voor binaire informatie gedurende deze loop wordt achteruit gereden buigrechts braftellen 0x0d,f braftellen nop buiglinks blaftellen 0x07,f blaftellen paniek 0x03 subwf btfss status,2 bijsturen achteruit bijsturen 0x00 subwf btfss status,2 afbuigen afbuigen maak de vier rb poorten alle 1 even rechtdoor rechter wiel stopt even, poort rb1 even 0 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 55

15 subwf btfss status,2 buigrechts buiglinks end na buiglinks en buigrechts wordt teruggegaan naar 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 IR detectors, 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 IR detector 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 H brug 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 trisa trisb option_reg 0x05 0x06 0x85 0x86 0x81 rp0 5 status 0x03 temp 0x34 0x30 0x31 loop3 0x32 loop4 0x33 IRvlaggen 0x40 hierboven de koptekst trisa van poort a alles als ingang instellen 56

16 0x00 trisb,0,1 van poort b alles als uitgang instellen hierboven de standaardinstellingen. vooruit en: poort b0 clear M1, ri vooruit poort b1 setten M1, snelheid clrf IRvlaggen IRvlaggen,1 rechtdoor IRmetingenL IRmetingenR paniek bijsturen IRmetingenL voor de linkerkant pulsuitl obstakeltestl pulsuitl 0x0f f pulsen aftellen tellerirl,f pulsvorml na f pulsen volgende regel overslaan terug naar IRL pulsvorml,4 IR LED lamp is aangesloten op rb4,4 pulsduur,4 pulspauze tellerirl naar pulsteller terug obstakeltestl btfsc porta,0 controleer de uitgang van de IR detector als die 0 is, dan is er obstakel en regel overslaan, anders terug gaan naar hoofdprogramma IRmetingenR voor de rechterkant pulsuitr obstakeltestr pulsuitr 0x0f f pulsen tellen tellerirr,f pulsvormr pulsvormr,5 IR LED lamp is aangesloten op rb5,5 pulsduur,5 pulspauze tellerirr 57

17 obstakeltestr btfsc porta,1 controleer de uitgang van de IR detector als die 0 is, dan is er obstakel en regel IRvlaggen,1 overslaan, anders terug gaan naar hoofdprogramma pulsduur 0x02 loop3 opn1 loop3,f opn1 pulspauze 0x01 loop4 opn2 loop4,f opn2 rechtdoor WERKT ZÓ!!!!!!!!!!!!!!!!!!!!!!!!!!,3 afbuigen uitzetten,0 poort b0 clear M1, ri vooruit,1 poort b1 setten M1, snelheid,2 poort b2 clear M2 linksaf set M2 rechtsaf gedurende deze loop wordt rechtdoor gereden rdaftellen,f rdaftellen achteruit,3 afbuigen uit zetten,0 rb0 bepaalt rijrichting: 0 = vooruit, 1 = achteruit,1 volle snelheid gedurende deze loop wordt achteruit gereden auaftellen,f auaftellen buigrechts,0 vooruit,2 poort b2 richting links/rechts M2,3 poort b3 set = sturen gedurende deze loop wordt rechtdoor gereden braftellen,f braftellen buiglinks,0 vooruit,2 2 richting links/rechts M2,3 3 set is sturen M2 58

18 blaftellen,f blaftellen paniek clrf status clrw 0x03 zet de waarde 03 in het werkgeheugen subwf trek deze af van IRvlaggen en sla op in f btfss status,2 als nul het resultaat is, dan een regel overslaan bijsturen als het resultaat geen nul is dan doorgaan achteruit bijsturen clrf status clrw 0x00 controleren of IRvlaggen wel een geset bitje hebben subwf als dat niet het geval is opnieuw meten. btfss status,2 is er wel een geset bitje, dan naar afbuigen afbuigen rechtdoor afbuigen clrf status clrw 0x01 subwf btfss status,2 buiglinks na buiglinks en buigrechts wordt teruggegaan buigrechts naar 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 H brug. 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 H brug. 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 IR detectie. De juiste frentie vinden, waar de de tector het meest gevoelig is. Experimenteren! We wensen je een groot doorzettingsvermogen en veel succes toe! 59