Stap 8: Programmeren van de Robot-Ant
Autonome pick-up modus draait de robot naar rechts op zoek naar een object. Als het vindt niets stopt na 360 graden draaien. Wanneer een object wordt gedetecteerd de vergelijking afgebroken en corrigeert voor over de besturing en het gebruik van de gemeten afstand voor het gevonden object, wordt voorwaarts verplaatst een bepaalde afstand. Vervolgens sluit de grijper en liften.
Afstandsbediening
Remote control mode wacht de robot op een signaal van een beweging zoals vooruit of voor een opdracht in autonome modus. Een standaard universele afstandsbediening kan zo worden geconfigureerd dat deze uitgang van de Sony TV-code die de Picaxe kan decoderen. Het infrarood van de afstandsbediening wordt ontvangen door de ontvangersmodule van Panasonic.
Hier is de programmacode voor de robot mier:
' robot mier, Picaxe 20 x-2
laat dirsc = % 00000000
setfreq m8
onderbreken van 2000
' loop22: ' test van infrarood afstandssensor
' readadc 7, b4
' debug b4
' goto loop22
startreset: ' opnieuw voor de start
serout c.0,t2400 ($80, $01, $01,2,30)'set4 voor servo #, snelheid set5 van 0to127
serout c.0,t2400,($80,$01,$04,2,26,30) ' lichaam neer 26
onderbreken van 200
serout c.0,t2400,($80,$01,$01,1,30) ' snelheid instellen laatste # van 0 tot 127
serout c.0,t2400,($80,$01,$04,1,26,127) ' sluit grijper 26
onderbreken van 200
serout c.0,t2400,($80,$01,$01,0,20) ' snelheid instellen laatste # van 0 tot 127
serout c.0,t2400,($80,$01,$04,0,26,127) ' hoofd center
onderbreken van 1000
loopinfra: ' afstandsbediening van bewegingen of autonome sequenties activeren
lage c.1 ' blauw oog
hoge c.2 ' blauw oog
IRIN c.5, b6
Als b6 = 0 dan act1 ' externe button1 = 0
Als b6 = 1 dan forw ' b2
Als b6 = 2 dan act2 ' b3
Als b6 = 3 dan links ' b4
Als b6 = 4 dan stop1 ' b5
Als b6 = 5 dan recht ' b6
Als b6 = 6 dan act3 ' b7
Als b6 = 7 dan rev1 ' b8
Als b6 = 8 dan biteme ' aanval
' als b6 = 59 dan biteme ' aanval
Als b6 = 11 then putdwn ' Voer put down
Als b6 = 21 dan startreset ' reset servo's
' debug b6
goto loopinfra
ACT1: ' robot ant manen de wereld
GOSUB forw2
onderbreken van 2500
GOSUB bodyup2
GOSUB left2
GOSUB headleft2
onderbreken van 300
GOSUB forw2
onderbreken van 1200
GOSUB stop2
GOSUB forw2
onderbreken van 500
GOSUB stop2
GOSUB headright2
hoge c.1 ' rode ogen l
lage c.2 ' rode ogen r
onderbreken van 1000
GOSUB right2
onderbreken van 2500
GOSUB headcenter2
GOSUB bodydwn2
GOSUB rev2
onderbreken van 2500
GOSUB stop2
GOSUB leftfast
onderbreken van 500
GOSUB stop2
GOSUB rightfast
onderbreken van 500
GOSUB leftfast
onderbreken van 500
GOSUB stop2
GOSUB rightfast
onderbreken van 500
GOSUB stop2
onderbreken van 2200
GOSUB forw2
GOSUB bodyup2
onderbreken van 3500
GOSUB opengrip2
onderbreken van 500
GOSUB stop2
goto loopinfra
biteme:
GOSUB forw2
B8 = 1
voor b8 = 1 tot 175
' obstakel vinden
hoge c.2 ' rode ogen l
lage c.1 ' rode ogen r
ReadADC 7, b4
Als b4 > 60 en b4 < 135 dan next3
onderbreken van 30
volgende b8
goto loopinfra
next3:
GOSUB stop2
GOSUB bodydwn2
GOSUB opengrip2
' 100 onderbreken
GOSUB bodyup2
GOSUB forw2
onderbreken van 300
GOSUB closegrip2
onderbreken van 900
GOSUB stop2
GOSUB rev2
GOSUB bodydwn2
onderbreken van 6000
GOSUB stop2
goto loopinfra
act2: ' Pickup, Sovjettanks
' voorwaarts
serout c.0,t2400,(128,0,1,100) ' vooruit rechts 20min tot 127
serout c.0,t2400,(128,0,2,100) ' naar voren links 20min
onderbreken van 4500
GOSUB stop2
onderbreken van 100
' vinden van blok
B8 = 1
voor b8 = 1 tot en met 75
' vinden blok en bestelwagen
hoge c.2
lage c.1
GOSUB right2
ReadADC 7, b4
Als b4 > 61 en b4 < 164 dan detect0 ' 34-75
onderbreken van 50
volgende b8
GOSUB stop2
goto loopinfra
detect0: ' stoppen en corrigeren
GOSUB stop2
' langzaam linksaf
serout c.0,t2400,(128,0,1,60) ' vooruit rechts 20min tot 127
serout c.0,t2400,(128,0,3,60) ' rev links 20min
onderbreken van 240
GOSUB stop2
onderbreken van 100
lage c.2 ' rode ogen
hoge c.1 ' rode ogen
' vooruit, pick-up
GOSUB opengrip2
onderbreken van 1000
GOSUB forw2
Als b4 < 50 dan pa1
Als b4 < 60 dan pa2
Als b4 < 70 dan pa3
Als b4 < 80 dan pa4
Als b4 < 90 dan pa5
Als b4 < 104 dan pa6
PA1:
onderbreken van 9000
goto stopat
PA2:
5920 onderbreken
goto stopat
PA3:
3800 onderbreken
goto stopat
pa4:
onderbreken van 2800
goto stopat
PA5:
onderbreken van 2100
goto stopat
PA6:
onderbreken van 980
goto stopat
stopat:
GOSUB stop2
onderbreken van 200
' closegrip
serout c.0,t2400,($80,$01,$01,1,30) ' de laatste # van de snelheid-set van 0 tot 127
serout c.0,t2400,($80,$01,$04,1,26,127) ' sluit grijper 26
onderbreken van 2000
GOSUB bodyup2
' Sovjettanks
GOSUB leftfast
onderbreken van 1400
GOSUB stop2
GOSUB forw2
onderbreken van 1800
GOSUB stop2
GOSUB bodydwn2
GOSUB opengrip2
onderbreken van 500
' gloat
GOSUB rev2
onderbreken van 2000
GOSUB stop2
GOSUB bodyup2
GOSUB headleft2
GOSUB headright2
GOSUB headcenter2
lage c.1 ' blauw oog
hoge c.2 ' blauw oog
goto loopinfra
act3: ' open grijper en doorsturen
' vooruit, pick-up
GOSUB opengrip2
onderbreken van 400
GOSUB forw2
onderbreken van 280
GOSUB stop2
goto loopinfra
act4: ' sluit grijper en lift
GOSUB closegrip2
onderbreken van 600
GOSUB bodyup2
goto loopinfra
' ' *** gosubs
leftslow:
' langzaam linksaf
serout c.0,t2400,(128,0,1,60) ' vooruit rechts 20min tot 127
serout c.0,t2400,(128,0,3,60) ' rev links 20min
onderbreken van 220
GOSUB stop2
terugkeer
stop2:
serout c.0,t2400,(128,0,0,0) ' harde stop gelijk
serout c.0,t2400,(128,0,6,0) ' harde stop links
terugkeer
forw2:
serout c.0,t2400,(128,0,1,100) ' vooruit rechts 20min tot 127
serout c.0,t2400,(128,0,2,100) ' naar voren links 20min
terugkeer
Left2:
serout c.0,t2400,(128,0,1,100) ' vooruit rechts 20min
serout c.0,t2400,(128,0,3,100) ' rev links 20min
terugkeer
right2:
serout c.0,t2400,(128,0,2,100) ' naar voren links 20min
serout c.0,t2400,(128,0,0,100) ' rev juiste 20min
terugkeer
rev2:
' stoppen
serout c.0,t2400,(128,0,0,0) ' harde stop gelijk
serout c.0,t2400,(128,0,6,0) ' harde stop links
onderbreken van 50
' omgekeerde
serout c.0,t2400,(128,0,0,100) ' rev juiste 20min
serout c.0,t2400,(128,0,3,100) ' rev links 20min
terugkeer
opengrip2:
serout c.0,t2400 ($80, $01, $01,1,30)'set4 voor servo #, snelheid set5 van 0to127
serout c.0,t2400,($80,$01,$04,1,13,127) ' open grijper, 13
onderbreken van 1000
terugkeer
closegrip2:
serout c.0,t2400,($80,$01,$01,1,30) ' snelheid instellen laatste # van 0 tot 127
serout c.0,t2400,($80,$01,$04,1,26,127) ' sluit grijper 26
onderbreken van 2000
terugkeer
headleft2:
serout c.0,t2400 ($80, $01, $01,0,20)'set4 voor servo #, snelheid set5 van 0to127
serout c.0,t2400,($80,$01,$04,0,15,127) ' hoofd links 15
onderbreken van 800
terugkeer
headright2:
' serout c.0,t2400,($80,$01,$01,0,20) ' snelheid instellen laatste # van 0 tot 127
serout c.0,t2400,($80,$01,$04,0,32,127) ' hoofd juiste 30
onderbreken van 1600
terugkeer
headcenter2:
serout c.0,t2400,($80,$01,$01,0,20) ' snelheid instellen laatste # van 0 tot 127
serout c.0,t2400,($80,$01,$04,0,26,127) ' hoofd center
onderbreken van 500
terugkeer
bodyup2:
serout c.0,t2400 ($80, $01, $01,2,30)'set4 voor servo #, snelheid set5 van 0to127
serout c.0,t2400,($80,$01,$04,2,13,30) ' lichaam omhoog, 13
onderbreken van 220
terugkeer
bodydwn2:
serout c.0,t2400 ($80, $01, $01,2,30)'set4 voor servo #, snelheid set5 van 0to127
serout c.0,t2400,($80,$01,$04,2,26,30) ' lichaam neer 26
onderbreken van 200
terugkeer
leftfast:
serout c.0,t2400,(128,0,1,127) ' vooruit rechts 20min
serout c.0,t2400,(128,0,3,127) ' rev links 20min
terugkeer
rightfast:
serout c.0,t2400,(128,0,2,127) ' naar voren links 20min
serout c.0,t2400,(128,0,0,127) ' rev juiste 20min
terugkeer
