Robot Recycling Rally

Transcription

Eurobot 2007 – Final design
Eurobot 2007 – Endeligt design
Rapporten er udarbejdet af:
Martin Juhl Jørgensen (s042353)
Vejleder(e):
Nils Andersen
Ole Ravn
Ørsted•DTU
Automation
Danmarks Tekniske Universitet
Elektrovej
Bygning 326
2800 Kgs. Lyngby
Denmark
www: www.iau.dtu.dk
Tel: (+45) 45 25 35 50
Fax: (+45) 45 88 12 95
Udgivelsesdato:
27. juni 2007
Klasse:
Offentlig
Bemærkninger:
Denne rapport er indleveret som led i opfyldelse af kravene for
opnåelse af bachelorgraden på Danmarks Tekniske Universitet.
Rapporten repræsenterer 15 ECTS point.
Rettigheder:
© Martin Juhl Jørgensen, 2007
Forord
Denne rapport er afslutningen på mit bachelorprojekt indenfor elektroteknologi. Projektet har bragt mig vidt omkring på flere måder. Projektet har været
mere praktisk end de fleste andre projekter jeg har lavet i min tid på DTU,
det har været en lærerig proces. Samtidig bragte projektet os til Frankrig hvor
dette års Eurobot konkurrence blev afholdt. Det har været en god oplevelse at
være med til Eurobot konkurrencen og møde flere ligesindede studerende fra
andre lande. Det er i hvert fald ikke svært at forstå hvorfor det er de samme
folk der bliver ved med at vende tilbage år efter år.
Udover vores vejledere Nils Andersen og Ole Ravn, så har Christian Andersen været til stor hjælp for projektet. I særdeleshed når kameraet eller laserscanneren ikke makkede ret.
Kongens Lyngby, 27. juni 2007
Martin Juhl Jørgensen
s042353
iii
Indhold
Forord
iii
Indhold
iv
1
Indledning
1
2
Analyse af Eurobot 2007
2.1 Affaldshåndtering . . . . . . . . . . . . . . . . . . . . . . . . . . .
2.2 Orientering & navigering . . . . . . . . . . . . . . . . . . . . . .
2.3 Forhindringer . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
3
4
8
8
3
Robot design - Mekanik
3.1 Dåse og flaske mekanik . . . . . . . . . . . . . . . . . . . . . . . .
3.2 Batterihåndtering . . . . . . . . . . . . . . . . . . . . . . . . . . .
3.3 Laserscanner . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
9
9
10
10
4
Robot design - Elektronik
4.1 SMR komponenter . . . . .
4.2 Startsnor . . . . . . . . . . .
4.3 Børstemotor . . . . . . . . .
4.4 Laserscanner . . . . . . . . .
4.5 Farve-sensorer . . . . . . . .
4.6 Batteri-sensor . . . . . . . .
4.7 Detektering af modstander .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
11
11
12
12
13
13
14
16
Software
5.1 SMRdemo server . . . .
5.2 Start & slut funktioner .
5.3 Fejlhåndtering . . . . . .
5.4 Basale kørselsfunktioner
5.5 Kamera . . . . . . . . . .
5.6 Dåse og flaske aflevering
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
17
18
18
18
19
20
23
Designevaluering
6.1 Designkoncepter
6.2 Mekanik . . . . .
6.3 Elektronik . . . .
6.4 Autonomitet . . .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
27
27
30
30
31
5
6
7
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
Konklusion
33
Litteratur
35
Appendiks
37
iv
Indhold
A Regler & FAQs
A.1 Regler . . . . . .
A.2 FAQ 1 . . . . . .
A.3 FAQ 2 . . . . . .
A.4 FAQ 3 . . . . . .
A.5 Approval Sheet
v
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
B SMR-CL
C Kildekode
C.1 haupt.c . . . . . .
C.2 klient.h . . . . . .
C.3 klient.c . . . . . .
C.4 myfunc.h . . . . .
C.5 myfunc.cpp . . .
C.6 Definitions.h . . .
C.7 Communication.h
C.8 Communication.c
C.9 Servostyring.h . .
C.10 Servostyring.c . .
39
39
67
74
79
83
87
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
95
95
113
114
116
118
129
130
130
132
132
K APITEL
Indledning
Eurobot er en international robotkonkurrence, hvor folk fra hele verden kommer for at dyste mod hinanden og dele erfaringer. Udfordringerne varierer fra
år til år og temaet for Eurobot 2007 er “Recycling”. Robotterne skal indsamle, sortere og smide affald ud i bestemte beholdere. Affaldet består af dåser,
flasker og batterier. Banen kan ses på figur 1.1.
Figur 1.1: Eurobot 2007 banen
De design der var på tegnebrættet er dels beskrevet i [HJJ07] og dels beskrevet af Shayantharan Sivarajalingam (s042297), Østed•DTU ultimo juni
2007. Der blev derefter arbejdet med et af disse designkoncepter. Det planlagte
design, beskrevet af Brian Hansen (s042307) i [Han07], blev dog skrottet grundet tidsmæssige årsager. Den endelige robot, den som denne rapport handler
om, er baseret på en robot der tidligere har deltaget i Eurobot konkurrencen.
Valget faldt på robotten der var med til Eurobot 2006, blandt andet fordi den
overholder kravene til omkredsen, da samme regler var gældende til Eurobot
2006. Et mål er at den skulle ombygges således at den originale version kan
genskabes uden for meget besvær.
1
1
K APITEL
Analyse af Eurobot 2007
Banen til Eurobot 2007 er vist på figur 1.1. Robotten starter i enten det røde
eller det blå hjørne, og spiller som den pågældende farve. Kurvene hvor det
sorterede affald skal afleveres er i det modsatte hjørne af starthjørnet. Batterierne skal derimod afleveres i skålen på midten af banen. Skålen har en tilfældig placering blandt tre mulige. Robotten skal aktiveres af en snor med en
længde på mindst en halv meter. Hver kamp varer 90 sekunder og når tiden
er gået skal robotten selv stoppe og slukke for alle motorer. Derudover skal
robotten også forsynes med en nødstopknap så dommerne kan skride ind,
skulle det være nødvendigt.
Som det kan ses på figur 1.1 er der på banen placeret 20 dåser, 20 flasker
og 3 batterier i to trekanter. Derudover er der placeret en flaske, en dåse og
to batterier i midten af banen. Flaskerne og dåserne er farvekodet, hhv. grøn
og gul så robotterne nemmere kan genkende dem. Ydermere er de fyldt med
skum der sørger for at de holder formen under kampen. Hver flaske og dåse
afleveret i deres respektive kurve giver et enkelt point. Batterierne er det eneste magnetiske affald på banen og de er farvet enten blå eller rød. Batterierne
må således kun opsamles de af den robot der kører med samme holdfarve.
For hvert batteri afleveret i skålen giver det to point. Holder robotten på modstanderens batteri kan man blive diskvalificeret.
Et af kravene til robotten er dens omkreds. I startposition må omkredsen
ikke overstige 120 cm, mens den i udfoldet tilstand må være op til 140 cm. Robotten må først folde sig ud efter startsnoren er hevet og kampen er i gang. Se
figur 2.1 for definitionen fra de officielle regler. Robotten må have en maksi-
Figur 2.1: Definition på omkredsen fra de officielle regler
3
2
4
KAPITEL 2. ANALYSE AF EUROBOT 2007
mal højde på 35 cm, men skal være udstyret med en solid mast i 45 cm højde.
Masten er beregnet til at modstander holdet kan montere et radiofyr, et såkaldte beacon, til at lokalisere den anden robot. Udover det beacon man må
sætte på sin modstander er der mulighed for at placere yderligere tre beacons.
Se figur 1.1 for placeringerne.
Udover at kunne samle og sortere affald, skal robotten også kunne detektere modstander robotten og vise velvilje til at stoppe for at undgå kollisioner.
Hvis robotten ikke stopper, eller med vilje påfører modstanderen skade vil
robotten blive diskvalificeret.
Udfordringerne kan koges ned til følgende fem discipliner:
• Robot størrelsen. Da flaskerne og dåserne er rimelig store i forhold til
størrelseskravene, er dette en af de sværeste udfordringer ved Eurobot
2007.
• Indsamlingen af affald. Affaldets position og retning er ukendt, og vil
med meget stor sandsynlighed ændre sig mange gange i løbet af en
kamp.
• Affaldet skal sorteres og afleveres i deres respektive beholdere. Batteri beholderens position er den eneste ukendte, men affaldet skal i alle
tilfælde løftes 3-5 cm op for at afleveres. Affaldet kan dog skelnes fra
hinanden ved deres størrelse, farve, vægt og magnetiske egenskaber.
• Detektering af forhindringer. Modstander robotten er den største forhindring i kampen, den anden er batteribeholderen.
• Robust design. Under kampen kan forholdene ændre sig meget for robotten, især lysforholdene siger erfaringerne [SJHJ05, RTJH06].
Se bilag A for det endelige udkast af de officielle regler.
2.1
Affaldshåndtering
Affaldet kan deles op i to grupper, flasker og dåserne, og batterierne. Opdelingen bunder i størrelsen på emnerne; flaskerne og dåserne har tilnærmelsesvis
samme diameter på lige omkring 60 mm. Batterierne er derimod mindre og
tungere. Til gengæld er de også meget magnetiske. Flaskerne og dåserne kan,
grundet deres tilnærmelsesvis samme diameter, behandles med samme mekanisme i mange tænkelige tilfælde. Der er dog en del problemer med flaskerne
på grund af deres udformning, flaskehalsen har det med at sidde fast. Dåserne er næsten helt cylindriske hvilket gør det noget nemmere at håndtere dem.
De er derimod næsten halvt så lange som flaskerne, hvilket vil sige at der, alt
efter mekanismen, kan snige sig to dåser ind i stedet for en.
For at se hvordan flaskerne og dåserne reagerer under en kamp, blev en
SMR1 sat til at køre en vilkårlig rute på banen. I praksis viste det sig at flaskerne og dåserne havde en tendens til at rulle op langs banderne efter en hel
kamp. Derudover blev det også observeret at flaskerne og dåserne var meget
letpåvirkelige. Da kun robottens egen rute er kendt kan der ikke regnes med at
flaskerne og dåserne ligger stille og rolig, lige til at samle op. Det stiller krav til
1 Small
Mobile Robot
2.1. AFFALDSHÅNDTERING
5
at indsamlingsmekanismen er tolerant overfor forstyrrende elementer, såsom
dåser og flasker der rulle rundt.
Hvis man vælger kun at gå efter batterier kan man højest score 4×2 point =
8 point. Hvis vi går ud fra at halvdelen af dåserne og halvdelen af flaskerne
bliver beslaglagt af modstanderen, er der maksimalt 10 × 1 point = 10 point
point fra både flaskerne og dåserne at hente. Blandt andet derfor har vi har
valgt at gå efter alle typer affald. Det vil forhåbentlig sprede pointene ud på
de forskellige typer affald, og hjælpe at vi ikke står i en situation hvor vi ikke
kan tage mere affald fordi modstanderen har brugt samme taktik. Vi kunne
selvfølgelig have valgt at gå kun efter batterierne, men de er svære at få øje på
blandt alt det andet affald grundet deres størrelse.
Dåse og flaske mekanik
Vi har valgt at satse på en simpel mekanisk løsning der kan klare både flasker
og dåser. En af de første tanker vi fik var at skubbe affaldet hen til kurvene,
hvorefter det enten kunne blive skubbet eller f.eks. skovlet op i kurven. For
at kunne skubbe affaldet rundt på banen skal vi sørge for at det ikke smutter
fra robotten når den bakker, drejer eller bare kører lige ud. Tankegangen er at
dåserne og flaskerne meget gerne vil rulle, selv hvis de bliver påvirket af en
kraft helt nede ved jorden. Det kan f.eks. realiseres med en bøjle som skitseret
på figur 2.2. Vores mock-up test viste at der skulle rimelig meget til før affaldet
Figur 2.2: Dåse og flaske indsamling
brød ud når det først var fanget. Testen viste også at affaldet ikke var til at få
indfanget ved bare at køre hårdt ind i det. Det vil sige at for at få en dåse eller
flaske indfanget bliver robotten nød til at presse den op i mod f.eks. en væg,
da den ellers vil rulle foran bøjlen.
Problemet med at have flaskerne og dåserne udenfor robotten er dels sorteringen og dels afleveringen. Idet der kun kan ligge én flaske eller to dåser
ad gangen, er problemet med sorteringen begrænset til at detektere hvilken
type affald robotten har fanget. Flaskerne og dåserne skal afleveres over en
6
4-5 cm høj væg, så det er nødvendigt med en mekanisme der kan transportere
affaldet over væggen. Der blev prøvet med to forskellige mekanismer:
Roterende pind der vipper affaldet over væggen. Simpel mekanisme der vil
kræve en servomotor og en pind. Den har det minus at robotten skal tæt
på væggen før den kan aflevere en dåse eller en flaske.
Børste der roterer med høj hastighed og fejer affaldet over væggen. Ulempen
er at der skal børsten skal presse dåsen/flasken mod væggen før det
virker overbevisende.
Efter at have prøvet sig frem med en testopsætning af hver mekanisme viser
det sig i praksis at være nemmere at få børsten til at virke. Løsningen kræver at
flasken skal presses godt op mod væggen så motoren får ordentligt fat, hvilket
betyder at styringen af robotten skal være rimelig tolerant ved aflevering af
affald.
Til at finde ud af hvilken type affald der er tale om, bruger vi et webkamera. Selvom lysforholdene ændre sig drastisk i løbet af en kamp, så er det ikke
så kritisk når man kun skal detektere farve forskelle.
Batterihåndtering
Batterimekanismen blev udviklet til den planlagte robot [Han07], og da vi
synes at det er langt den bedste måde at samle batterierne op på har vi også
brugt mekanismen til den endelig robot.
Testkørseler, se tabel 2.1, har vist at en tragt i langt de fleste tilfælde virker
upåklageligt. Testen er kørt hvor batteriet er placeret således at robotten vil
køre ind i det ved kørsel ligeud. Batterierne var derudover placeret og roteret
vilkårligt. I de fleste tilfælde har batteriet lagt sig fint forenden af tragten uden at sidde fast. De gange hvor det går galt er det fordi batteriet har sat sig
fast på tværs af tragten der hvor den snæver sig ind. Dette kan løses ved at
installere en form for sensor der kan detektere hvornår et batteri sidder fast,
og foretage en manøvre så batteriet kommer helt ind. I en anden test har vi
Tilfældig placering
0. forsøg (%)
88,2
Fanget (%)
100
Mistet (%)
0
Tabel 2.1: Opsummerede resultater af batteritragt-testen (vilkårlig orientering).
netop afprøvet en manøvre vi kom frem til eksperimentelt. Den går ud på at
vi bakker lidt, drejer en anelse og derefter fortsætter ligeud. Resultaterne af
manøvren kan er vist i tabel 2.2. I testen er batterierne lagt så de lægger sig
fast i tragten, hvorefter robotten skulle udføre manøvren. Som det kan ses får
robotten næsten fat i alle batterierne efter et enkelt forsøg, og sammenholdt
med resultaterne fra tabel 2.1 ser løsningen meget lovende ud.
Idéen med batteritragten er at indsamlingen af batterier skal ske passivt,
altså at kørslen ikke afbrydes fordi robotten skal hen og aflevere et batteri. Da
robotten maksimalt må tage 4 batterier op, resten er modstanderens, fylder
en batteribeholder ikke meget. Et hurtigt overslag er 4 × 3, 3 cm ≈ 13, 5 cm,
2.1. AFFALDSHÅNDTERING
Flad side mod venstre forfra
Flad side mod højre forfra
Plus-side mod højre forfra
7
1. forsøg (%)
96
100
100
Fanget (%)
96
100
100
Mistet (%)
4
0
0
Tabel 2.2: Opsummerede resultater af batteritragt-testen (fastsiddende batteri).
hvor de 3, 3 cm er diameteren på et type D batteri. Da spillereglerne siger
at man ikke må holde på en modstanders batteri, bliver robotten nød til at
være i stand til at frasortere modstanderens batterier. Rent mekanisk kan det
gøres ved at have en lem for enden af tragten der enten er åben eller lukket
alt efter om batteriet er hhv. modstanderens eller ej. Derved undgår man at
batteriet batteriet fortsætter ud, som det vil være tilfældet hvis der ingen lem
var. Batterierne er farvekodet og til at kende forskel på farverne har vi ladet
os inspirere af Team DTU fra Eurobot 2006 [RTJH06]. De brugte to lysdioder
og en fotocelle til at se om der dels er et objekt og dels hvilken farve det har.
Batterierne skal afleveres i en skål med en kant højde på 3 cm. For at få
batteriet op i den højde, når det ligger i tragten, var der flere idéer i spil. Udgangspunktet var at udnytte batteriets magnetiske egenskaber. Det er mekanisk simplere end at f.eks.gribe om batteriet. Idéerne blev kogt ned til følgende, se figur 2.3:
(a) Lodret løft, op gennem lem
(b) Skråt løft
Figur 2.3: Idéer til batteriopsamlingen.
Lodret løft: En magnet sættes fast på en tandrem. Når batteriet ligger klar
køres magneten ned, fanger batteriet og kører op igen. Batteriet vil blive
transporteret op igennem to hængsler, der åbner i opad, og fortsætte lige
indtil der er nok plads til at hængslerne kan lukke igen. Samtidig bliver
batteriet holdt igen, således at det bliver skubbet af magneten. Det vil
resultere i at batteriet vil falde ned på de to hængsler og rulle så langt
frem som det kan i batteribeholderen.
8
Skråt løft: For enden af en stang/arm fastgøres en magnet. Når batteriet ligger klar i tragten, kører armen ned indtil magneten får fat batteriet. Herefter hejser armen batteriet op ad en skrå plade. Når batteriet når toppen
af pladen vil batteriet svinge sig fri fra magneten og ned i beholderen.
Begge mekanismer er blevet testet med nogle simple mock-up modeller, men
hverken fuldt automatiseret og kørende. Det lodrette løft, figur 2.3(a), er noget
svære at lave mekanisk end løsningen nummer to.
2.2
Orientering & navigering
For at kunne navigere rundt på banen bliver robotten nødt til at kunne orientere sig. Det blev tidligt besluttet at orienteringen skulle bestå af odometri og
en laserscanner. Kamera vision blev hurtigt udelukket da tidligere års erfaringer på IAU har vist at de skiftende lysforhold under kampen er meget svære
at tage højde for og få til at virke robust. På både banen og modstanderen er
der plads til at opstille de såkaldte beacons til at hjælpe med at stedbestemme robotten. Et beacon kan f.eks. bruge infrarødt lys eller radiosignaler til at
bestemme afstanden til robotten. At lave sådan et system kræver i midlertidig meget tid, især for at gøre systemet robust. På den baggrund er brugen af
beacons fravalgt.
En metode til at finde ud af hvor robotten befinder sig er odometri, der
kort sagt går ud på at måle den kørte afstand på hjulene. Da IAU bruger odometri til SMR’erne, og det har vist sig at være tilstrækkeligt robust, findes
alle værktøjerne i vid udstrækning allerede. Odometri udmærker sig med høj
præcision ved langsom kørsel og relative korte afstande, men ved længere
kørsler bliver vinklen meget usikker da vinkelfejlen akkumuleres i løbet af
kørselen. En løsningen er at opdatere den absolutte vinkel og/eller position
ved brug af kendte landmærker. Da banen er lille og simpel, de to eneste forhindringer er modstander robotten og batteriskålen, kan robotten klare sig i
langt stykke ved brug af kun odometrien. Derudover er det kun når der skal
afleveres batterier at robotten skal bruge præcisionskørsel. Det gør det vinkelopdateringen mindre kritisk.
Det blev tidligere nævnt at robotten også er udstyret med en laserscanner.
Laserens tre funktioner er; dels at korrigere odometrien, at scanne banen for
affald og sidst finde batteriskålen i midten af banen.
2.3
Forhindringer
Det blev først overvejet om robotten skulle udstyres med en kofanger til dels
at formindske skaden ved påkørseler, men også til detektering af dem. Den
blev dog afvist til fordel af IR-afstandssensorer af flere grunde. Reglerne er
lidt uklare på det område og siger at robotten skal udvise vilje til at stoppe.
Det har vi valgt at tolke som at robotten skal reagere inden kontakt med modstanderen. Det vil selvsagt være umuligt med en kofanger da det er noget
sparsomt med plads i robotten. Derudover har IAU stortset standardiseret IRafstandssensorerne, hvilket gør det uhyggelig nemt at arbejde med dem uden
at skulle bruge for meget tid på dem.
K APITEL
Robot design - Mekanik
Robotten fra 2006 er stort set bevaret intakt. Robotten blev skrællet ned til stort
set kun det overordnede skelet, vel og mærke uden at ødelægge nogen dele.
Skulle det ønskes kan den forholdsvis nemt bygges om til dens oprindelige
funktion.
3.1
Dåse og flaske mekanik
Robotten havde nogle arme der foldede sig ud efter start, da de ikke har nogen
praktisk betydning er de fjernet. Uden armene er der plads til forøge omkredsen ca. 5 cm inden start og yderligere ca. 10 cm efter start. Det er akkurat lige
nok til at kunne montere en bøjle som vist i figur 2.2 på side 5. Da bøjlen går
for langt ud i forhold til start kravene, er det nødvendigt at den kan folde sig
ud efter start. Det mekaniske simpleste er at bøjlen rotérbar omkring den akse
hvor den sidder fast på robotten, og sætte en kile i klemme. Kilen er sat fast
til startsnoren, og vil løsne bøjlen når snoren trækkes. Selvom det betyder at
bøjlen sidder løst under kampen har det vist sig at det ikke betyder noget for
dens funktion.
Da der sidder en børstemotor på robotten i forvejen, som man kan bruge
til afleveringen af flasker og dåser, har det været et spørgsmål om at indstille
bøjlen så den virker optimalt. Se billedet i figur 3.1.
Figur 3.1: Dåse- og flaske indsamling, og aflevering med børstemotoren
9
3
10
3.2
KAPITEL 3. ROBOT DESIGN - MEKANIK
Batterihåndtering
Der akkurat var plads til installere batteritragten i bunden af robotten uden at
bore eller fræse i robotten.
Det blev indset den første løsningen ikke var en reel mulighed, grundet
pladsmangel i robotten. Løsning nummer to blev afprøvet med forskellige typer arme for at se hvilken der ville være den bedste. Hvis armen kun skulle
bestå af en lige stang, som i vores mock-up, ville den stikke langt ud fra robotten. Derfor eksperimenterede vi med arme der havde flere led. Den bedste var
en to-delt arm, forbundet med en fjeder for at efterligne den samme lineære
bevægelse som stangen. Resultatet kan ses på billedet i figur 3.2. Som det kan
Figur 3.2: Batterimekanismen
ses blev den endelige konstruktion noget anderledes end den originale idé
der er illustreret på figur 2.3(b). Løsningen blev modificeret så en servomotor
kunne bruges som drivkraft.
På billedet kan det også ses at for enden af batteribeholderen er der en
roterende låge. Lågen er styret af en servomotor.
3.3
Laserscanner
Da den originale robot fra Eurobot 2006 kørte med samme type laserscanner
er der allerede lavet en beskyttende holder til laseren. Det mindst element
på banen er et type D batteri med dens diameter på 3,2 cm. Højden til selve
laserhovedet er ca. 2,3 cm over banen, hvilket er lavt nok til at se alle elementer
på banen.
K APITEL
Robot design - Elektronik
Udover det mekaniske er der også taget udgangspunkt i Eurobot 2006 robotten med henblik på elektronikken. Systemet er opbygget omkring et mini-ITX
bundkort tilsluttet IAUs SMR komponenter.
4.1
SMR komponenter
Fordelen ved SMR komponenterne er at de er let tilgængelige, gennemtestet
og stabile. I robotten er der blevet brugt følgende SMR-komponenter:
Strømforsyning der leverer 12 V til alle SMR-modulerne og motorerne.
Bus-converter der forbinder bundkortet og SMR-modulerne ved at konvertere signalerne mellem de to forskellige seriel protokoller RS-485 ⇐⇒
RS-232 ??.
Motorcontroller ×2, en til hvert hjul. Udover at styre hjulene giver motorcontrollerne også information om hvert hjuls position som bruges til odometri, og til at se om startsnoren er trukket.
Linjesensor-kortet består blandt andet af otte fotoceller og ni lysdioder, der
alle sidder på linje. Modulet bruges normalt til at finde og f.eks. følge
linjer, men på robotten bruger vi det som en grænseflade for lysdioder
og fotoceller.
IR-sensor-kortet er en grænseflade mellem IR-sensorerne og SMR-bussen.
Servo-kortet styrer servomotorerne på robotten. Kortet blev lavet til Eurobot
2005 [SJHJ05] og blev revideret til Eurobot 2006 [RTJH06].
“kick”-kortet er lavet til en fodboldspillende robot [VCPSKT06]. Robotten
bruger kortet til at styre børstemotoren via servokortet.
laserscanneren er forbundet direkte til bundkortet via USB.
webkameraet er ligesom laserscanneren forbundet via USB til bundkortet.
En oversigt over forbindelserne kan ses på illustrationen i figur 4.1. Alle SMRkomponenterne er brugt i deres originale tilstand på nær linjesensorkortet,
der stammer fra Eurobot 2006 robotten. Alle lysdioder og fotoceller er blevet
erstattet med stik så de kan placeres hvor det er passende. Det har betydet at
forbindelserne til fotocellerne og dioderne ikke har været lige så stabil, som
hvis de var loddet direkte på printet.
11
4
12
KAPITEL 4. ROBOT DESIGN - ELEKTRONIK
Figur 4.1: Elektroniske forbindelser mellem de forskellige komponenter.
4.2
Startsnor
I følge reglerne skal robotten startes på afstand ved at hive i en snor. Robotten havde i forvejen et startsnor-system og det viste sig at være meget stabilt
under Eurobot 2006 [RTJH06], derfor blev det besluttet at beholde systemet.
Selve kablet består af en snor bundet til et RCA-stik. Stikket kortslutter to ben
på venstre motorcontroller hvilket sætter status-bit 7 høj.
4.3
Børstemotor
Til at aflevere dåserne og flasker bruges der en børste der er sat til en DCmotor. Modsat Eurobot 2006 robotten skal motoren køre den modsatte vej
rundt og skal kunne tændes og slukkes fra softwaren.
For at kunne styre motoren er der blevet brugt et print fra en fodboldspillende robot [VCPSKT06]. Printet omsætter et lavspændingssignal fra f.eks.
en mikroprocessor til en spænding der er stor nok til at drive en DC-motor.
Selve styre signalet kommer fra et ledigt ben på mikroprocessoren der styrer
servomotorerne, så for at kunne tænde og slukke for børstemotoren skal ben
PORTA7 sættes henholdsvis høj og lav.
4.4. LASERSCANNER
4.4
13
Laserscanner
Robotten er udstyret med en URG-04LX USB laserscanner fra Hokuyo. Med
en præcision på ±10mm og en opløsning på 0.36◦ er det en højpræcisions
og højopløsnings laser. Da der på IAU er der allerede udviklet software der
understøtter denne type laser, og da IAU har gode erfaringer med laseren
[RTJH06] faldt valget på denne laserscanner. Laserscanneren falder under kategorien “Laser Safety Class 1” og dermed opfylder den Eurobot 2007 reglerne
(se bilag A) vedrørende styrken på lasere der må tages i brug.
Da den samme laserscanner var brugt i robotten fra Eurobot 2006 skulle
der ikke laves nogle modifikationer til robotten for at få laseren til at virke.
4.5
Farve-sensorer
Designet kræver at robotten kan kende forskel på dels egne og modstanderens
batterier, og dels flasker og dåser.
Batterier
Da den eneste forskel på batterierne er deres farve kræver det at robotten har
en form for syn. Der blev valgt en simpel løsningen bestående af en lysdiode og to fotoceller. Dioden og fotocellerne kan kobles direkte til linesensorkomponenten. Som det kan ses på illustrationen i figur 4.2 er de to fotoceller
Figur 4.2: Farvesensor.
monteret overfor hinanden og lysdioden sidder ved siden af den ene. Ideen
er at den ene sensor skal kunne detektere en farveændring og den anden skal
kunne se om der ligger et batteri eller ej. Når der kommer et batteri ind i tragten vil batteriet afbryde lyset fra dioden til fotocellen på den anden side, og
dermed vil den virke som en batterisensor. På grund af at det røde batteri reflekterer den røde lysdiodes lys i lang højere grad end det blå batteri, kan den
fotocelle se røde batterier. Det viser sig at det blå batteri slet ikke giver udslag.
Et rødt batteri vil give udslag på begge fotoceller hvorimod et blåt batteri kun
vil kunne ses på at lyset bliver afbrudt til den ene fotocelle. På den baggrund
kan vi finde ud af om det er et rødt eller blåt batteri der ligger i tragten ved
følgende boolsk-algebra:
Blå = F1 F2
Rød = F1 F2
14
hvor F1|2 er defineret ved grænseværdier. Grænseværdierne afhænger af kalibreringen, men som det kan ses på graferne i figur 4.3 er det tydeligt at der
er tale om henholdsvis intet batteri, et blåt batteri og et rødt batteri.
(a) Blåt batteri i tragt
(b) Rødt batteri i tragt
Figur 4.3: Batteri test.
Flasker og dåser
Da flaskerne og dåserne ligger foran robotten under transporten, er det nødvendigt at kunne skelne mellem dem for at kunne putte dem i de korrekte
kurve. Der kan ikke bruges samme metode med lysdiode og fotocelle som ved
batteri farve-detektering, der er alt for store udsving på sensoren på grund
af lysforhold og afstanden. Istedet bruges der et standard USB webkamera.
IAUs erfaringer [RTJH06, SJHJ05] er at det generelt er for ustabilt at bruge kamerabaseret navigering på grund af lysforholdene under konkurrencen. Da
kameraet ikke skal bruge til navigering men til at se farveforskelle er det ikke
et lige så stort problem. For at undgå eventuelle problemer med lys udsendt i
det infrarøde spektrum, blev der til kameraet valgt en optik med et infrarødt
filter. På figur 4.4 er vist det påmonterede webkamera.
4.6
Batteri-sensor
Som nævnt i kapitel 2 under Batterihåndtering kan det hænde at batterierne
sætter sig fast på tværs i tragten. For at få batteriet fri kræver det at robotten
udfører en specifik manøvre, hvilket kræver at robotten skal vide hvornår et
batteri sidder fast i tragten.
Den første løsning byggede på samme princip som bliver brugt til at se om
der ligger et batteri for enden af tragten; en lysdiode lyser fra den ene side over
på en fotocelle i den modsatte side af tragten. Når et batteri kommer forbi eller
sætter sig fast vil lyset blive afbrudt, og dermed blive opdaget. Det har dog
vist sig at være omstændigt at få metoden til at virke pålidelig i længere tid ad
gangen. Fejlen er i stor grad blevet tildelt lysforholdene i tragtens udmunding,
andelen af lys udefra er langt større end andelen af lys fra dioden.
I stedet blev en IR-afstandssensor påsat op over tragtens indsnævring, se
billedet på figur 4.5. Da afstanden til banen er stort set konstant er blot at
4.6. BATTERI-SENSOR
Figur 4.4: Webkameraet ses her fast gjort med gule strips
Figur 4.5: IR-Sensoren sidder over tragten, i det kritiskeområde
15
16
kigge efter en afstandsændring. Løsningen med IR-afstandssensoren har vist
sig mere robust end den først tiltænkte fotocelle/lysdiode løsning.
4.7
Detektering af modstander
På IAU bruger SMR’erne som standard infrarøde afstandssensorer til at se
forhindringer med. Derfor har det været oplagt at bruge disse sensorer da de
har vist sig effektive og nemme at arbejde med. Som på SMR’erne er der blevet
påsat tre IR-afstandssensorer i fronten af robotten, de kan ses på billedet i figur
4.6.
Figur 4.6: I toppen kan de tre IR-afstandsensorer ses.
På billedet kan det ses at alle tre IR-sensorer er sat til at pege fremad og
ikke ud til siderne. Da sensorerne nogle gang kan blive snydt af refleksioner
og farver, er det derfor mest sikkert at kræve at to sensorer samtidigt kan se
modstander robotten. Det kan kun lade sig gøre fordi robotterne skal være
tilpas store ifølge reglerne, så de ikke kan forveksles med andre objekter på
banen.
Stolperne i målhjørnerne og midtbanden er dækket af reflektivt tape. Det
har de ulemper at de infrarødesensorer dels opfatter afstanden til refleksen
som meget kortere end den egentlig er, og dels får sensorerne meget nemt øje
på refleksen. Praktisk talt er det umuligt at have sensorerne til at pege lige
ud da IR-sensorerne hele tiden vil tro at modstanderen er nær, selvom det
er en refleks robotten har fået øje på. Problemet er blevet løst delvist ved at
dreje IR-sensorerne ned mod jorden og derved indskrænke deres synsfelt, og
delvist i softwaren. Når robotten kommer tættere på stolperne vil refleksen,
og stolperne for den sags skyld, komme ind i synsfeltet og kan fejlagtigt blive
opfattet som modstanderen. Men når robotten er så tæt på kanten kan det helt
udelukkes at der er en modstander i mellem robotten og kanten, det vil sige
IR-sensorerne kan ignoreres indtil robotten igen drejer væk fra kanten.
K APITEL
Software
Computeren på robotten kører en specielt tilpasset version af GNU/Linux for
at fungere på IAUs SMR’er. Til at kommunikere med SMR-modulerne har IAU skrevet programmet SMRdemo. I den sammenhæng har de også lavet et
script-sprog kaldet SMR-CL, se bilag B for vejledning, til at styre robotten med
simple kommandoer som forward 1, der kører robotten 1 meter frem. SMRCL er dog ikke særligt avanceret sprog, jvf. bilag B, og det er derfor mere praktisk at bruge et programmeringssprog som C til mere elegante og omfattende
programmer. Da SMRdemo kan køre i server tilstand, og da alle de basale
funktioner er implementeret, blev det besluttet at kode hovedprogrammet i
C og lade det forbinde sig til SMRdemo serveren. Kommunikationen mellem
C programmet og SMRdemo serveren foregår ved hjælp af nogle funktioner
skrevet i C kode (bilag C.2) til Eurobot 2005 [SJHJ05], der brugte samme program opdeling. Hele kildekoden kan ses i bilag C.
Figur 5.1: Hardware/software oversigt
Følgende funktioner er skrevet i haupt.c, bilag C.1, med mindre andet er
skrevet.
17
5
18
5.1
KAPITEL 5. SOFTWARE
SMRdemo server
Når SMRdemo kører i server tilstand opfører programmet sig en smule anderledes end ved normal brug. Når en ny kommando modtages tildeles den
et unikt ID, som er et fortløbende nummer, og den lægges bagerst i kommandokøen. Når en kommando er udført sender SMRdemo svar tilbage med det
tilsvarende ID og en stopbetingelse. Stopbetingelsen kan enten være at kommandoen blev udført, eller også er det en brugerdefineret stopbetingelse. Klienten kan angive en tidshorisont når der forspørges om nye begivenheder,
events. Hvis der ikke er nogen stopbetingelser indenfor tidshorisonten sender serveren et event timeout så klienten ved at der er gået for lang tid.
5.2
Start & slut funktioner
void initialize(); linje 250 – 258
Funktionen sender udelukkende SMR-CL kommandoer til SMRdemo. Først
bliver SMRdemos logfunktion sat til at logge odometri-, linesensor- og irsensordata. Dernæst bliver alle servomotorerne sat til start position og børstemotoren stoppet.
void finalize(); linje 250 – 258
Funktionen sender SMR-CL kommandoer der sætter servomotorerne til den
ønskede slut position. Sluk derudover for dels motorerne og dels for børstemotoren.
void waitForCable(); linje 284 – 316
Først tjekker funktionen om startkablet sidder i stikket, ved hjælp af en løkke.
Når det bliver detekteret at startkablet er isat går funktionen videre til næste
løkke hvor der bliver tjekket for om startkablet er hevet ud. I begge løkker
bliver der taget gennemsnittet af værdien $motorstatusr for at sikre sig
at der ikke er tale om en strøm/bit-spike et sted i systemet. Til sidst starter
funktionen den nedtællings alarm der er påkrævet af reglerne, på 90 sekunder.
Alarmen er sat til kalde catchAlarm() som et SIGALRM signal, når den er
udløbet.
void catchAlam(int sig); linje 273 – 279
sig er signaltypen, her vil den dog altid være SIGALRM.
Når denne funktion bliver kaldt betyder det at kampen er ovre, derfor kaldes finalize() for at slukke for alle bevægende dele på robotten. Derefter
bliver forbindelsen til SMRdemo afsluttet og programmet afsluttes.
5.3
Fejlhåndtering
SMRdemo venter på at SMR-CL kommandoerne er udført før næste kommando bliver udført. Det kan dog gå galt når der køres på grund af præcisionen
eller udvendige begivenheder, f.eks. hvis robotten bliver bedt om at køre 0.005
5.4. BASALE KØRSELSFUNKTIONER
19
cm så rykker den sig ikke ud af stedet, og da den tjekker odometrien for at se
om der nogensinde blev kørt den korte afstand så viser odometrien at robotten
ikke har flyttet sig og programmet går en uendelig løkke. Hvis først robotten
er sat til at køre f.eks. 2 meter frem, så er det også vigtigt at den stopper hvis
den ser modstanderen således at programmet kan reagere på forhindringen.
Hvor det første eksempel kan blive fanget af et event timeout, så kræver
det andet eksempel at der bliver indsat stopbetingelser for at tage højde for at
programmerne kører asynkront.
Hvis robotten kører ind i noget eller hvis hjulene bliver blokeret på anden måde, sender serveren i stedet for motioncontrol blocked. SMRdemo tillader ikke robotten at køre videre før den har modtaget kommandoen resetmotors. For at rydde kommandokøen kan der sendes flushcmds.
På grund af måden SMRdemo virker internt er det nødvendigt også at kalde
targethere for at robotten ikke tror at den er et andet sted end den er.
void getID(id); linje 771 – 776
Som skrevet tildeler SMRdemo, i server tilstand, hver modtaget kommando
med et unikt nummer. Dette nummer bliver returneret når en kommando er
modtaget. For at gemme dette id i variablen id, kaldes getID(id) lige umiddelbart efter der er sendt en SMR-CL kommando.
int isStopped(id, stopcond); linje 759 – 766
Parameteren id kan fåes ved brug af getID(). Derimod afhænger stopcond
parameteren af rækkefølgen på de stopbetingelser afgivet af kommandoen,
som f.eks. ved forward(), hvor netop stopbetingelsen for detektering af
modstanderen er 2.000000 da det er betingelse nummer 2.
Funktionen returnerer 1 når eventet indholder både id og stopcond, ellers
bliver 0 returneret.
int checkStopConditions(id); linje 1445 – 1503
Parameteren id er det unikke SMR kommando id getID() gemmer.
Funktionen er basalt en løkke der først afbrydes når der er modtaget et
nyt event fra SMRdemo serveren der matcher en af de typer der testes for
i løkken. Der bliver først testet for om motorerne er blokerede, dernæst om
modstanderen er at få øje på, så testes der om der sidder et batteri fast i tragten, dernæst testes der for om kommandoen er udført og sidst testes der om
det er et event timeout.
Funktionen returnerer et enum af typen event der kan have følgende værdier: cmd_complete, motors_blocked, battery_stuck, opponent_detected
eller event_timeout.
5.4
Basale kørselsfunktioner
Hjælpe funktionerne forward(), turn() og turnr() blev introduceret for
at øge læsbarheden af koden, og for at gøre den mere struktureret ved at samle
stopbetingelserne et sted.
20
KAPITEL 5. SOFTWARE
void forward(dist, speed); linje 406 – 410
Parametrene dist og speed svarer til SMR-CLs, se bilag B. Derudover bliver
der også sat to stopbetingelser. Den første betingelse er til at håndtere hvis IRsensoren i tragten spotter et batteri. Den anden stopbetingelse ser om mindst
to af de tre front IR-sensorer ser en modstander tæt på. Grænseværdierne for
hver af IR-sensorerne er givet ved konstanterne IRBAT_THR, IRLEFT_THR,
IRMIDDLE_THR og IRRIGHT_THR, hvor den første grænseværdi er for IRsensoren i batteritragten.
void turn(angle, using_rad, speed); linje 415 – 420
Parametrene angle og speed svarer til SMR-CLs, se bilag B. Parameteren
using_rad bruges som en boolsk variable for at tjekke om vinklen er angivet
i radianer eller i grader.
void turnr(radius, angle, using_rad, speed); linje 425
– 430
Parametrene radius, angle og speed svarer til SMR-CLs, se bilag B. Parameteren using_rad bruges som en boolsk variable for at tjekke om vinklen
er angivet i radianer eller i grader.
void getPosition(); linje 1157 – 1164
Funktionen gemmer robottens odometri position og vinkel i en datastruktur
i C programmet. Fra SMRdemo bliver variablene $odox, $odoy og $odoth
gemt i curLoc, en global struct der indeholder koordinaterne x, y og θ.
5.5
Kamera
Robotsyn i form af laser og kamera har været og er til stadighed under udvikling af Christian Andersen ved IAU, Ørsted•DTU. Der er udviklet dels en
laser-server (ulmsserver) og dels en kamera-server (ucamserver). Begge
servere er ment til brug sammen med en TCP/IP klient som f.eks. telnet eller den klient der også er lavet som et eksempel (uclient). I SMRdemo er
serverne delvist understøttede, det vil sige at kunne nogle funktioner giver
returværdier som SMRdemo kan forstå. For at sende en kommando til lasereller kamera-serveren bruges henholdsvis SMR-CL kommandoerne laser og
vision.
Kameraet skal bruges til først at se om robotten har fanget noget affald,
og dels skal det bruges til at skelne om affaldet er en dåse eller en flaske. Den
funktionalitet er implementeret i plugin-modulet myfunc, et eksempel plugin
der følger med aurobotservers. Koden følger samme struktur som resten
af plugin-modulet hvilket er i form af hjælpefunktioner der klarer fortolkning
af kommandoen, imens selve udregningen klares i en anden funktion. Her
gennemgåes kun selve udregningen da hjælpefunktionen er godt dokumenteret i bilag C.4. Fra klienten kan funktionen kaldes med kommandoen BOC.
5.5. KAMERA
21
calcBottleOrCan(); myfunc.cpp linje 565 – 719
I figur 5.2(a) er vist det billede som kameraet ser, læg mærke til at bøjlen er
slået ned. For ikke at komme til at detektere affald der er udenfor bøjlen el-
(a) Ubearbejdet udsyn
(b) Udsyn med ROI
Figur 5.2: Robottens udsyn.
ler detektere noget inden i robotten, bliver der som standard kun kigget på
arealet fra bøjlen og ind til børsten. Et sådan område kaldes også et Range Of
Interest eller ROI. ROI området kan dog redefineres med areal-parametrene
w og h, og med offset-parametrene x og y. På billedet i figur 5.2(b) er ROI
markeret med en rød firkant.
Dåserne og flaskerne hver deres farve henholdsvis gul og grøn. Farveintensiteterne af en flaske og en dåse i både RGB og i YUV domæne er vist i figur
5.3. Det ses at det er nemmest at skelne mellem den gule og den grønne farve
i YUV diagrammet og derfor vil YUV formatet blive brugt til udregningerne.
Hver pixel indenfor ROI bliver tjekket for om både U og V koordinaterne
er indenfor et tolerance område defineret af parametrene thresholdCanU,
thresholdCanV, thresholdBottleU og thresholdBottleV. Hvis en pixel
er indenfor en af de to områder bliver den talt med som enten en dåse eller en
flaske pixel. Når alle pixel indenfor ROI er gennemløbet bliver sandsynligheden for om der er en dåse og/eller en flaske tilstede, udregnet som
P =
optalt antal pixel
,
tolerance antal
hvor tolerance antal er givet af parametrene areaCan og areaBottle. De
udregnede sandsynligheder for fem forskellige tilfælde kan ses i tabel 5.1. De
syv tilfælde er når der ikke ligger noget affald foran robotten, når robotten har
fanget henholdsvis en dåse og en flaske, når en flaske eller dåse står vinkelret
på robotten, og de sidste to tilfælde er når der ligger henholdsvis en dåse
eller en flaske lige foran bøjlen. Som det fremgår af tabellen kan der altså med
rimelighed afgøres hvornår der ligger en flaske og hvornår der ligger en dåse
indenfor bøjlen.
For grafisk at kunne analysere hvad der bliver detekteret bliver hver pixel
farvet i forskellige farve alt om det er en flaske eller en dåse pixel. Ydermere
bliver Center of Gravity også markeret. Eksempler kan ses på billederne i figur
5.4.
For at SMRdemo kan forstå resultatet af SMR-CL kommandoen vision
skal svaret være i formatet
22
KAPITEL 5. SOFTWARE
(a) UV-diagram for dåse
(b) RGB-diagram for dåse
(c) UV-diagram for flaske
(d) RGB-diagram for flaske
Figur 5.3: Farvespektra for en dåse og en flaske, taget med funktionerne UV og
CHROMA i uclient.
<vision vis0="værdi0" . . . vis9="værdi9"/>
De to eneste værdier robotten skal bruge for er sandsynligheden for om der
ligger en flaske eller/og en dåse. Derfor bliver tekststrengen
<vision vis0="pCan" vis1="pBottle"/>\n",
hvor pCan og pBottle bliver erstattet med de faktiske værdier, sendt til SMRdemo. I SMRdemo kan værdierne læses ud med $vis0 og $vis1.
void hasItem(); linje 882 – 909
Funktionen sender følgende SMR-CL kommando til SMRdemo for at få input
fra webkameraet:
5.6. DÅSE OG FLASKE AFLEVERING
Intet affald
Dåse, fanget
Flaske, fanget
Dåse, vinkelret
Flaske, vinkelret
Dåse, foran bøjlen
Flaske, foran bøjlen
23
pCan
0.000
0.992
0.002
0.520
0.004
0.506
0.002
pBottle
0.000
0.000
1.642
0.000
0.000
0.000
0.009
Tabel 5.1: Udregnede sandsynligheder for at der ligger en dåse eller en flaske,
med standard toleranceværdier.
(a) Dåse
(b) Flaske
Figur 5.4: Et par eksempler med BOC kommandoen.
vision "BOC thresholdCanU=CanU_THR
thresholdCanV=CanV_THR
thresholdBottleU=BottleU_THR
thresholdBottleV=BottleV_THR"
, hvor CanU_THR, CanV_THR, BottleU_THR og BottleV_THR er konstanter.
Som alle andre sensorer bliver kommandoen kaldt seks gange og resultatet
midlet således at usikkerheden og fejl minimeres. Gennemsnittene avg_pCan
og avg_pBottle tjekkes derefter om de er større end tolerance værdierne PROB_CAN
og PROB_BOTTLE. Er det tilfældet, antages det at affaldet er af den type der
har den største sandsynlighed. Hvis begge værdier er under tolerance konstanterne antages at det der ikke er fanget noget affald.
Funktionen sætter den globalt defineret enum variabel item til enten can,
bottle eller none hvis der ikke er fanget noget affald.
5.6
Dåse og flaske aflevering
Afleveringen af flasker og dåser består af to funktioner: toBasket() og deliver().
24
KAPITEL 5. SOFTWARE
toBasket(); linje 913 – 977
Når en flaske eller dåse er indfanget med collectItem()1 , bliver toBasket()
kaldt umiddelbart efter. Overordnet set kører funktionen robotten på plads
foran den affaldskurv hvor det indfangede affald skal afleveres. Det gøres
ved at kalde gotoPosition(bin);2 der får robotten til at køre hen til den
korrekte kurv og vende sig mod den, derefter køres der 15 cm tætter på.
Til at undersøge hvilken stopbetingelse der har stoppet robotten bruges
checkStopConditions(id) som tidligere beskrevet. Begivenhederne deles op i tre tilfælde; blokerede motorer, kommando udført, og andre. Hvis
kommandoen er udført afsluttes funktionen ved at sætte den globale variabel
toBasket_done til 1. Hvis motorerne er blokerede bakker robotten 10 cm og
prøver derefter igen. I de andre tilfælde prøver den at sende gotoPosition
kommandoen igen.
void deliver(); linje 982 – 1039
Funktionen er ment at blive kaldt efter toBasket() for at sikre at robotten
er tæt ved affaldskurvene.
Når kaldt, vil funktionen sende kommandoen drive til SMRdemo der
får robotten til at køre ligeud uden at stoppe. Det vil sige at den eneste måde den kan stoppe på er ved at motorerne bliver blokeret eller ved at stoppe
den direkte ved at modtage et event_timeout. Når motorerne bliver blokeret antages det derfor at robotten har ramt kanten eller noget affald der
ligger i vejen. Da børstemotoren kan skyde affaldet over noget affald der eventuelt skulle ligge i vejen, er dette intet problem. Robotten bliver derefter
sat til at bakke 2 cm, det er nødvendigt for at kunne starte børstemotoren uden at ligge pres på affaldet. Først når børstemotoren er oppe i omdrejninger
kan der ligges pres på. Når robotten har bakket bliver børstemotoren tændt
med SMR-CL kommandoen rs485send 10 8. Robotten kører derefter 2 cm
frem, for at lægge pres på affaldet, venter et sekund og slukker børstemotoren
med rs485send 10 9. Nu skulle affaldet gerne være afleveret og robotten
bakker 10 cm, for gøre sig fri fra kanten så den kan dreje.
For at sikre sig at affaldet rent faktisk er blevet afleveret eller at robotten
ikke har fanget mere affald af samme type, kaldes funktionen hasItem().
Hvis det viser sig at robotten stadig har noget affald af samme type som den
lige har afleveret, forsøger den forfra med at aflevere det.
Når funktionen er færdig bliver den globale variabel deliver_done sat til 1.
Børstemotor
I kapitel 4.3 blev det nævnt at for at kunne styre børstemotoren, skulle ben
PORTA7 sættes høj og lav i servo-mikroprocessoren.
For at kunne tale med SMRdemo er der i stil med resten af Communication.c
(bilag C.7) blevet indført to nye flags; kick_dc_on_f lag og kick_dc_of f _f lag.
De er blevet tilføjet som henholdsvis kommando 8 og 9, hvilket vil sige at de
fra SMRdemo kan kaldes med kommandoen rs485send 10 x, hvor x er
kommando nummeret. De to flags aktiverer henholdsvis kick_dc_on() og
1 Beskrevet
2 Beskrevet
af Brian Hansen (s042307) [Han07]
af Shayantharan Sivarajalingam (s042297), ultimo juni 2007
5.6. DÅSE OG FLASKE AFLEVERING
25
kick_dc_off() i Servostyring.c (bilag C.9). Funktionen kick_dc_on()
sætter således blot ben PORTA7 høj, og kick_dc_off() sætter benet lav.
K APITEL
Designevaluering
Dette kapitel vil handle om det endelige robotdesign set i perspektiv til de
konkurrerende holds robotter der deltog i Eurobot 2007 konkurrencen. Da det
er en konkurrence har holdene holdt tæt med deres design ideer, de eneste
ledestråde har været gennem de spørgsmål folk har stilet i de officielle F.A.Q.
(se bilag A).
6.1
Designkoncepter
Hvor vores robot er designet med henblik på at samle alle tre typer affald op,
var det ikke normen blandt de deltagende hold. De fleste hold koncentrerede
sig kun om dåser og batteri, og en del udelukkende om enten dåser eller batterier. Tendensen var at de hold der gik efter flasker også gik efter både dåser
og batterier.
Dåser & flasker
Som man kan forvente af sådan en konkurrence gik mange af designprincipperne igen. Vores koncept med at indfange affaldet med en bøjle blev dog ikke
set brugt af noget andet hold, så der kan ikke blive nogle direkte sammenligning. Til gengæld var det udbredt at køre op mod kanterne for at få affaldet
ind i robotten, selv de par af de robotter der endte i finale runderne brugte
denne løsning. En anden udbredt metode at få affaldet ind i robotten på var
at bruge et system af børster, som i en vaske hal, der til tider kunne præstere
at skyde affaldet ind i robotten. En sådan robot er vist på figur 6.1. En af de
mest udbredte metoder til indsamlingen af dåserne og flaskerne var at bruge
en roterende skovl eller børste, se billerne i figur 6.2. Nogle af dem kunne
detektere når en flaske prøvede at komme op eller sad fast, og kunne derefter
give slip på den igen. Det var de samme hold der kun gik efter dåser og ikke
flasker. De hold der prøvede at få flasker op med denne metode endte ofte
deres kampe med at en flaske sad fast. Omkring halvdelen af de robotter der
brugte denne metode var nødsaget til at køre op mod kanten for at få dåsen
eller flasken helt ind, hvorimod den anden halvdel klarede sig fint uden.
En lige så udbredt metode til at aflevering af dåserne og flaskerne var at
skyde dem af sted på tværs af banen. Den åbenlyse fordel ved denne metode
er at robotten sparer den tid det vil tage at køre frem og tilbage til affaldskurven. I praksis viste det sig ofte at være en dårlig løsning, i hvert fald var
der mange af robotterne der kun ramte kurvene hver anden eller hver tredje
gang. Der var nogle robotter der ramte modstander robotten, hvilket kunne
have været fatalt for nogle af de medvirkende robotter, og nogle få skød så
27
6
28
KAPITEL 6. DESIGNEVALUERING
Figur 6.1: Club Elek der endte på en 3. plads.
Figur 6.2: Felbot benyttede sig af et møllehjul.
6.1. DESIGNKONCEPTER
29
meget ved siden af kurvene at de kunne have ramt tilskuerne. Kun få at de
skydende robotter havde problemer med selve skydemekanismen.
Batterier
Magneter var den klart mest fortrukne metode at samle batterierne op med.
Den mest innovative batteri opsamlings løsning må have været det engelske
holds løsning. De havde monteret en roterende magnet, og en roterende pind.
Når batteriet blev fanget af magneten drejede den rundt så batteriet kom højere op i robotten, hvorefter pinden ville skubbe batteriet af magneten. Ikke
nok med at denne løsning var effektiv, var den også betydelig mere kompakt
og mere simpel end den løsning der er brugt i dette projekt. En anden metode
var at bruge den roterende skovl, som robotterne også brugte til dåse og flaske opsamlingen, til at skovle batterierne op. Det var dog ikke alle der havde
kraft og præcision nok til at kunne få batterierne op med skovlen.
Blandt de hold der indsamlede batterier var det omkring halvdelen, der
som i vores robot, gemte batterierne inden i robotten og afleverede dem senere, og halvdelen der afleverede batteriet med det samme.
De specielle
Der var to robotter der skilte sig ud fra mængden, navnlig det franske hold
der vandt Eurobot 2007 RCVA, og den spanske Ackrobot der fik designprisen.
RCVAs robot var udstyret med to identiske arme, en på hver side, der hver
havde en sugekop for enden. Robotten kunne således tage en flaske eller dåse
og gemme den i toppen af robotten hvor der i alt var plads til fire flasker
eller dåser, to på hver side. Armene blev også brugt til at aflevere affaldet i
Figur 6.3: RCVA, vinderen af Eurobot 2007 i færd med at tage en flaske.
kurvene. På figur 6.3 er der et billede af den vindende robot. RCVA klarede
sig ofte uden problemer, men i finalerunden hvor den virkelig blev udfordret
var det tydeligt at der kunne være problemer med opsamlingen af flasker og
30
dåser når de trillede idet sugekoppen prøvede at få fat. Havde sugekoppen
først fået fat var det stykke affald så godt som afleveret, det gik sjældent galt.
Vinderen af designprisen, Ackrobot, brugte også en robot arm. Forskellen fra RCVAs robotarme var at Ackrobots robotarm havde en langt større
frihedsgrad og at det er en såkaldt portalrobot. På billedet i figur 6.4 ses Ackrobot. Trods Ackrobots avanceret robotarm og objekt genkendelse software
Figur 6.4: Acrobot og sin avanceret robotarm
endte den på en delt 25. plads. Når den virkede, gik det virkelig hurtigt med
at få afleveret affaldet. Problemet så dog ud til at være med objektgenkendelsen og ikke selve robotarmen.
6.2
Mekanik
Konkurrencen bar præg af en generel høj mekanisk indsigt. De fleste robotter benyttede sig af smarte og til tider meget avanceret mekanik. Mange af de
mekanisk virkelig smarte robotter endte i finale runderne, men der var også
undtagelser som f.eks. med Ackrobot der aldrig nåede så langt. På den anden
side var der også mange robotter der lignede noget hastværk, og som ikke
var særligt robuste. Robotterne µ der endte på en 9. plads og MIBE der endte på en 7. plads, var begge mekaniske simple og konstruktionerne så noget
tvivlsomme ud.
6.3
Elektronik
Hvor de fleste hold var mekaniske stærke, var det de færreste der ikke havde
elektroniske problemer. Der er kun en ting der er værre end at sidde og debugge kode kort for en kamp, det er at debugge elektriske kredsløb. Her viste
SMR-komponenterne deres værd ved i kraft af at være gennemtestet og utrolige stabile. Ligeledes kunne det ses at hold der havde deltaget i flere år og
genbrugte komponenterne klarede sig generelt bedre end deres modstandere.
6.4. AUTONOMITET
31
F.eks. kan det nævnes at alle i top tre har genbrugt mange komponenter fra
de tidligere år.
De fleste robotter brugte mikrocontrollere til at styre robotten. Overfor et
PC-baseret system er der flere fordele:
• Hurtigere opstart, hvilket gør det nemmere at håndtere robotten under
konkurrencen
• Mindre strømforbrug, gør dels levetiden længere eller muliggør mindre
batterier.
• Mikrocontrollerne er fysik langt mindre end et PC-baseret system.
Et PC-baseret system skalerer dog langt bedre og er mere kraftfuld.
Det var udbredt at bruge både IR-sensorer, fotoceller, kameraer, sonarer,
støddæmpere og simple kontakter, der så ikke ud til at være en overvægt at
den ene eller den anden type. Blandt de mere eksotiske sensorer var laserscanneren, også vores robot også er udstyret med, og beacons. De beacon der
blev brugt var alt lige fra laser/spejl opsætning til radiobølger til infrarødt lys.
Næsten alle hold der brugte beacon havde problemer med dem på eller andet
tidspunkt i løbet af konkurrencen.
6.4
Autonomitet
Konkurrencen bygger på at robotterne er selvtænkende i en vis udstrækning.
Det blev dog klart under finalerunderne at man kunne langt uden at robotten
var helt autonom. Den før omtalte robot µ kørte i ring ved sin kurv for at lede
efter affald, men der var intet affald på dens side af banen. Det vil sige at den
kørte rundt i håb om at finde noget at samle op, men kunne ikke se at affaldet
lå længere væk.
Mere generelt kan det siges at konkurrencen sagtens kunne klares uden
at robotten var helt autonom. De hold vis robot var autonom klarede sig dog
generelt bedre end dem vis ikke gjorde, men der var også undtagelser. Det
kom til syne når kollisioner skulle undgås.
Antikollision
Det virkede som de fleste hold ikke havde brugt megen tid på Obstacle Avoidance, selvom det blev fremhævet meget tydeligt i F.A.Q. #3 (Se bilag A.4). Det
var alt lige fra detekteringen af modstanderen til håndteringen der gik galt.
Mange hold havde problemer med det reflekstape der sidder på stolperne,
det fik robotten til at fejlbedømme afstanden til modstanderen. Der kun er to
udfald ved fejlbedømmelser; enten tror robotten at den er tættere på end den
i virkeligheden er og prøver derfor at udføre sin undvigelsesmanøvre, eller så
tror robotten at modstanderen er længere væk og kører frontalt ind i den. Det
så ikke umiddelbart ud til at være betinget af hvilken type afstandssensor der
blev brugt, både blandt de hold de gik godt for og de hold det gik skidt for.
Halvdelen, hvis ikke flere, kampe endte med at robotterne så hinanden og
stoppede. Hvis ikke begge robotter stoppede helt op, så gjorde modstanderens
robot, det var næsten mere regelen end undtagelsen. Sammenholdt med det
faktum at en del robotter fejlbedømte afstanden til modstanderen og troede at
32
den var tættere på, betød at nogle robotter gik helt i stå selvom modstanderen
bevægede sig rundt omkring i det andet hjørne af banen.
K APITEL
Konklusion
Robotten der tidligere har medvirket i Eurobot 2006 er blevet ombygget således at den kan gennemføre Eurobot 2007. Ombygningen er foretaget i sådan
en grad at robotten let kan ombygges til det originale design. Robotten har
vist sit værd ved at være i stand til at indsamle, sortere og aflevere alle tre
typer affald.
Det stod klart fra starten at det ville være svært at kunne håndtere alle
typer affald da der er tale om relative store elementer i forhold robottens størrelse. Derudover voldte flaskerne de største problemer på grund af kraveudformningen omkring flaskehalsen, den har en tildens til at sætter sig fast.
Derudover bliver det vanskeliggjort at indsamle affaldet i fred da der under
konkurrence forholdene til tider er halv-destruktive modstandere. Efter at have deltaget i Frankrig stod det også klart at vi ikke var de eneste der havde
haft problemer med at affaldet. Faktisk var vi blandt den lille mængde hold
der kunne klare alle typer affald, og endnu med en endnu simplere løsning
end mange af de andre.
Ved det planlagte design1 var fordelene at flaskerne og dåserne kunne opsamles ude på banen, og dels at den ville kunne opbevare et par dåser eller
flasker så der kunne spares noget kørselstid. Efter konkurrencen vil jo dog
satse mere på det endelige design på baggrund af det simple men effektive
affalds-indsamling. Begge design bruger samme batteri indsamlings princip,
nemlig ved at rette batteriet op ved en tragt og derefter samle det op med
et batteri i en skål. I praksis virkede både tragten og opsamlingsmekanismen
efter planen.
Det lykkedes at bruge webkameraet til at genkende dåser og flasker, og gør
derfor sorteringen af dåser og flasker vellykket. Derudover er funktionerne
deliver() og toBasket() afprøvet og ser ud til at virke fejlfrit. Desværre
opstod der software problemer der har været svære at opspore og løse. Muligvis ligger problemet i funktionen checkStopConditions(), men det har
været svært at genskabe den samme fejl gang på gang.
1 Gennemgået
af Brian Hansen (s042307), Ørsted•DTU
33
7
Litteratur
[Div07]
Diverse. Automation’s wiki. http://www.iau.dtu.dk/
wiki, 2007. Hjemmeside per 18/6-2007.
[Han07]
Brian Hansen. Eurobot 2007 – planlagt design. Ørsted•DTU,
2007.
[HJJ07]
Brian Hansen og Martin Juhl Jørgensen. Forprojekt til eurobot
2007. Ørsted•DTU, 2007.
[RTJH06]
Martin Rødgaard, Peter Tjell og Claes Jæger-Hansen. Eurobot
2006. Ørsted•DTU, 2006.
[SJHJ05]
Søren Svane Jensen, Søren Hansen og Morten Jønsson. Eurobot
2005. Ørsted•DTU, 2005.
[VCPSKT06] Jørn Villesen Christensen, Mikkel Peter Stæhr og Michael Kølbæk Thomsen. Robots for robocup. Ørsted•DTU, 2006.
35
Appendiks
37
A PPENDIKS
Regler & FAQs
A.1
Regler
39
A
Eurobotopen 2007
Official Rules 2007
Waste, Sort & Win!
The robot which sorts the most waste into correct bins will be the winner.
1/27
Eurobotopen 2007
Official Rules 2007
1. Scope
The following game rules are applicable to all the national qualifications and the final of EUROBOT
2007 autonomous robot contest.
EUROBOT is a amateur robotics contest open to world-wide teams of young people, organised either in
student projects, in independent clubs, or in an educational project. A team is composed of several
people. . A team must be made up of two or more active participants. Team members may be up to
(including) 30 years old, each team may have one supervisor for which this age limit does not apply.
The contest aims at interesting the largest public to robotics and at encouraging the hands on, group
practice of science by young people. EUROBOT and its national qualifications are intended to take
place in a friendly and sporting spirit.
More than an engineering championship for young people, EUROBOT is a friendly pretext to free
technical imagination and exchange ideas, know-how, hints and engineering knowledge around a
common challenge. Creativity is at stake and interdisciplinarity requested. Technical and cultural
enrichment is the goal.
Participation to the competitions assumes full acceptation of these principles as well as the rules and
any interpretation of them that will be made by the refereeing committee (throughout the year) and
by the referees (during the matches). The referees’ decisions are final and may not be challenged,
unless an agreement is reached between all the parties involved.
EUROBOT is a contest taking place in Europe, but is open to other countries. The countries presenting
more than three teams shall organise a national qualification (or national cup), in order to select three
teams among them. The selection will typically include the two most competitive teams. But it is left
to each national organisation committee to agree on a possible alternative to competitiveness for
selecting its last team. For example, the third team can be chosen by a jury according to other
qualities valued by the contest as: best-concept, creativity, fair-play, etc.
EUROBOT values, fair play, solidarity, technical knowledge sharing and creativity both through
techniques and project management more than competition.
The happy selected teams during the national robotics cups of Algeria, Austria, Belgium, Czech
Republic, Germany, France, Italy, Roumania, Serbia, Spain, Switzerland, United Kingdom and any 2007
new selection will meet their international counterparts in May 2007 from Thursday 17th to Sunday
20th, in La Ferté-Bernard (France) to compete for the EUROBOT final.
EUROBOT is born in 1998, in the wake of the French Cup of Robotics, following the constitution of a
similar competition in Switzerland. Today, to stand with the international spreading of the contest,
and to structure the spirit of exchange and co-operation that exists between the different organisers,
all have gathered into a European association.
2/27
Eurobotopen 2007
Official Rules 2007
The association, officially born on May 2004, is named EUROBOT. You can find its statutes on our
website: www.eurobot.org . All individuals and structures sharing our values are very welcome, either
to support, or join as volunteers in the different organising groups.
It shall be noticed that most of the national qualification are open to foreign teams but in the limit of
their resources, and that many teams organise their own friendly tournaments. It shall be finally
noticed that multinational teams are welcome.
EUROBOT and its national selections are attended mostly by volunteers of all nationality who believe in
the educational value of the experience, and are for most former participants themselves.
Welcome!
And have a nice adventure!
3/27
Eurobotopen 2007
Official Rules 2007
1.1. Contents
1.
Scope .....................................................................................................................2
1.1.
2.
Contents ............................................................................................................... 4
Basic rules ...............................................................................................................6
2.1.
3.
Referees ............................................................................................................... 6
Playing area and element definition ...............................................................................6
3.1. General notes ......................................................................................................... 6
3.1.1. Tolerances ....................................................................................................... 6
3.2.
The playing area...................................................................................................... 7
3.3.
Starting Areas ......................................................................................................... 8
3.4.
Border .................................................................................................................. 8
3.5.
Beacon supports / Reflectors....................................................................................... 8
3.6. Waste / Placement of Waste ....................................................................................... 8
3.6.1. Aluminium cans ................................................................................................. 8
3.6.2. Plastic bottles ................................................................................................... 9
3.6.3. Batteries ......................................................................................................... 9
3.6.4. Placement of the waste ....................................................................................... 9
3.7. Bins and basket ..................................................................................................... 10
3.7.1. Bins.............................................................................................................. 10
3.7.2. Basket........................................................................................................... 10
4.
The robots ............................................................................................................. 10
4.1. General notes ....................................................................................................... 10
4.1.1. Visibility........................................................................................................ 11
4.2. Limitations and safety issues ..................................................................................... 11
4.2.1. Fair-play........................................................................................................ 11
4.2.2. Safety ........................................................................................................... 11
4.3. Mandatory equipment .............................................................................................
4.3.1. For starting the robot........................................................................................
4.3.2. Emergency OFF button.......................................................................................
4.3.3. Automatic shutdown .........................................................................................
4.3.4. Obstacle avoidance system..................................................................................
12
12
12
12
12
4.4.
Robot Dimensions .................................................................................................. 12
4.5.
Energy Sources...................................................................................................... 13
4.6.
Control Systems..................................................................................................... 14
4.7.
Robot localisation beacon support .............................................................................. 14
4/27
Eurobotopen 2007
Official Rules 2007
4.8.
5.
Technical poster.................................................................................................... 15
Beacons ................................................................................................................ 15
5.1.
General Comments ................................................................................................. 15
5.2.
Robot Localisation Beacon ........................................................................................ 17
5.3.
Fixed beacons ....................................................................................................... 17
5.4.
Communication signals ............................................................................................ 17
6.
Match timeline........................................................................................................ 18
6.1.
Robot identification................................................................................................ 18
6.2.
Start procedure ..................................................................................................... 18
6.3.
Match sequence..................................................................................................... 19
6.4. Score calculation ...................................................................................................
6.4.1. Points ...........................................................................................................
6.4.2. Penalties .......................................................................................................
6.4.3. Scratching ......................................................................................................
7.
19
19
20
20
Contest main timeline............................................................................................... 21
7.1. Approval .............................................................................................................
7.1.1. Physical examination and team interview................................................................
7.1.2. Practical trials ................................................................................................
7.1.3. Notes on the approval process..............................................................................
21
21
21
21
7.2.
Qualification round ................................................................................................ 22
7.3.
Final round .......................................................................................................... 23
8.
Appendices ............................................................................................................ 24
8.1.
Playing area drawing............................................................................................... 24
8.2.
Reflective tape ..................................................................................................... 25
8.3.
Polyurethane Foam ................................................................................................ 26
8.4.
Painting references ................................................................................................ 26
8.5. Safety ................................................................................................................
8.5.1. On-board voltage .............................................................................................
8.5.2. Compressed-air systems .....................................................................................
8.5.3. Lasers ...........................................................................................................
8.5.4. Powerful light .................................................................................................
5/27
26
26
27
27
27
Eurobotopen 2007
Official Rules 2007
2. Basic rules
The following rules are applicable to the Eurobot Open 2007 autonomous robot contest. It is expected
that the national qualifications will conform to most of these rules. However, participants in every
specific country are advised to check with their National Organization Committee for differences.
This year, the robots are sorting waste. There are three kinds of waste to find: bottles, cans and
batteries. Only one robot is allowed per team in a match. The matches involve two teams and last 90
seconds.
Each team is associated with a colour, red or blue . Each team has two bins: one for bottles and one
for cans. In addition, there is one, shared, basket for the batteries.
Each robot finds some garbage on the table, transports it to the correct bin, deposits it, and returns to
look for some more garbage.
2.1. Referees
Each match will be overseen by two referees. For the finals, at least one of the two referees will be
from a country different from both of the competing teams.
The referees are intended to help the teams, as well as interpret and apply the rules during the
competition and approval phase. Teams are encouraged to contact the referees with questions about
the rules or the competition process.
3. Playing area and element definition
3.1. General notes
A reference of parts and products used to make the game components is included in an appendix at the
end of this document.
3.1.1.
Tolerances
The organisers are committed to build the playing area to the highest degree of accuracy. But they do
allow for the following fabrication tolerances:
•
2% with respect to the playing field construction,
•
5% with respect to the playing element construction,
•
10% with respect to painted markings.
6/27
Eurobotopen 2007
Official Rules 2007
No complaints related to fabrication variations within the above tolerances will be considered.
The teams are warned that the paint finish of the table may vary from one playing area to another and
may degrade during the competition.
As we discover problems with the rules, the definition of the field and game components may be
modified over the course of the year. We strongly advise participating teams to regularly consult our
web site www.eurobot.org in the ‘FAQ’ section for potential amendments.
Please note: the above tolerances apply to the playing field and other components of the game. Robots
and the localisation beacons prepared by the participating teams may be required to be more precise.
3.2. The playing area
The playing field is 210 cm wide and 300 cm long. It is painted in grey anthracite. There are 4 bins and
one basket for depositing the waste. Each robot has 2 bins, one each for cans and bottles, at one
corner of the table. There is also a basket for the batteries. This is placed randomly and fixed to the
table at the start of the match, and is used by both robots for depositing the batteries.
Starting aera of
the blue team
Beacon
supports
Median line
(colours symmetry)
2 fixed
positions
Bottles
Starting
aera of
the red
team
Cans
Bins of the red
team
Basket
3 random
positions
7/27
Bottles
Cans
Bins of the blue
team
Eurobotopen 2007
Official Rules 2007
3.3. Starting Areas
The starting areas are on the corners of the table opposite the bins. Each starting area is a square of
500mm sides in the respective team’s colour (red or blue). The robots are placed touching the two
borders on either side of their start areas at the beginning of the match.
3.4. Border
The border is 70mm high (above the table level) and 22mm wide. The border in front of the bins is
slightly lower, 50mm for the can bins, and 40mm for the bottle bins.
The border is half painted in blue and red (symetrically, with the boundary in the middle of the long
side). The blue bins and red start-area are on the blue side, and the red bins and blue start-area are on
the red side.
3.5. Beacon supports / Reflectors
At each of the four table corners, and in the middle of the short sides are installed beacon supports for
the localisation beacons (see the table plan).
The beacon supports consist of an 80mm square platform supported 350mm above the table surface.
The platform is covered with Velcro (“snaggable” or “hard” side).
The beacon support mast is 10mm wide, painted black.
Cylinders about 22mm in diameter and 280mm high are located in front of the two masts on the
corners where are located the bins. They are covered with reflective tape (see appendix for
specifications of the tape). Another cylinder with reflective tape is located in the middle of the
opposite border (between the two starting areas). It is 280mm high from the border.
3.6. Waste / Placement of Waste
There are 50 total pieces of waste, of three types: 21cans, 21 bottles and 8 batteries.
3.6.1.
Aluminium cans
There are 21 cans on the table: 20 random and 1 fixed. They are aluminium cans of 330ml volume,
type sweet drinks with gas. The sides of the cans are painted yellow (RAL 1018). The tops and bottoms
of the cans are left unpainted (metal). In order to improve the stability and the
solidity of the cans, they are filled with polyurethane foam (see appendix). The
cans are 115mm high and 60mm in diameter on the major aera.
60mm
115mm
The cans are not magnetics.
8/27
Eurobotopen 2007
Official Rules 2007
3.6.2.
Plastic bottles
There are 21 transparent plastic (PET) bottles: 20 random and 1 fixed. The colour of PET is clear but
not determined. The plastic bottles are 500ml size, type sweet drinks with gas. The bottles are empty,
and without caps and rings. The original label is removed and a uniform green insulatingadhesive tape
of 50mm wide is placed instead. The label is located between 100 and 150mm high from the bottom of
the bottle. The bottles are 230mm high and 60mm in diameter at the label emplacement.
230mm
60mm
115mm
3.6.3.
100mm
Batteries
There are 8 batteries, 4 red and 4 blue. Three of each color are
fixed and one of each is
random. The batteries are Type “D” or “LR20”, 1.5V. The body of the battery is 55mm high and the
positive terminal 5mm high. The batteries are discharged. Only undamaged batteries
without corrosion may be used.
The original label is covered with an insulating adhesive tape of uniform colour (red or
blue). The adhesive tape is 50mm wide and placed from 4mm from the bottom (the
negative terminal) of the battery.
3.6.4.
Placement of the waste
There are 5 equidistant positions (every 350mm) on the median line of the table: 2 fixed positions and
3 random positions. The fixed positions are for a can and for a bottle. The position nearest the starting
area is for the can. The second position is for the bottle. The three other random positions are for a
red battery, a blue battery and the basket. Before the match a randomly drawn card is used to
determine the position of those three elements.
Another 46 waste items (20 cans, 20 bottles and 6 batteries) are placed on the table with the aid of
triangular frames. First the frames are placed (see schema of the table) on each side of the table.
Then, three batteries of a colour are placed inside and on every corner of the opposite triangle of the
starting aera. After, the 10 cans and 10 bottles are put in each frame, randomly, lain down. The side
interior of this equilateral triangle is 850mm. Each triangle is at 300mm from the middle line of the
table (border interior of the triangle, one face parallel to the line.
One black point indicates the localization of the three fixed batteries of each triangle, the fixed bottle
and the fixed can.
9/27
Eurobotopen 2007
Official Rules 2007
3.7. Bins and basket
3.7.1.
Bins
The bins are outside the table on two corners of the table, facing the audience and opposite the robot
starting areas. There are two bins at each of the two corners, 500mm long and 250mm wide. The
opening of the bin is level with the table border. The bin in front (on the long side) of the table is for
cans, the border of the table is 50mm high in front of this bin. The other bin is for bottles, and the
table border is 40mm high in front of this bin. Each of the bins should be deep enough to potentially
accommodate all the correct waste. The bins must be transparent (plexiglass, net, transparent plastic
bags), so that the audience can see the contents.
The top of the bins is 350mm high from the playing area and slop down to 200mm high.
The top of the bins is covered by the same insulating adhesive tape of uniform colour and 50mm wide
(red or blue) of the batteries.
3.7.2.
Basket
There is a “basket”, randomly placed on the middle line of the table and fixed to the table at the
beginning of the match by a screw (∅6). The screw is at the center of the basket and determined its
position. The basket is a hollow cylinder, 200mm in diameter and 30mm tall, with 10mm walls and
open at the top. The interior depth is 25mm. It is white in colour. The position of the basket is
indicated by the card drawn at the start of each match
4. The robots
4.1. General notes
•
Each team can engage at most one robot.
•
The robot is a fully autonomous machine. It shall carry its own power source, actuators and
control system.
•
The robot is only allowed to communicate with the localisation beacons (see beacons, below).
•
During the match no remote controlled action is allowed: the robot runs strictly on its own.
10/27
Eurobotopen 2007
Official Rules 2007
4.1.1.
Visibility
The refereeing committee encourages the team to make the interior parts of their robot visible.
In the finals Eurobot, two rectangular spaces of 100x70 mm are required for stickers (numbers of the
teams and sponsors of the event, printed by the organizer). The space of the stickers is free but they
must appear on two different faces of the robot.
4.2. Limitations and safety issues
4.2.1.
Fair-play
The aim is to share a friendly time and play as many matches as possible. Therefore, any action not
directly in line with the match spirit as laid out in this document or harmful for match development
will be penalised. The goal of the match is not to prevent the other robot from playing.
In this spirit, teams shall not follow strategies that are, for example, meant to:
•
block the opponent robot’s access to an element or area of the playing field
•
deliberately designing the robot (colour, shape) in order to confuse the opponent robot
•
deliberately cause damage to the opponent robot, the playing area, or any of the playing field
elements.
In particular, none of the colours mentioned in these rules for playing field elements may be used on
the robot.
4.2.2.
Safety
•
The robots should not have any protruding or sharp parts that may be able to inflict damage or
that can be dangerous.
•
The use of liquid products, corrosive products, pyrotechnic materials or living beings in the
robot is forbidden.
•
All the systems on the robots shall respect existing national and European laws and
specifications. Specifically, the systems used shall comply with legal safety regulations and
must not endanger the participants or the public both during matches and backstage .
•
As a general rule, any device or system considered as potentially dangerous by the referee will
be rejected, and must be removed from the robot prior to competition, or result in
disqualification.
11/27
Eurobotopen 2007
Official Rules 2007
4.3. Mandatory equipment
All robots must include the following systems, or they will not be approved for competition.
4.3.1.
For starting the robot
This starting device shall be easily accessed on the robot. It shall be triggered by pulling a cord at least
500mm long. This cord shall not remain attached on the robot after it has been started. Any other
systems (remote control, manually activated toggle switch, etc…) will not be approved.
4.3.2.
Emergency OFF button
The robot must include an emergency off button, at least 20mm in diameter (for example a safety
button). It shall be placed in a conspicuous position in a zone that is not dangerous and that is
immediately accessible to the referee at any time during the match, on the top of the robot. Pressing
the emergency button shall result in immediate shutdown of all the robot’s actuators, leaving them
limp (not actively braked).
4.3.3.
Automatic shutdown
Each robot shall accommodate a system which shall stop the robot automatically at the end of the 90
second match duration. “Stop” implies complete shutdown of all actuators. Robots moving after the
end of the match will be penalized or disqualified.
4.3.4.
Obstacle avoidance system
The teams are required to equip their robots with an obstacle avoidance system. The system is
intended to prevent collisions between robots during a match. The system is intended to react to the
opponent robot. It could be realized through contact (e.g.: physical bumper sensor) or contact free
(e.g.: infra-red, laser, sonar, camera) sensors. It may make use of the localization beacon (see below)
placed on the opponent robot. If a bumper sensor is used, it must be sensitive enough, and the robot
reaction fast enough, to reduce the impact of a collision. The obstacle avoidance system will be tested
in a trial during the robot’s approval.
4.4. Robot Dimensions
The teams are allowed to design their robot with deployable mechanisms. If such mechanisms are
used, the robot shall typically unfold them after the match start signal.
The perimeter of the robots is defined as the convex envelope which fits the vertical projection of the
robot on the ground.
The perimeter of the robot in its starting configuration shall not exceed 120cm.
12/27
Eurobotopen 2007
Official Rules 2007
The perimeter of the robot in a fully deployed configuration shall not exceed 140cm at any time during
the match.
Those perimeters are dimensions without waste.
The height of the robot shall not exceed 35cm, excluding the beacon supporting mast (see beacons,
below), possible sensor equipment within the beacon support mast’s envelope (see beacons, below)
and possibly the pressing area of the emergency stop button.
All other systems, including mandatory systems, must be contained within the volume specified above.
All parts of the robot must remain physically connected – therefore the robot cannot leave parts of
itself on the playing area.
Starting position
≤ 120cm
≤ 140cm
Deployed configuration
4.5. Energy Sources
•
Permitted energy sources include springs, pressurised gas (but see limits, below!), solar cells
(note that the competition will be held indoors!), all types of commercially available batteries
and power cells,
•
Prohibited energy sources include all types of combustion engines, rocket engines, hydrogen
fuel cells, or any other type of burning or pyrotechnics, living beings and radioactive energy
sources of all types
13/27
Eurobotopen 2007
Official Rules 2007
•
If in doubt about your unusual energy source, ask the refereeing committee ahead of time!
•
With respect to batteries use only models with solid electrolyte in order to prevent any
problems with corrosive liquids
•
It is strongly recommended for teams to possess several battery sets and to design for easy
access in the robot for their replacement. The teams are reminded to have spare, fullycharged batteries available at all times. The teams shall be capable of playing two matches in
succession. Note that this includes the necessary “setup time”, when the robot is powered on
and waiting to start, but the match has not yet begun.
4.6. Control Systems
The teams may use any kind of robot control system (analogue, microprocessors, microcontrollers,
computers, programmable logic, etc).
Those systems must be fully integrated into the robot.
The control system must permit the robot to play a match as either colour. Ideally, this can be easily
set just before the match.
The control system must permit the robot to pass the approval phase (see approval, especially with
respect to obstacle avoidance).
4.7. Robot localisation beacon support
It is strongly recommended to design the robot with a support to accommodate a localisation beacon
prepared by the opposing team.
If desired, the support can be designed to be detachable, so that it is only used if the opponent needs
it. In this case the design must allow the support to be quickly attached before the match.
Finally, a team may choose not to set its robot with a beacon support. In this case, if the opponent
provides a beacon and wants to use it during the match, the team will be disqualified for not having
the support.
The beacon support shall at anytime comply with the following constraints:
•
It is a 80x80 mm square surface, located 430mm above the floor level. The structure
supporting this platform must stay within the vertical projection of this platform. This mast
cannot host any parts of the robot other than sensors. The mast shall be robust and rigid
enough to support the opponent’s beacon in a stable fashion. The team is responsible for the
robustness of its mast.
•
The platform surface of the support shall be fully covered with Velcro ("snaggable" or “hard”
face)
14/27
Eurobotopen 2007
Official Rules 2007
•
The support shall be located close to the horizontal centre of the robot. In robot non deployed
configuration, the distance between the support and the maximum robot extension on one side
shall not be less than 50% of the equivalent distance on the opposite side.
4.8. Technical poster
Each team is required to provide a technical poster during the approval phase.
This poster shall present information related to the design of the robot (drawings, technical
references, design specification, etc.). It should be at least DIN A1 in size, and ideally should be
printed. The poster is intended to promote exchange and communication between teams.
Effort should be made to make the poster understandable to a technical audience. Ideally the poster
should include pictures and diagrams to help explain the concepts.
The poster must also include:
•
the name of the team
•
the names of the team members
•
the nationality of the team
This poster will be displayed in the team area of the competition. An English version of the poster must
be supplied. Optionally, the team can provide other language versions as well.
The poster shall be supplied to the Eurobot association in PDF Format. The chosen resolution of the
PDF must guarantee all texts to remain readable. The file size of the PDF should remain below 10MB.
The team explicitly confers all rights to the Eurobot organization to use the poster, in whole or in part,
in any way desired. In particular, but without being limited to this, the Eurobot organization will
publish the poster on its website, and may use it for marketing purposes. The team is responsible for
ensuring that it has the rights to all graphics and texts used in their poster.
The PDF Version of the poster may be sent to Eurobot beforehand via your National Organisation
Committee, or may be provided on two CD-ROMs at the competition, during the approval for your
robot.
5. Beacons
5.1. General Comments
•
Beacons are meant to help the robot to locate itself on the playing area and to locate the
opponent robot’s position
15/27
Eurobotopen 2007
Official Rules 2007
•
Beacons are not allowed to obstruct the opposing robot (communication, weight...). If there is
any doubt that they may deliberately disturb the development of the match, the team will not
be allowed to use them.
•
Three fixed beacon supports are placed each end side of the playing area: one at the centre,
the two others on each corner (see the drawing in the appendix). They are black and placed at
a height of 350mm above table level. The masts of the beacon supports are round, 10mm in
diameter, and covered with reflective tape (see appendix).
•
The beacons’ undersides are covered with Velcro (soft "smooth" face) as a way to fasten them
on their assigned support.
•
The beacons (robot localisation or fixed ones) shall remain on their support throughout the
matches.
•
The use of beacons is optional and they shall be designed and built by the teams.
•
All safety standards applicable to robots are also applicable to the beacons.
Fixed beacon (8x8x16)
Mast (area for sensors only)
Robot localisation beacons (8x8x8)
35
all dimension in Centimetres
16/27
43
51
Eurobotopen 2007
Official Rules 2007
5.2. Robot Localisation Beacon
•
One localisation beacon can be fitted onto the other robot, in order to locate it. This beacon
will be fitted on the beacon support provided for this purpose by the opponent robot.
•
Only one localisation beacon is allowed per team.
•
The maximum size for a robot localisation beacon is an 80mm edge cube.
•
The elements used for the beacon design shall be useful. The referee may request if necessary
that the team opens its beacon casing for inspection and verification.
•
The robot localisation beacon top shall be able to support the flag module identifying the robot
allocated colour.
5.3. Fixed beacons
•
Each team can use a maximum of three beacons, to be placed on the provided supports around
the playing area.
•
A team can place its beacons on the beacons support which is in the middle of his starting area
and on the 2 beacons supports which are in the other side of the playing area, at the two ends
of it.
•
Only the 2 beacons on the same side can be linked together by a wire. This optional wire shall
not disturb the development of the match.
•
The beacons must remain within a square base of 80x80 mm and can be 160 mm high.
5.4. Communication signals
•
In order to avoid interference between the teams, it is recommended to encode the
communication signals. We strongly recommend that teams using infrared devices take into
account the strong ambient light used during the competition. Moreover, this illumination may
vary during the competition depending on the time and on the area.
•
We also mention that the competition staff uses high frequency radio devices during the
contest.
•
No protest regarding interference problems will be taken into account. The beacons must be
able to deal with the conditions that may change depending on the moment and their location
during the contest.
17/27
Eurobotopen 2007
Official Rules 2007
6. Match timeline
6.1. Robot identification
•
For each match, the robots are allocated a colour marking built as a small flag module: red or
blue. The use of this marking is to help the public to associate at any instant a robot with its
team.
•
The flag module mass is negligible. It is placed directly on the robot beacon support, or
directly on the robot (see chapter about the beacon support).
•
If the robot does not provide a beacon support, it must provide a 8x8cm square surface
covered with Velcro at the top of the robot to place the flag marker on.
6.2. Start procedure
•
A colour (red or blue) and therefore a side of the playing area are allocated to the team before
each match.
•
Only two members from each team are allowed access to the playing area for robot
preparation.
•
Both teams have 3 minutes to put their robot on the starting positions, they do this
concurrently.
•
The robots are placed on the table, entirely within their starting areas, and touching the two
table borders adjacent to the starting area.
•
After the 3 minutes of preparation, no more intervention or transmission of external
information to the robot is allowed.
•
When both teams indicate they are ready, the referee will determine random positions for the
two batteries and the basket. This is done by drawing from a set of cards. The referre set the
elements on the playingfield area. During this procedure, the teams don't have the right to
touch their robot.
•
The referee asks the participants whether they are ready and if they have any remarks about
the placement of all the game elements. No objection regarding the placement will be
accepted after this.
•
At the start signal given by the referee, the robot is activated by one of the team members.
The robot shall then run on its own in a fully autonomous way.
18/27
Eurobotopen 2007
Official Rules 2007
•
Any team which does not scrupulously follow this starting procedure (anticipated or delayed
start) is charged with a false start. A new start shall be given with a new random layout for the
game elements.
•
The referees reserve the right to start the robots themselves.
6.3. Match sequence
•
Robots have 90 seconds to score as many points as possible. This must be accomplished in
complete autonomy.
•
It is prohibited to remove garbage from the bins.
•
The team members are not allowed, in any way, to touch the robots, the playing area or any of
the fixed game elements during a match. Any action made without the referees consent will
lead to disqualification for the current match. The team will then lose all the points it may
have scored during the match.
•
If the robot leaves the playing area, it can't be put back in it. The match is not replayed and
the other robot is allowed to finish the match normally.
•
A robot that deliberately pushes its opponent out of the playing area is disqualified.
•
At the end of the match the robots must stop (shutdown of all actuation) by themselves. If the
robot does not shut down by itself, a referee will push the emergency stop button to stop the
robot.
•
The referees will count the points without touching the robots. Then they will announce the
score.
•
The team members are allowed to touch the robots and leave the game only with the explicit
consent of the referees after common agreement on the score.
6.4. Score calculation
6.4.1.
Points
Points are counted after the match is over. After the match, the waste in the bins and the batteries in
the basket are checked.
•
For the correctly sorted waste:
Each team gets one point for each bottle in its bottle bin.
Each team gets one point for each can in its can bin.
19/27
Eurobotopen 2007
Official Rules 2007
•
Incorrectly sorted waste is handled as follows:
The first four incorrectly sorted waste items give no point, because a bad sorted bin is not
accepted by a recycling company. Each additional incorrectly sorted waste item beyond the
first four items counts one point for the opposing team.
•
Each battery in the basket gives three points for the team of its colour.
6.4.2.
Penalties
A penalty results in the removal points from the score of the team at the end of the match. Any actions
not complying with the spirit of the rules may be penalized by the referees. For example, the referees
may assign penalties in the following cases:
•
When a robot violently collides with its opponent
•
When a robot is considered dangerous to the table, the audience or its opponent
•
When a robot deliberately prevents its opponent to reach a game element
•
If a robot’s shutdown system fails to work
•
If a robot intentionally or systematically throws waste outside of the table
•
If a robot removes waste from its opponents bins
Warning: this list is not exhaustive! Additional penalties can be applied if the referees consider this
justified.
6.4.3.
Scratching
The referees will scratch a team:
• That has not come on time at the backstage waiting room for matches;
• That takes more than 3 minutes to get ready on the playing area;
• Whose robot has not completely left the starting aera.
• Whose robot doesn't have a mobile beacon support, when its opponent requests and requires one.
• Whose robot shows a deployment or an action that has not been previously approved by the
referees, or any deliberate action not in line with the rules.
A team being scratched during a match loses all the points gathered during this match. The opposing
team carries on with the match and keeps its points.
20/27
Eurobotopen 2007
Official Rules 2007
7. Contest main timeline
7.1. Approval
To participate in the tournament a robot must be approved. Approval is mandatory. For logistical
reasons, and reasons of fairness, there is a deadline for approval. All teams must approve their robots
before this deadline. Robots that are not approved may not participate in the tournament.
To be approved, the robot must pass two tests: a physical examination and practical trials.
7.1.1.
Physical examination and team interview
The referee examines the robot and interviews the team to check the following:
•
That the robot complies with the rules (the robot should be capable of demonstrating all of its
possible actions and deployment configurations to help verification).
•
That the team provides a technical file (see paragraph 4.8).
•
That the team understands the rules and the spirit of the tournament
•
That the emergency stop button works, and all actuators are shut down when it is pressed
7.1.2.
Practical trials
That under match conditions, without opponent:
7.1.3.
o
The robot is capable of leaving the start area
o
The robot is able to win a match, without opposition within 90s
o
The robot’s shutdown system works properly
o
The robot’s obstacle avoidance system works: The referee can test the system, by
placing an obstacle in the robot’s path. The obstacle may be slightly to the side of the
robot. This obstacle is a fake robot. It has a beacon support like the opponent robot,
which may be used by the team if desired. The robot must avoid the obstacle,
demonstrating to the referee in a convincing way that it is reacting to the obstacle.
Notes on the approval process
It is mandatory to keep referees informed of any major modifications (functionality, size, etc.) made
after approval. The referees will check the modifications and re-approve the robot.
21/27
Eurobotopen 2007
Official Rules 2007
Actions used during the match which were not demonstrated during the approval process can lead to a
disqualification.
Referees can require a robot to subject itself to another approval process at any time during the
competition, if there is any doubt about its compliance to the rules.
7.2. Qualification round
The Eurobot organization will try to organize at least five matches for each team during the
qualification round. The results of the qualification round decide which teams go through to the final
round.
The qualification score is determined by adding the points accumulated during each match with bonus
points, as follows:
•
4 points for a victory,
•
2 points for a draw,
•
1 for a defeat,
•
0 for a scratched match
A score of 0 to 0 is considered to be a double defeat. Hence, each team gets only 1 bonus point
When the qualification rounds are over, the teams are sorted using their qualification points count.
The teams that have the same points count are sorted by comparing the points accumulated during
each match without adding the bonus points. The organisers may, if necessary, request extra matches.
22/27
Eurobotopen 2007
Official Rules 2007
7.3. Final round
At Eurobot, the first 16 teams from the qualifying phase are selected for the final round. In a national
competition the final round may be smaller, depending on the number of registered teams.
During the final phase, matches are on a knockout basis. The matches for the final round are organised
as shown in the diagram below.
In the event of a double defeat, a draw or a double disqualification the match is replayed immediately.
If this second match is also a double defeat, a draw or a double disqualification, the winner will be
determined by the position at the end of the qualification rounds.
Quarter-Final
1 meet 16
Final
Semi-Final
..........
8 meet 9
..........
5 meet 12
..........
3rd place final
.........
4 meet 13
3 meet 14
..........
6 meet 11
..........
7 meet 10
..........
2 meet 15
23/27
.........
Eurobotopen 2007
Official Rules 2007
8. Appendices
8.1. Playing area drawing
Dimensions are given in mm and are subject to the general tolerances defined (see Tolerances, above).
Top View
Bin for
bottles
Bin for
cans
Median line (one
basket, two
batteries, one
bottle, one can)
24/27
Bin for
cans
Bin for
bottles
Eurobotopen 2007
Official Rules 2007
Face View
Bin for
bottles
Bin for
115mm
cans
Bin for
cans
Bin for
cans
Right view
Bin for
bottles
Bin for
bottles
8.2. Reflective tape
This is made of an adhesive retroreflector tape 0,2mm thick and 25mm wide, white in coulor. Its part
number is #XUZB05 at “Telemecanique”. The tape is distributed online by “Radiospares” (“RS
Electronics”) under part 324-1591 (1m length).
25/27
Eurobotopen 2007
Official Rules 2007
8.3. Polyurethane Foam
The polyurethane foam is used to strengthen and to stabilize the cans. It is saled in every do-it-yourself
store.
8.4. Painting references
Element
Colour
Paint Type
Paint
Brightness
Reference
Arena floor
Grey
(Anthracite)
Acrylic
Satin
RAL 7016
Blue start area,
blue border, blue
batteries
Blue (sky)
Acrylic
(Adhésive tape)
Satin
RAL 5015
(Type RAL
5015)
Red start area,
red border, red
batteries
Red (fire)
Acrylic
(Adhesive tape)
Satin
RAL 3000
(Type RAL
5015)
Basket
White
Acrylic
Satin
RAL 9010
Bottles
Green
(bright)
Adhesive tape
Satin
Type 879
PF
Cans
Yellow
Acrylic
Satin
RAL 1018
8.5. Safety
Below you will find a list of safety regulations. This list is not exhaustive - the referees’ decisions are
final on the dangerousness.
As a general rule, the teams shall develop systems that satisfy design and manufacturing criteria that
do not endanger the team or the audience both in the pits and during the matches.This is also why you
must ensure that your systems comply with applicable laws for technical devices.
8.5.1.
On-board voltage
All robots must comply with the legal standards concerning “low voltage”. Therefore, the internal
voltage of the robots shall not exceed 48 V.
26/27
Eurobotopen 2007
Official Rules 2007
8.5.2.
Compressed-air systems
All pressure systems must comply with applicable laws. In France this shall be in accordance with the
“Conseil Général des Mines” (French safety and approval board).
Decree 63 of January 18, 1943 and Ministerial Order of July 25, 1943 (French legislation):
•
Maximum pressure: 4 Bars
•
Maximum Pressure x Tank Volume product: 80 bar lt.
Further information may be found on http://www.industrie.gouv.fr/sdsi/ (for France).
8.5.3.
Lasers
Max output power for lasers shall be lower than 1mW (0dBm). Technical documentation of the laser
device may be required at the approval. It must be supplied in French, English or German.
8.5.4.
Powerful light
In case of a high intensity source being used, the light intensity shall be dangerous for the human eye
in case of direct illumination. Note: Some powerful LED devices can exceed this limit.
27/27
A.2. FAQ 1
A.2
FAQ 1
67
Eurobot open 2007
Official Rules 2007
FAQ 1
Introduction
This FAQ is divided into two sections: “Corrections”, containing information which replace or add
precisions to the official rules for 2007, and “Answers to common questions”, which contains official
answers to some of the most common questions posted on the forum or asked by email.
Corrections
3.6.1 Aluminium Cans
The cans are 115mm high and have a 66mm diameter around most of the can’s length. For the
Eurobot final, the cans are silk-screen printed entirely in yellow and have a vertical line of 5mm from
top to bottom without colour (metallic).
3.6.2 Plastic bottles
The bottles are 230mm high and have a 65mm diameter at height of the label
3.7.1 Bins
The bottoms of the bins (the part below the table) are made of hard plexiglass and include a door so
that the content can be removed. The other parts are made of netting (like the nets to protect fruit
trees from birds). The netting is tight.
1/6
Eurobot open 2007
Official Rules 2007
3.7.2 Basket
The basket is a hollow cylinder, 200mm in diameter (external diameter) and 30mm tall, with 10mm
walls.
4.2.1 Fair-play
This sentence is missing: No system is allowed which attach the robot to the playing-field (eg: suction
cups). At any time during the match, the force required to lift the robot must not exceed its own
weight.
5.1 General Comments
The masts of the beacon supports are not covered by reflective tape. The cylinders about 22mm in
diameter and 280mm high are located in front of the two masts, on the corners where the bins are
located. They are covered by reflective tape (see paragraph 3.5)
Answers to common questions
Q1) Is the number of waste transported in the robot limited?
A1) No, there is no limitation on the amount of waste transported by the robot.
Q2) Is it allowed to put waste in the opponent’s bins? Or to help the opponent to put objects in her
bins?
A2) Yes, it is allowed for well sorted waste. The points are for the other robot and are counted at the
end of the match. But be careful, don’t obstruct the way of the robot to its bins and don’t put cans in
the bottles’ bin and bottles in the cans’ bins. In this case, you can be disqualified.
We encourage the fair-play but try to solve the problem posed (to sort the waste in your bins), not a
different problem!
Q3) Can we intentionally put incorrect waste into opponent bins?
A3) No it is forbidden, and you can be disqualified if you do that.
2/6
Eurobot open 2007
Official Rules 2007
Q4) Is it allowed to throw waste?
A4) Yes of course, but only in the bins.
Q5) Is it authorized to withdraw the opponent’s batteries from the basket?
A5) No, it is forbidden.
Q6) a. Can we touch/move the batteries of the other team?
b. It is possible to catch and keep the batteries of the other team?
c. Can we remove the cans/bottles out of the bins of the other team?
A6) a) yes, but they have to be accessible for the other robot.
b) Yes but only during a short time. The robot cannot keep the opponent’s batteries for a long time. It
will be penalised if it seems to be a strategy.
c) No it is forbidden.
Q7) What happens if we put batteries in the bin for cans or bottles? Is that "incorrectly sorted
waste" or does it mean nothing?
A7) It is ‘incorrectly sorted waste”. If the batteries are to the opponent team, you will get penalties or
even disqualification.
Q8) Is it allowed to push waste towards the baskets? They will be considered in the perimeter of
the robot?
A8) Yes it is allowed; They are not considered in the perimeter of the robot.
Q9) What happens if a battery is on the edge of the basket? Is it counted in the basket?
A9) No, it doesn’t count.
3/6
Eurobot open 2007
Official Rules 2007
Q10) Can one store the bottles in a plastic bag and deposit the bottles and plastic bag in the bin or
only the bottles necessarily should be put, without the plastic bag? How about cans?
A10) No, the robot has to stay in one part, so no detaching bags from the robot.
Q11) Waste will not be counted as being in the perimeter of the robot: can one store waste above
the top of the robot?
A11) Yes it is allowed to store waste between 35cm and 43cm above the ground, provided it does not
interfere with beacon systems. In particular, there must be clear line of sight between a beacon on the
robot beacon platform and the beacons on the side of the playing field.
Q12) The bottles will be very fragile because open. Would it be possible to fill them with
polyurethane foam in order to make them a little more solid?
A12) No, the bottle will not be filled with foam, and will not have their caps attached. They are solid
enough.
Q13) Which type of battery will be used?
A13) Alkaline batteries will be used. They are magnetic.
Q14) What is the reference of the green adhesive band?
A14) It is available in every DIY shop. It is an extra-adhesive tape, watertight (or waterproof), resisting
to high traction, UV and bad weather. It is a tape for reparation, protection, water tightness,
reinforcement, packaging... It is possible to cut by hand. The colour is bright green.
Q15) For the localization system we are developing this year, we will use a dedicated electronic
board. Is it possible to place this board on the mast (right under the support, and without
exceeding 80x80 mm)?
A15) It is allowed if it is a part of the localisation system. Any sensor or similar systems are allowed
there, including associated electronics, but not actuation systems.
4/6
Eurobot open 2007
Official Rules 2007
Q16) The borders are 7cm tall, and the diameter of the bottles and cans is 66mm. That leaves only
one small centimetre of margin to the robots to make the distinction between a wall to be avoided
and some waste.) Can you make the border taller?
A16) No.
Q17) How will our obstacle avoidance system be tested?
A17) The obstacle avoidance system will be examined during approvals, and may be tested in a trial on
the game table. In the trial, the system should be able to successfully avoid a static dummy obstacle
(30cm high cylinder, 20cm diameter, between 2kg and 3kg weight) placed at a random distance in
front of the robot on the table. If desired, you will be able to place a beacon on the dummy obstacle,
which has a beacon platform.
Q18) If the other robot is 7cm tall and our infra-red based detection systems (for example) are
located 20 cm above the ground, how could it be detected? What if my bumpers don’t touch the
opponent robot? If the opponent robot has a very particular shape, it might not be reliably
detected!
A18) You must try to make your obstacle avoidance system as robust as possible. Obviously, a very
small opponent robot (e.g.: smaller than a bottle!) needs not be avoided. Your system will be
examined, and might be tested as described above. Make sure your system works with a simple
obstacle like that. For robots with a hollow structure, if you want to be safe, we suggest you to add
bodywork, made of cardboard for instance. This will increase the visibility of your robot to IR and sonar
based systems. In the same idea, you should avoid painting your robot in black since it could confuse IR
based detection systems.
Q19) What happen if a bottle or a can rolls out of place when the referees remove the triangle?
A19) We put it back in the triangle.
Q20) The borders are painted in red/blue; is it only the interior side or the tops of the borders too?
A20) Only the interior border is painted in blue/red.
Q21) Can the robot ram the piles of waste at the start?
A21) Yes, but if there is damage to the waste, you might be penalized.
5/6
Eurobot open 2007
Official Rules 2007
Q22) a) Does my beacon mast have to be rigid?
b) Does that mean the mast may turn/swivel, if it stays in place?
c) May my mast include moving sensors (like a swivelling laser or mirror)?
A22) a) Yes, totally.
b) No, the mast may not turn or swivel either, it must be completely rigid.
c) Yes, you may, but these may not move the mast, or occlude the area on top of the mast. Mirror
systems may not be designed to deliberately confuse the opponents.
6/6
74
A.3
APPENDIKS A. REGLER & FAQS
FAQ 2
Eurobotopen 2007
Official Rules 2007
FAQ 2
Introduction
This FAQ is divided into two sections: “Corrections”, containing information which replace or add
precisions to the official rules for 2007, and “Answers to common questions”, which contains official
answers to some of the most common questions posted on the forum or asked by email.
Corrections
3.5 Beacon support/ Reflectors
The cylinders covered with reflective tape are 21,3mm of diameter instead of 22mm. The top of
the cylinders is 350mm high from the playing area. So the cylinders located on the corners where are
the bins are 310mm high (because the border is 40mm high). The other cylinder with reflective tape
located in the middle of the opposite border (between the two starting area) is 280mm high. The
three cylinders are covered with reflective tape. But the masts of the beacon supports are not
covered by reflective tape. (see the pictures page 4)
3.6.2 Plastic bottles
The label is located between 100 and 150 mm high from the bottom of the bottle. There is a mistake in
the drawing. The value "115mm" on the drawing, should be "150mm”.
1/4
Eurobotopen 2007
Official Rules 2007
3.7.1 Placement of the waste
Position of the batteries: are they inside or outside the triangle just before the beginning of the match?
In the text of the rules (paragraph 3.6.4), it is written "inside" and in the draw, they are "outside".
What is the correct position of the batteries?
The batteries will be outside the triangle just before the beginning of the match.
For the installation, we use the followed procedure: (see the four pictures)
- triangle in place, we put the waste inside (Picture 1 and picture 2)
- the triangle is removed, and batteries are placed at this time on the dots painted on the playing field
(Picture 3 and Picture 4).
Picture 1
Picture 2
Picture 3
Picture 4
2/4
Eurobotopen 2007
Official Rules 2007
3.7.2 Basket
There is a “basket” randomly placed on the middle line of the table and fixed to the table at
the beginning of the match by a screw (6mm diameter).
For the screw is not 6mm but 8mm. The basket is a hollow cylinder, 194mm in exterior diameter
and 30mm tall, with 4mm walls and open at the top.
Answers to common questions
Q1: The rules says that "The top of the bins is covered by the same insulating adhesive tape of
uniform colour and 50mm wide (red or blue) of the batteries".
Since the bin is 350mm high, what do we have to assume?
1. the gate is 300mm high and then there is a 50mm bar covered with tape (or)
2. the gate is 350mm high and then there is a 50mm bar covered with tape, for a global height of
400mm
Another question. Since bins are made with net (in the upper part), do we have to assume that
also each vertex is covered with the tape (as the drawing in the front page of the rules suggests)? If
this is the case, what is the size of such a covering?
A1: The gate is 350mm high. The bin is made with net that is maintained with a rigid bar with a
negligeable thick (10 or 12mm maximum). Only this bar is covered with the adhesive tape (borders of
the opening). (see below the pictures of the bins)
3/4
Eurobotopen 2007
Official Rules 2007
4/4
A.4. FAQ 3
A.4
FAQ 3
79
Eurobotopen 2007
Official Rules 2007
FAQ 3
Introduction
This FAQ regroup some advices from the refereeing committee and some precisions on the playing field
for Eurobot final 2007.
Notes
1. Obstacle Avoidance System
Your obstacle avoidance system will be tested during homologation, as described in the rules.
The rules require you to “react to the obstacle”. During the National competitions, we have observed
that many robots interpret this to mean: “Stop until the way is clear.”
If your robot employs this strategy, please consider what will happen when it meets another robot
employing the same strategy! (Hint: Answer: Nothing will happen, both robots will stop, the match is
effectively over -  )
So, please program your robot to do something other than just stop if it detects an obstacle. For
example, your robot could:
1.Back away
2.Turn around, go the other way
1/4
Eurobotopen 2007
Official Rules 2007
3.Head either left or right around the obstacle
Note that this is a recommendation based on observed matches, not a rule. Your robot will not be
penalized for being boring and just stopping.
2. Homologation Program
During the national competitions it was observed that many robots had difficulty with the
homologation.
For the homologation it is recommended that you design special “homologation software” for your
robot. Your robot does not have to homologate with the full program used during competition matches.
You can write a simpler, specific homologation program.
Also, we recommend having separate programs for the point scoring and the obstacle avoidance parts
of the homologation.
To speed up the homologation process, all teams are required to perform the physical homologation of
their robots within 2h of arrival at La Ferté Bernard. When you arrive, sign in, get your pit and
accommodation, unpack your robot at the pit, and then get a referee to approve your robot.
3. Strategy: Keep it simple!
Employ simple strategies! Eurobot is less than 2 weeks away. If your super-complex robot involving
computer vision, q-learning and a six degree of freedom waste processing system is not yet working,
perhaps it is time to simplify your design!
In many national competitions the simpler robots proved more reliable.
4. Blowing the Waste
The rules do not prohibit blowing the waste across the table. Blowing items of waste individually is
certainly allowed. However, blowing all the waste (randomly) across the table is not the idea of the
competition, and depending on what happens, your robot could even be penalized! This will be the
case if, for example, waste is blown off the table, waste is blown into the incorrect opponent bin, or
the waste is blown in such a fashion that the opponent robot cannot access it (blocking). Play fair, give
the opponent his chance!
5. Technical poster
Chapter 4.8 of the rules.
The poster shall be supplied in PDF Format. The chosen resolution of the PDF must guarantee all texts
to remain readable. The file size of the PDF should remain below 10MB, on a key USB (USB flash
drives). A computer will be at the pre-approvals. The referees will ask you your poster.
2/4
Eurobotopen 2007
Official Rules 2007
6. Playing field and playing element of the French Cup and Eurobot final
The weight of the batteries is between 130g and 150g.
The basket is a hollow cylinder, 200mm in diameter and 30mm tall, with 10mm walls and open at the
top. The interior depth is 25mm.
For the Eurobot final and the French Cup, t he cans are silk-screen printed entirely in yellow and have
a vertical line of 5mm from top to bottom without colour (metallic).
The bin with the net :
The net will tight.
3/4
A.5. APPROVAL SHEET
A.5
Approval Sheet
83
Eurobot 2007
Robot Recycling
Rally
N°Pit :
Team Name :
Referees :
PRE-APPROVED
NUMBER OF TESTS ON THE
TABLE
APPROVED
Points :
1 Structure of the Main Robot
1.1
1.2
1.3
1.4
1.5
1.6
1.7
1.8
1.9
1.10
1.11
Before the match starts, the unfolded convex envelope that fits the vertical projection of the robot onto the
ground does not exceed 120 cm.
The perimeter of the robot in fully deployed configuration doesn’t exceed 140 cm at any time during the
match.
The height of the robot never exceeds 35 cm.
The robot has an obstacle avoidance system.
The robot hosts a beacon support measuring 8x8cm, located at 43cm high and covered with Velcro (soft
"snaggable" face).
This beacon support is robust enough to host the opponent’s beacon and to interpret its data. It is located at
the centre of the robot as much as possible and contains only systems connected to sensors.
The robot is composed of interdependent elements (and shall not leave parts or elements on the playing
field).
The robot is a totally autonomous machine with embedded power supply, actuators, and control systems.
The robot has an emergency power off switch, installed within the limits of the dimensions of the robot. It
must be put in evidence by a red distinguishing mark of at least 20 mm in diameter and must be placed in
evidence in a not dangerous zone and of immediate and vertical access for the referee at any time of the
match. This switch must interrupt the power for driving motors and actuators.
The robot has a starting device which must be easily accessed and triggered in a single movement by a wire
at least 50cm long.
The robot has 2 sets of batteries or sufficient long-life batteries.
1.13
1.14
The robot will not cause intentional damage to the opponent robot and/or to the playing field or to people in
the attendance.
The robot doesn’t hold any element that would intentionally jam the opponent robot or that would hinder the
game. It doesn’t use accessories, colours or drawing looking like playing area elements in an attempt to lure
the opponent robot.
The robot has no protruding or sharp part that may be able to inflict damages or that can be dangerous.
The robot doesn’t make use of any corrosive or pyrotechnic products, nor spraying of liquids.
1.15
1.16
1.17
The robot is only allowed to communicate with the beacons, if they are used.
The robot never totally prevents its opponent to access the waste, the bins and the basket.
Any pressure system complies with the applicable french regulation (PV<80 and 4 bar at output).
1.18
Any laser complies with the constraint: max optical output power : 1mW (0dBm).
1.19
No anchoring system is allowed (suction….) Consequently, at any time during the match, the necessary force
to lift up the robot shall not be exceed its own weight.
1.12
At this point, the robot is said pre-approved.
Eurobot 2007
Robot Recycling
Rally
2 Beacons (skip this section if the robot has none)
2.1
2.2
2.3
2.4
2.5
Each beacon fits in 8x8x8cm (embedded beacon) and 8x8x16cm (static beacon) . One side of the beacons is covered
with Velcro (soft snagging face); the top face of the embedded beacon is covered with Velcro (hard snagging face).
Beacons may in no way serve to jam or hinder the opponent robot.
Embedded beacon (1 maximum per opponent robot) shall be totally autonomous and independent.
Static beacons (3 maximum) must globally be autonomous but 2 of them may be connected together by a wire link. In
that case, the wire shall not hinder the game progress.
Any beacon using laser complies with the constraint: maximum optical output power : 1mW (0dBm).
3 Technical file
3.1
3.2
The team provides an A1 poster to display in its pit.
The team provides a CD-ROM containing a maximum of 10Mo of data. On the CD are mentioned: the name of the
team, the name of the school/university/club, the pit number.
4.1.
4.2
4.3
Under match conditions (1min30), free of opponent, the robot is able to win a match in 5 attempts maximum and to
leave his departure area.
The robots doesn’t damage the playing field nor the waste, nor the bins and the basket during the attempts.
The robot has an obstacle avoidance system. This system should be able to successfully avoid a static dummy
obstacle (30 cm high cylinder, 20 cm diameter, between 2 and 3 kg weight) placed at a random distance in its path
on the table.
4.4
The robot has a timer in order to make it stop and stay in position at the end of the match time (1min 30).
4.5
The strategy of the team does not have actions against the fair unfolding of the games. Referees could, if they
consider it happens, give a penalty to the team.
Robot is not dangerous for the audience.
4 Game rules
4.6
4.7
4.8
4.9
Team members agree to accept the decisions of the referees (decisions may overpass the rules).
Teams are informed about the procedures of the match (start procedure, game phases, point validity, score
counting).
Team members are informed that only 2 people of each team are allowed on stage.
Comments :
A PPENDIKS
SMR-CL
87
B
SMR-CL.
Small Mobile Robot Control Language, SMR-CL is a simple interpreted language
intended for control of mobile robots. The language supports sequences of basic robot
actions with multiple stop criterion as well as standard mathematical expressions. The
system has two kinds of variables system variables that reflects the state of the vehicle
and user variables that are created the first time they are used in an assignment clause.
The language is intended to be used in the tactical layer just over the control layer i.e. it is
fast enough to shift between control strategies and react to state changes in real time. The
hope is that it will be able to bridge the gab between the hard real-time demands of the
control layer and the soft realtime demands of the planning (strategic) layer. The
language may be used in two ways either as scripts run directly from text files or as a
command language through a socket interface. Most commands are available in both
modes but a few are only available in one of the modes.
Basic format.
The language is line oriented i.e. it is interpreted line by line. The basic format is
command parameters [@v velocity] [@a acceleration] [: stopconditions]
where [ ] means optional.
command
parameters
is the desired command
is a number of parameters required by the command. The parameters can
be numbers, variables or string litterals (characters within " " )
velocity
desired vehicle velocity in m/s (variable or number)
acceleration desired acceleration in m/s2 (variable or number)
stopconditions
( expression ) [ | (expression)]*
a list of ORed stopconditions, the command will end if one or more of the
conditions are true.
The system variable $condition will hold the number of the first true
condition (from left)
Examples:
fwd 2.0 @v0.5 @a0.05
move forward 2 m with acceleration 0.05 m/s2 max velocity 0.5 m/s
drive @v0.5 :($drivendist > 4)| ($cmdtime > 12)
drive at velocity 0.5 m/s until 4 m is driven or time used is 12 seconds
turn 90
turn 90 degress around center
Motion control commands
The following motion control commands exist:
all commands will use current velocity and acceleration references as max values
fwd
turn
d
b ["rad"]
move forward d meters
turn b degrees (radians if "rad" )around center ( between the
driven wheels)
turnr r b ["rad"] turn b degrees(radians if "rad") with turning radius r m
drive [x y th]
drive with current speed along the line defined by point (x,y) and
direction th. If x,y and th are omitted the current position and
direction of the vehicle is used
stop
stop vehicle and keep position and direction
idle
stop vehicle and keep speed at zero.
followline "type"
follow line using the method described by type:
bl
left side of black line
bm
middle of black line
br
right side of black line
wl, wm, wr as above with white line.
followwall "side" dist
follow a wall to the side of the vehicle indicated by side:
l left , r right
dist distance to wall in meters
resetmotors
resets the motor modules
ignoreobstacles
make the vehicle ignore obstacles in the next command
Examples
% square program1 (comments are indicated with the % character)
fwd 1 @v0.3
turn 90
fwd 1
turn 90
fwd 1
turn 90
fwd 1
turn 90
The vehicle runs in a square with side length 1m and max velocity 0.3 m/s
Flow control.
The following flow control statements are implemented:
The label, goto and if statements are only available in scripting mode.
label "labelname"
goto "labelname" continues execution at the line with label "labelname"
if (exp) "labelname" jump to "labelname" if expression is true ( not zero)
switch (exp)
statements
these statements are executed if exp < 0.5
case 1
statements
these statements are executed if 0.5 < exp < 1.5
case 2
statements
these statements are executed if 1.5 < exp < 2.5
case n
statements
endswitch
these statements are executed if n-0.5< exp< n+0.5
The switch statement must not be nested.
%
the rest of the line after % is a comment
example:
% square program2
n=4
label "start"
fwd 1 @v0.3
turn 90
n=n-1
if (n >0) "start"
Expressions.
The language supports normal arithmetic expressions:
+- * /
> >= < <= == !=
|
logical or
&
logical and
()
=
assignment
Functions.
sin(x)
cos(x)
tan(x)
atan(x)
atan2(y,x)
ln(x)
exp(x)
sqrt(x)
abs(x)
Example:
x=1.0
y=3
a=2.5*sin(x)+sqrt(y)
Logging of Data.
The language supports simultaneus logging of up to 9 variables. Logging is started with
the log command and the result is stored in the text file 'log' in the current directory when
the run is stopped. The variables are sampled every 0.01 seconds and stored as a line for
each sample time i.e. they may be loaded into e.g Matlab or Excel.
log "varname" ["varname"]*
Example:
log "$odox" "odoy" "$odoth"
eval exp [; exp]*
wait time
Miscellaneous.
print the results of the expressions on the screen in scriptmode and
returns the values in socket mode.
makes the program wait for time seconds
vision “command text”
sends the “command text” to the cameraserver if started. The
server might return data in the default variables $vis0 to $vis9
If the command text is $string the string generated by the latest
stringcat command will be used
laser “command text”
sends the “command text” to the laserserver if started. The server
might return data in the default variables $l0 to $l9
If the command text is $string the string generated by the latest
stringcat command will be used.
stringcat parameters
concatenates the parameters to a string saved in $string. If a
parameter
is a string it is added unchanged if the parameter is a number
or a variable it is converted to a string and added. The command is
intended to be used with the “laser” or “vision” commands
example:
stringcat “send “ 9 “ apples”
will generate the string “send 9 apples”
Socket interface.
The socket interface is also lineorientated. The interface consists of the following special
commands:
getevent [time-out] fetches the next event in the event queue. If the queue is empty the
command will return after 'time-out' seconds with the message
'eventtimeout'. If no time-out is given the command will return
immediately.
putevent "text "
generates an event with the text 'userevent text' where text is the
text entered between the "'s
flushcmds
flushes all commands from the command queue
logoff
logs off and leaves the robot ready for a new logon
exit
logs off and stop the robot controlprogram
addwatch “name” type :( expression)
puts a watch in the watch table. If expression becomes true an
event 'watch name fired time' , where name is the name given in
addwatch and time is the time where the expression became true.
When a command is sent to the socket the robot will respond with 'IDnn queued' where
nn is a unique number. Execution of the command will generate a number of events that
may be retrieved with the command 'getevent' . If the command is finished in one sample
the event Idnn assignment will be generated. If the command takes more samples to
execute the start of execution will be signalled with 'IDnn started' and end of execution
with 'IDnn stopcond xx' where xx is the value of the stopcondition.
System variables.
Odometry:
$odox
$odoy
$odoth
$odovelocity
$ododist
x-coordinate of vehicle position (m)
ycoordinate of vehicle position (m)
direction of vehicle (rad)
velocity of vehicle (based on odometry measurements)
distance driven since start of vehicle (m)
$drivendist
distance driven during the current command (m)
IR-sensors:
all distances are measured from the sensor itself.
$irdistleft
distance (m) measured by the left side sensor
$irdistfrontleft
distance (m) measured by the front left sensor
$irdistfrontmiddle
distance (m) measured by the front middle sensor
$irdistfrontright
distance (m) measured by the front right sensor
$irdistright
distance (m) measured by the right side sensor
$cmdtime
$batteryvoltage
$supplyvoltage
$time
$motionsstatus
$motorstatusr
$motorstatusl
Linesensor:
$line0 to $line7
$linepos
$blacklinefound
$crossingblackline
time spent in the current command (sec)
batteryvoltage (v) lowpass filtered with a time constant of a second
voltage of the external power supply (v) lowpass filtered with a
time constant of 0.1 second
system time in seconds (Linux time of day)
status telling if the robot is stopped by an obstacle
motor status for the right motor
motor status for the left motor
linesensor values
position of found line
true if a black line is detected ( used to find a line approched from
the side)
true if a black line is crossing the line the vehicle follows
Robot Control Program.
The program implementing SMRCL is called smrdemo. It is started by logging on to the
robot using ssh and starting the program from the terminalwindow.
smrdemo [options] [scriptfile]
The program has the following options:
-c calibration. This will start a calibration menu where the linesensor, the irsensor and
the odometry may be calibrated. The calibration results will be saved in files
in the directory calib which must exist before calibration.
-t n start the program as a socket server on port 31000+n. (n is an integer)
-s n start the program with a simulated smr
-v off
runs the program in silent mode (no printing on the screen).
A PPENDIKS
Kildekode
C.1
1 #
#
#
#
#
6
#
haupt.c
include
include
include
include
include
< s i g n a l . h>
< s t d l i b . h>
< s t d i o . h>
< s t r i n g . h>
<math . h>
include " k l i e n t . h "
/ / Konstanter
# define SPEED_LOW 0 . 1
11 # define SPEED 0 . 2
# define SPEED_HIGH 0 . 3
# define TURNR 0 . 2 5
16 # define
# define
# define
# define
# define
21 # define
# define
# define
# define
START_OFFSET_X 0 . 1 8 5
START_OFFSET_Y 0 . 1 8 5
START_OFFSET_TH 3 . 1 4 1 5 / 4
BIN_OFFSET_X 0 . 4 0
BIN_OFFSET_Y 0 . 3 0
ITEM_OFFSET_X 0 . 0 4
ITEM_OFFSET_Y 0 . 0 4
ARENA_X 2 . 1
ARENA_Y 3
26 # define LASER_X_OFFSET 0 . 0 7 9
# define LASER_Y_OFFSET 0 . 0 9 8
# define DODGE_OFFSET 0 . 6
31 # define COLLECT_DIST 0 . 1 5
# define TIME_ROUND 90
# define PI 3 . 1 4 1 5
/ / P a r a m e t e r s som s k a l t w e a k ’ e s . . . : :
# define IRBAT_THR 0 . 1 5
# define IRLEFT_THR 0 . 3 8
# define IRMIDDLE_THR 0 . 3 8
# define IRRIGHT_THR 0 . 3 8
41 # define LINE_THR 1
# define RED_THR 4 . 8
36
# define
# define
46 # define
# define
# define
# define
51
CanU_THR
CanV_THR
PROB_CAN
BottleU_THR
BottleV_THR
PROB_BOTTLE
120
130
0.7
120
125
0.7
/ / Minimum væglængde
# define WALL_MIN 0 . 2
# define CMD_COMPLETE " 0 . 0 0 0 0 0 0 "
# define IRBAT_BLOCKED " 1 . 0 0 0 0 0 0 "
56 # define IRFRONT_BLOCKED " 2 . 0 0 0 0 0 0 "
# define p r i n t L o c ( l o c ) p r i n t f ( " L o c a t i o n \" " # l o c " \ " : (%f , %f , %f ) \n " , l o c . x , l o c . y , l o c . th )
# define p r i n t I t e m ( ) p r i n t f ( " Item : %s\n " , item ? ( item == can ? " can " : " b o t t l e " ) : " none " ) ;
61 # define round ( x ) ( ( x−f l o o r f ( x ) >= 0 . 5 ) ? c e i l f ( x ) : f l o o r f ( x ) )
/ / Srtuct t i l angivelse af positioner
typedef s t r u c t {
95
C
96
66
APPENDIKS C. KILDEKODE
float x ;
float y ;
f l o a t th ;
} location ;
71 void
void
void
void
void
76
void
initialize () ;
setColorVar ( ) ;
waitForCable ( ) ;
catchAlarm ( i n t ) ;
finalize () ;
start () ;
/ / SMRCL commands
81 void forward ( f l o a t d i s t , f l o a t speed ) ;
void turn ( f l o a t angle , i n t using_rad , f l o a t speed ) ;
void t u r n r ( f l o a t radius , f l o a t angle , i n t using_rad , f l o a t speed ) ;
/ / Obstacle avoidance
86 i n t isOpponentDetected ( ) ;
void dodgeOpponent ( ) ;
// Batterifunktioner
int isBatteryDetected ( ) ;
91 i n t i s B a t t e r y S t u c k ( ) ;
int batteryColor ( ) ;
void g r a b B a t t e r y ( ) ;
void f r e e B a t t e r y ( ) ;
void d e l i v e r B a t t e r y ( ) ;
96
/ / F l a s k e− og d å s e f u n k t i o n e r
void hasItem ( ) ;
void d e l i v e r ( ) ;
void t o B a s k e t ( ) ;
101 void c o l l e c t I t e m ( l o c a t i o n itemLoc ) ;
void scanIt ems ( ) ;
/ / V i n k l e r og o d o m e t r i
f l o a t normAngle ( f l o a t v ) ;
106 void updateOdometry ( ) ;
// Posiotionsfunktioner
void g e t P o s i t i o n ( ) ;
void g o t o P o s i t i o n ( l o c a t i o n Loc ) ;
111
/ / F u n k t i o n e r t i l h å n d t e r i n g a f e v e n t s f r a Smrdemo
void getID ( char ∗) ;
i n t isStopped ( char ∗ , c o n s t char ∗) ;
i n t checkStopConditions ( ) ;
116
/ / I n i t i a l i s e r i n g a f v a r i a b l e r mv .
i n t ON = 1 , GAME_ON = 1 , IN_PROCESS = 1 ;
i n t s t a r t _ d o n e = 0 , scanItems_done = 0 , c o l l e c t I t e m _ d o n e = 0 , whichType_done = 0 ;
121 i n t toBasket_done = 0 , d e l i v e r _ d o n e = 0 ;
int motorsBlocked_flag = 0 , o b s t a c l e _ f l a g = 0 , batteryTrapped_flag = 0 ;
i n t dodgeing = 0 ;
float dist = 1;
char msg [STRING_LENGTH] = " \0 " ;
126 f l o a t ans = 0 ;
enum c o l o r s {BLUE , RED} teamColor = BLUE ;
enum items { none , can , b o t t l e } item ;
enum e v e n t s { cmd_complete =1 , motors_blocked , b a t t e r y _ s t u c k , opponent_detected , e v e n t _ t i m e o u t } ;
131
l o c a t i o n s t a r t L o c , curLoc , b o t t l e B i n , canBin , b a t t e r B i n , bins , t e s t 1 , t e s t 2 , b a t S t a r t ;
float
float
136 f l o a t
float
int
int
141 i n t
int
int
scanx , scany , s c a n t h ;
d , alpha ;
xbatmin = 0 . 1 , xbatmax = 2 . 9 , ybatmin = 0 . 1 , ybatmax = 1 . 9 ;
basketx = 0 ;
noMoreItems = 1 ;
obstacleTimes = 0 ;
k = 0;
batNo = 0 ;
basketFound_flag = 0 ;
kl i en tt yp e kt ;
146
/ /−−−−− B e s k r e v e t a f S h a y a n t h a r a n S i v a r a j a l i n g a m −−−−−
i n t main ( i n t argc , char ∗∗argv ) {
/ / Tolker inputtet fra terminalen
C.1. HAUPT.C
151
156
161
166
171
176
/ / S i k r e r a t programmet i k k e s t a r t e r h v i s d e r e r a n g i v e t en f a r v e som i k k e f i n d e s
i f ( a r g c == 4 ) {
k t . hostname = argv [ 1 ] ;
k t . p o r t = 31000 + a t o i ( argv [ 2 ] ) ;
i f ( strcmp ( argv [ 3 ] , " blue " ) == 0 ) {
teamColor = BLUE ;
}
e l s e i f ( strcmp ( argv [ 3 ] , " red " ) == 0 ) {
teamColor = RED ;
}
else {
p r i n t f ( " Usage : haupt <host > <port > <colour >\n " ) ;
exit (1) ;
}
}
else {
p r i n t f ( " Usage : haupt <host > <port > <colour >\n " ) ;
exit (1) ;
}
k l _ c o n n e c t (& k t ) ;
i f ( kt . error )
exit (1) ;
/ / Sætter r e l e v a n t e konstanter afhængig af farven
setColorVar ( ) ;
initialize () ;
waitForCable ( ) ;
181
186
97
/ / V e n t e r på a t s t a r t s n o r e n b l i v e r t r u k k e t ud
while (GAME_ON) {
i f ( IN_PROCESS ) {
/∗ i f ( m o t o r s B l o c k e d _ f l a g ) {
/ / motorsBlocked () ;
IN_PROCESS = 0 ;
GAME_ON = 0 ;
} else i f ( batteryTrapped_flag ) {
batteryTrapped () ;
IN_PROCESS = 0 ;
191
196
} else i f ( deliver_done ) {
scanItems_done = 0;
collectItem_done = 0;
whichType_done = 0 ;
toBasket_done = 0;
deliver_done = 0;
IN_PROCESS = 0 ;
201
} e l s e i f ( toBasket_done ) {
d e l i v e r ( ) ; / / A f l e v e r f l a s k e n e l l e r dåsen .
IN_PROCESS = 0 ;
206
} else if ( collectItem_done ) {
t o B a s k e t ( ) ; / / Kør t i l den r i g t i g e k u r v .
IN_PROCESS = 0 ;
211
} e l s e i f ( toWall_done ) {
c o l l e c t I t e m ( ) ; / / Saml f l a s k e
e l l e r d å s e op .
IN_PROCESS = 0 ;
216
} else
i f (1) {
IN_PROCESS = 0 ;
221
226
} else i f ( start_done ) {
/ / s c a n I t e m s ( ) ; / / Data f r a l a s e r e n b r u g e s t i l a t f i n d e den n æ s t e i t e m .
IN_PROCESS = 0 ;
} e l s e ∗/ i f ( ! s t a r t _ d o n e ) {
start () ;
IN_PROCESS = 0 ;
}
231
else {
p r i n t f ( " J e g ved i k k e hvad j e g s k a l gøre . . . ! \ n " ) ;
IN_PROCESS = 0 ;
GAME_ON = 0 ;
98
}
}
236
IN_PROCESS = 1 ;
}
241
finalize () ;
p r i n t f ( " c l o s i n g \n " ) ;
k l _ d i s c o n n e c t (& k t ) ;
exit (0) ;
246 }
/ /−−−−− B e s k r e v e t a f Martin J u h l J ø r g e n s e n −−−−−
void i n i t i a l i z e ( ) {
kl_send (& kt , " l o g \" $odox \" \" $odoy \" \" $odoth \" \" $ l i n e 0 \" \" $ l i n e 4 \" \" $ i r d i s t r i g h t \"
\" $ i r d i s t f r o n t r i g h t \" \" $ i r d i s t f r o n t m i d d l e \" \" $ i r d i s t f r o n t l e f t \"\n " ) ;
kl_send (& kt , " rs485send 10 1\n " ) ; / / Sæt b a t t e r i s e r v o a r m e n t i l k l a r p o s i t i o n
kl_send (& kt , " rs485send 10 4\n " ) ; / / Luk b a t t e r i b a g p l a d e n
kl_send (& kt , " rs485send 10 6\n " ) ; / / Luk b a t t e r i h o l d e r e n
kl_send (& kt , " rs485send 10 9\n " ) ; / / S l u k b ø r s t e m o t o r e n
256
kl_send (& kt , " i d l e \n " ) ; / / S t o p m o t o r e r n e
}
251
261 void f i n a l i z e ( ) {
kl_send (& kt , " rs485send 10 1\n " ) ; / / Sæt b a t t e r i
kl_send (& kt , " rs485send 10 4\n " ) ; / / å b e n b a t t e r i
kl_send (& kt , " rs485send 10 7\n " ) ; / / å b e n b a t t e r i
kl_send (& kt , " rs485send 10 9\n " ) ; / / S l u k b ø r s t e
266
kl_send (& kt , " i d l e \n " ) ; / / S t o p m o t o r e r n e
}
servoarmen t i l
bagpladen
holderen
motoren
/ / The s i g n a l h a n d l e r c l e a r s t h e f l a g and r e−e n a b l e s i t s e l f .
/ / R e s e t s e r v o s and m o t o r s , and d i s c o n n e c t s
void catchAlarm ( i n t s i g ) {
GAME_ON = 0 ;
s i g n a l ( s i g , catchAlarm ) ;
276
finalize () ;
k l _ d i s c o n n e c t (& k t ) ;
exit (0) ;
}
271
281
/ / Sikrer at robotten ikke kører før s t a r t k a b l e t b l i v e r trukket
void waitForCable ( ) {
int i =0;
286
i n t go_on = 0 ;
i n t avg = 0 ;
291
296
301
306
311
/ / wait f o r the c a b l e to be i n s e r t e d
while ( ! go_on ) {
f o r ( i = 1 ; i < 6 ; i ++) {
kl_send (& kt , " e v a l $ m o t o r s t a t u s r \n " ) ;
avg += a t o i ( k t . answer ) ;
}
go_on = ( avg / ( i −1) == 1 2 8 ) ? 1 : 0 ;
avg = 0 ;
}
/ / wait f o r the c a b l e to be p u l l e d
go_on = 0 ;
while ( ! go_on ) {
f o r ( i = 1 ; i < 6 ; i ++) {
kl_send (& kt , " e v a l $ m o t o r s t a t u s r \n " ) ;
avg += a t o i ( k t . answer ) ;
}
go_on = ( avg / ( i −1) == 0 ) ? 1 : 0 ;
avg = 0 ;
}
/ / E s t a b l i s h a h a n d l e r f o r SIGALRM s i g n a l s .
s i g n a l (SIGALRM, catchAlarm ) ;
/ / S e t an a l a r m t o go o f f i n a l i t t l e w h i l e .
alarm (TIME_ROUND) ;
316 }
klarposition
C.1. HAUPT.C
/ / Sætter r e l e v a n t e p o s i t i o n e r afhængig af hvilken f a r v e r o b o t t e n t i l h ø r e r
321 /∗ Works l i k e a charm 1 1 / 5 2 3 : 3 4 ; −) ∗/
void s e t C o l o r V a r ( ) {
//
//
(0 ,0) i s blue corner , the arena i s the 1. quadrant
//
326
switch ( teamColor ) {
c a s e RED :
s t a r t L o c . y = ARENA_Y − START_OFFSET_Y ;
s t a r t L o c . x = 0 + START_OFFSET_X ;
331
s t a r t L o c . th = −START_OFFSET_TH ;
b o t t l e B i n . y = 0 + BIN_OFFSET_Y ;
b o t t l e B i n . x = ARENA_X − BIN_OFFSET_X ;
b o t t l e B i n . th = −PI / 2 ;
336
canBin . y = 0 + BIN_OFFSET_Y ;
canBin . x = ARENA_X − BIN_OFFSET_X ;
canBin . th = 0 ;
341
346
351
b i n s . y = 0 + BIN_OFFSET_Y ;
b i n s . x = ARENA_X − BIN_OFFSET_X ;
b i n s . th = −PI / 4 ;
test1 . x = 1.5;
test1 .y = 1.5;
t e s t 1 . th = 0 ;
test2 . x = 0.5;
test2 .y = 2.5;
t e s t 2 . th = (3∗ PI ) / 4 ;
batterBin . x = 0 . 0 ;
batterBin . y = 1 . 5 ;
b a t t e r B i n . th = 0 . 0 ;
356
break ;
361
366
c a s e BLUE :
s t a r t L o c . y = 0 + START_OFFSET_Y ;
s t a r t L o c . x = 0 + START_OFFSET_X ;
s t a r t L o c . th = START_OFFSET_TH ;
b o t t l e B i n . y = ARENA_Y − BIN_OFFSET_Y ;
b o t t l e B i n . x = ARENA_X − BIN_OFFSET_X ;
b o t t l e B i n . th = PI / 2 ;
canBin . y = ARENA_Y − BIN_OFFSET_Y ;
canBin . x = ARENA_X − BIN_OFFSET_X ;
canBin . th = 0 ;
371
b i n s . y = ARENA_Y − BIN_OFFSET_Y ;
b i n s . x = ARENA_X − BIN_OFFSET_X ;
b i n s . th = PI / 4 ;
376
381
386
test1 . x = 0.5;
test1 .y = 1.5;
t e s t 1 . th = −PI / 8 ;
test2 . x = 0.5;
test2 .y = 0.5;
t e s t 2 . th = −PI / 4 . 0 ;
batterBin . y = 1 . 5 ;
b a t t e r B i n . th = 0 . 0 ;
break ;
}
391
396
/ / s e t odometry to s t a r t l o c a t i o n
s p r i n t f ( msg , " s e t \" $odox \" %f \n " , s t a r t L o c . x ) ;
kl_send (& kt , msg ) ;
s p r i n t f ( msg , " s e t \" $odoy \" %f \n " , s t a r t L o c . y ) ;
s p r i n t f ( msg , " s e t \" $odoth \" %f \n " , s t a r t L o c . th ) ;
kl_send (& kt , " t a r g e t h e r e \n " ) ;
curLoc = s t a r t L o c ;
401 }
99
100
/ / Gør d e t mere b e k v e m t a t s k r i v e s m r c l−kommandoer
406 void forward ( f l o a t d i s t , f l o a t speed ) {
s p r i n t f ( msg , " fwd %f @v%f : ( $ i r d i s t r i g h t >%f ) | ( ( $ i r d i s t f r o n t l e f t <%f & $ i r d i s t f r o n t m i d d l e
<%f ) |( $ i r d i s t f r o n t m i d d l e <%f & $ i r d i s t f r o n t r i g h t <%f ) |( $ i r d i s t f r o n t r i g h t <%f &
$ i r d i s t f r o n t l e f t <%f ) ) \n " , d i s t , speed , IRBAT_THR , IRLEFT_THR , IRMIDDLE_THR ,
IRMIDDLE_THR , IRRIGHT_THR , IRRIGHT_THR , IRLEFT_THR ) ;
//
s p r i n t f ( msg , " fwd %f @v%f \n " , d i s t , s p e e d ) ;
}
411
void turn ( f l o a t angle , i n t using_rad , f l o a t speed ) {
416
char ∗ rad = using_rad ? " \" rad \" " : " " ;
s p r i n t f ( msg , " turn %f %s @v%f \n " , angle , rad , speed ) ;
}
421
void t u r n r ( f l o a t radius , f l o a t angle , i n t using_rad , f l o a t speed ) {
426
char ∗rad = using_rad ? " \" rad \" " : " " ;
s p r i n t f ( msg , " t u r n r %f %f %s @v%f \n " , radius , angle , rad , speed ) ;
}
431
436
/ / De f ø r s t e i n d l e d e n d e kommandoer
/ /KODEN HERUNDER ER KUN EN TEST−KODE
void s t a r t ( ) {
char id [STRING_LENGTH ] ;
p r i n t f ( " \n================\ n s t a r t ( ) \n================\n " ) ;
/ / R e s e t m o t o r s and p l a c e t a r g e t h e r e ,
/ / a s we h a v e moved t h e r o b o t a f t e r s t a r t i n g t h e SMRdemo s e r v e r
/ / which r e s u l t s i n b l o c k e d m o t o r s
kl_send (& kt , " r e s e t m o t o r s \n " ) ;
441
446
item=can ;
toBasket ( ) ;
getID ( id ) ;
p r i n t f ( " event : %d\n " , checkStopConditions ( id ) ) ;
deliver ( ) ;
getID ( id ) ;
p r i n t f ( " event : %d\n " , checkStopConditions ( id ) ) ;
451
p r i n t f ( ">−− End −−>\n " ) ;
456
start_done = 1;
GAME_ON = 0 ;
kl_send (& kt , " rs485send 10 7\n " ) ;
sleep ( 3 ) ;
461
}
/ /−−−−− B e s k r e v e t a f B r i a n Hansen −−−−−
/ / T j e k k e r om d e r s i d d e r e t b a t t e r i f a s t i t r a g t e n
/ / Returnerer 1 h v i s sandt , e l l e r s 0 .
int isBatteryStuck ( ) {
int battery_stuck = 0 , i = 1;
471
f l o a t s e n s o r = 0 . 0 , avg = 0 . 0 , sum = 0 . 0 ;
466
f o r ( i = 1 ; i < 6 ; i ++) {
/ / i r−s e n s o r e n s i d d e r i p o r t e n t i l den h ø j r e i r−s e n s o r på en n o r m a l SMR−r o b o t
kl_send (& kt , " e v a l $ i r d i s t r i g h t \n " ) ;
s s c a n f ( k t . answer , "%f " ,& s e n s o r ) ;
sum += s e n s o r ;
}
avg = sum/( i −1) ; / / Gennemsnit a f fem s a m p l e s
p r i n t f ( " i r d i s t r i g h t value = %f \n " , avg ) ;
476
481
b a t t e r y _ s t u c k = ( avg > IRBAT_THR ) ? 1 : 0 ; / / P a s s e r e s g r æ n s e v æ r d i e n ?
return battery_stuck ;
}
C.1. HAUPT.C
101
486
/ / T j e k k e r om d e r d e r l i g g e r e t b a t t e r i i bunden a f t r a g t e n .
/ / Returnerer 1 h v i s sandt , e l l e r s 0 .
491 i n t i s B a t t e r y D e t e c t e d ( ) {
int battery_detected = 0 , i = 1;
f o r ( i = 1 ; i < 6 ; i ++) {
kl_send (& kt , " e v a l $ l i n e 0 \n " ) ; / / S e n s o r e n e r k o b l e t t i l l i n e 0 −p o r t e n
}
avg = sum/( f l o a t ) ( i −1) ; / / Gennemsnit a f fem s a m p l e s
b a t t e r y _ d e t e c t e d = ( avg < LINE_THR ) ? 1 : 0 ; / / P a s s e r e s g r æ n s e v æ r d i e n ?
496
501
return battery_detected ;
}
506
/ / T j e k k e r om b a t t e r i e t i t r a g t e n m a t c h e r den t i l d e l t e f a r v e
/ / R e t u r n e r e r RED h v i s r ø d t , BLUE h v i s b l å t og −1 h v i s d e r i k k e l i g g e r n o g e t b a t t e r i i t r a g t e n
int correctBatteryColor ( ) {
511
i n t c o l o r = BLUE , c o r r e c t C o l o r = 0 , i = 0 ;
i n t r e s u l t = −1;
f o r ( i = 1 ; i < 6 ; i ++) {
kl_send (& kt , " e v a l $ l i n e 4 \n " ) ; / / S e n s o r e n e r k o b l e t
}
avg = sum/( f l o a t ) ( i −1) ; / / Gennemsnit a f fem s a m p l e s
516
t i l l i n e 0 −p o r t e n
521
c o l o r = ( avg > RED_THR) ? RED : BLUE ;
p r i n t f ( " Color : %d\n " , c o l o r ) ;
526
c o r r e c t C o l o r = ( c o l o r == teamColor ) ? 1 : 0 ;
r e s u l t = i s B a t t e r y D e t e c t e d ( ) ? c o r r e c t C o l o r : −1 ;
p r i n t f ( " R e s u l t : %d\n " , r e s u l t ) ;
return r e s u l t ;
531 }
/ / S e k v e n s a f kommandoer som s t y r e r armen n å r den s k a l s a m l e e t b a t t e r i op f r a t r a g t e n .
536 void g r a b B a t t e r y ( ) {
i n t grabbed = 0 ;
/ / B l i v e r v e d med a t p r ø v e t i l d e r i k k e e r e t b a t t e r i i t r a g t e n mere
while ( ! grabbed ) {
541
546
/ / K ø r e r f r e m f o r a t s i k r e a t b a t t e r i e t e r i bunden a f t r a g t e n
kl_send (& kt , " i d l e \n " ) ;
s p r i n t f ( msg , " d r i v e @v%f : ( $cmdtime > 0 . 3 ) \n " ,SPEED_LOW) ;
kl_send (& kt , " i d l e \n " ) ;
551
/ / Servokommando : Sænk arm
/ / Vent t i l kommandoen e r u d f ø r t
sleep ( 3 ) ;
556
/ / Servokommando : Hæv arm
sleep ( 3 ) ;
561
/ / Servokommando : Arm t i l u d g a n g s p o s i t i o n
sleep ( 2 ) ;
/ / T j e k om d e r s t a d i g e r e t b a t t e r i i t r a g t e n .
if ( ! isBatteryDetected ( ) ) {
grabbed = 1 ;
batNo ++;
p r i n t f ( " Grabbed\n " ) ;
}
566
}
}
102
571
/ / Afviser batteri af forkert farve
void r e j e c t B a t t e r y ( ) {
576
/ / Åben lem i b a g e n d e
kl_send (& kt , " i d l e \n " ) ;
581
while ( i s B a t t e r y D e t e c t e d ( ) ) {/
/ / K ø r s e l f o r a t f o r b a t t e r i e t igennem å b n i n g e n
forward ( 0 . 0 8 ,SPEED_LOW) ;
kl_send (& kt , " i d l e \n " ) ;
turn ( 5 0 , 0 ,SPEED_LOW) ;
kl_send (& kt , " i d l e \n " ) ;
forward ( 0 . 1 , SPEED_LOW) ;
getID ( id ) ;
586
/ / T j e k f o r f e j l der måtte o p s t å under k ø r s l e n
/ / samt r e g i s t r e r n å r kommandoen e r u d f ø r t
switch ( checkStopConditions ( id ) ) {
c a s e cmd_complete :
/ / Kommandoen e r u d f ø r t og lemmen l u k k e s i g e n
kl_send (& kt , " i d l e \n " ) ;
return ;
break ;
591
596
601
c a s e motors_blocked :
break ;
case battery_stuck :
break ;
606
c a s e opponent_detected :
break ;
case event_timeout :
break ;
611
}
}
/ / Lemmen l u k k e s
616
}
621
626
/ / F r i g ø r e l s e s m a n ø v r e som k ø r e s h v i r e t b a t t e r i s i d d e r f a s t i t r a g t e n .
void f r e e B a t t e r y ( ) {
enum e v e n t s event ;
/ / D o b b e l t−t j e k om b a t t e r i e t
while ( i s B a t t e r y S t u c k ( ) ) {
siider fast
631
/ / S e k v e n s a f kommandoer f o r a t f r i g ø r e b a t t e r i e t
forward ( −0.1 ,SPEED_LOW) ;
turn ( 1 0 , 0 ,SPEED_LOW) ;
getID ( id ) ;
636
event = checkStopConditions ( id ) ;
641
646
switch ( event ) {
/ / S e k v e n s a f kommandoer h v i s m o t o r e r n e b l i v e b l o k e r e t
forward ( −0.3 ,SPEED ) ;
getID ( id ) ;
checkStopConditions ( id ) ;
break ;
break ;
651
break ;
C.1. HAUPT.C
103
break ;
656
break ;
}
}
661 }
/ / F i n d e r b e h o l d e r e n t i l b a t t e r i e r n e på banen og a f l e v e r e r dem
void d e l i v e r B a t t e r y ( ) {
666
batStart . x = 0.5;
batStart . y = 1.5;
b a t S t a r t . th = 0 . 5 ;
671
i f ( b a t t e r B i n . x == 0 . 0 ) {
gotoPosition ( batStart ) ;
676
/ / Kører i n d t i l motorerne b l i v e r b l o k e r e t
s p r i n t f ( msg , " fwd 2 @v%f \n " ,SPEED_LOW) ;
getID ( id ) ;
681
kl_send (& kt , " g e t e v e n t 10\n " ) ;
p r i n t f ( "%s " , k t . answer ) ;
/ / Konkluderer hvor b a t t e r i b e h o l d e r e n er p l a c e r e t
getPosition ( ) ;
686
i f ( curLoc . x > 0 . 5 ) {
i f ( curLoc . x > 0 . 7 5 ) {
i f ( curLoc . x > 1 . 0 ) {
i f ( curLoc . x > 1 . 4 ) {
p r i n t f ( " The b a s k e t was not found ! ! ! " ) ;
}
}
else {
}
}
else {
batterBin . x = 0.85;
}
}
else {
}
691
696
701
706
711
s p r i n t f ( msg , " fwd −0.3 @v%f \n " ,SPEED ) ;
getID ( id ) ;
break ;
716
break ;
break ;
721
break ;
break ;
726
}
731
/ / Hvis b e h o l d e r e n b l e v f u n d e t b l i v e r b a t t e r i e r n e l æ s s e t a f i den
i f ( basketFound_flag ) {
batterBin . x = batterBin . x + 0.15;
batterBin . y = batterBin . y + 0 . 2 ;
g o t o P o s i t i o n ( b a t t e r B i n ) ; / / Drops t h e b a t t e r i e s o f f i n t h e b a s k e t
736
kl_send (& kt , " wait 0 . 5 \ n " ) ;
104
741
batNo = 0 ;
}
e l s e { / / E l l e r s s m i d e s d e b l o t e t s t e d på banen
gotoPosition ( batterBin ) ;
kl_send (& kt , " wait 0 . 5 \ n " ) ;
746
batNo = 0 ;
751
}
}
}
/ / T j e k k e r om s e r v e r e n h a r g i v e t e t s v a r s v a r e n d e t i l d e t a k t u e l l e j o b−i d .
/ / Returnerer 1 hvis sandt
i n t isStopped ( char∗ id , c o n s t char∗ stopcond ) {
char tmp [STRING_LENGTH ] ;
761
s t r c p y ( tmp , id ) ;
s t r c a t ( tmp , " stopcond " ) ;
s t r c a t ( tmp , stopcond ) ;
s t r c a t ( tmp , " \n " ) ;
r e t u r n strcmp ( k t . answer , tmp ) == 0 ;
766 }
756
/ / H e n t e r d e t a k t u e l l e j o b−i d .
771 void getID ( char ∗id ) {
char i d b u f [STRING_LENGTH ] ;
s t r c p y ( idbuf , k t . answer ) ;
i d b u f [ k t . answer_len −7] = ’ \0 ’ ;
s t r c p y ( id , i d b u f ) ;
776 }
/ / F o r s ø g e r a t i n d f a n g e en f l a s k e e l l e r d å s e
781 void c o l l e c t I t e m ( l o c a t i o n itemLoc ) {
int state = 0;
f l o a t xMin = 0 . 0 , yMin = 0 . 0 ;
f l o a t thMinX = 0 . 0 , thMinY = 0 . 0 ;
786
f l o a t deltaX = 0 . 0 , deltaY = 0 . 0 ;
l o c a t i o n getLoc ;
p r i n t f ( " \n================\ n c o l l e c t I t e m ( ) \n================\n " ) ;
791
796
801
806
811
816
821
/ / U n d e r s ø g e r h v i l k e n væg d e r e r nærmest
i f ( (ARENA_X/ 2 . 0 − itemLoc . x ) > 0 ) {
/ / Væg v e d minimum y
xMin = itemLoc . x ;
thMinX = PI ;
d e l t a X = itemLoc . x + COLLECT_DIST ;
}
else {
/ / Væg v e d maksimum y
xMin = ARENA_X − itemLoc . x ;
thMinX = 0 ;
d e l t a X = itemLoc . x − COLLECT_DIST ;
}
i f ( (ARENA_Y/ 2 . 0 − itemLoc . y ) > 0 ) {
/ / Væg v e d minimum x
yMin = itemLoc . y ;
thMinY = −PI / 2 . 0 ;
d e l t a Y = itemLoc . y + COLLECT_DIST ;
}
else {
/ / Væg v e d maksimum x
yMin = ARENA_Y − itemLoc . y ;
thMinY = PI / 2 . 0 ;
d e l t a Y = itemLoc . y − COLLECT_DIST ;
}
/ / Finder udgangspositionen t i l opsamlingen
getLoc = itemLoc ;
i f ( xMin < yMin ) {
getLoc . x += d e l t a X ;
C.1. HAUPT.C
105
getLoc . th = thMinX ;
826
}
else {
getLoc . y += d e l t a Y ;
getLoc . th = thMinY ;
}
831
g o t o P o s i t i o n ( getLoc ) ;
/ / T j e k k e r om e t e l e m e n t a l l e r e d e e r b l e v e t o p s a m l e t v e d e t t i l f æ l d e
hasItem ( ) ;
836
/ / F o r s ø g e r i n d t i l e l e m e n t e t e r s a m l e t op
while ( item == none ) {
i f ( s t a t e == 0 ) {
kl_send (& kt , " d r i v e @v0 . 1 \ n " ) ;
getID ( id ) ;
p r i n t f ( " \nID : %s\n " , id ) ;
}
841
i f ( s t a t e == 0 ) {
getID ( id ) ;
s t a t e ++;
}
else {
sleep ( 1 ) ;
forward ( −0.15 ,SPEED_LOW) ;
getID ( id ) ;
}
break ;
846
851
856
i f ( s t a t e == 1 ) {
/ / Det t j e k k e s om d e r e r i n d f a n g e t e t e l e m e n t e f t e r r u t i n e n
hasItem ( ) ;
state = 0;
}
break ;
861
866
break ;
break ;
871
break ;
}
876
}
collectItem_done = 1;
}
881
/ / B r u g e r webcam t i l a f d e t e k t e r e h v o r v i d t d e r e r i n d f a n g e t en f l a s k e r
void hasItem ( ) {
f l o a t pCan = 0 . 0 , p B o t t l e = 0 . 0 ;
f l o a t avg_pCan = 0 . 0 , a v g _ p B o t t l e = 0 . 0 ;
int i = 0;
e l l e r dåse
886
f o r ( i = 1 ; i < 6 ; i ++) {
s p r i n t f ( msg , " v i s i o n \"BOC thresholdCanU=%d thresholdCanV=%d t h r e s h o l d B o t t l e U=%
d t h r e s h o l d B o t t l e V=%d\"\n " ,CanU_THR, CanV_THR , BottleU_THR , BottleV_THR ) ;
kl_send (& kt , " e v a l $ v i s 0 ; $ v i s 1 \n " ) ;
s s c a n f ( k t . answer , "%f %f " ,&pCan , &p B o t t l e ) ;
avg_pCan += pCan ;
a v g _ p B o t t l e += p B o t t l e ;
}
avg_pCan /= ( f l o a t ) ( i −1) ;
a v g _ p B o t t l e /= ( f l o a t ) ( i −1) ;
891
896
i f ( avg_pCan > PROB_CAN || a v g _ p B o t t l e > PROB_BOTTLE)
{
item = ( avg_pCan > a v g _ p B o t t l e ) ? can : b o t t l e ;
}
else {
item = none ;
}
printItem ( ) ;
901
906
}
106
/ / K ø r e r t i l f l a s k e − og d å s e b e h o l d e r n e f o r a t a f l e v e r e
911 void t o B a s k e t ( ) {
l o c a t i o n bin = b i n s ;
i n t go_on = 0 ;
p r i n t f ( " Item : %s " , item ? ( item == can ? " can " : " b o t t l e " ) : " none " ) ;
p r i n t f ( " b i n s . th : %f \n " , b i n s . th ) ;
916
i f ( item == can ) {
bin = canBin ;
}
e l s e i f ( item == b o t t l e ) {
bin = b o t t l e B i n ;
}
921
926
g o t o P o s i t i o n ( bin ) ;
forward ( 0 . 1 5 , SPEED_LOW) ;
getID ( id ) ;
while ( ! go_on ) {
/ / T j e k k e r f o r f e j l d e r m å t t e o p s t å u n d e r k ø r s l e n samt r e g i s t r e r n å r kommandoen
er udført
forward ( −0.1 , SPEED ) ;
getPosition ( ) ;
break ;
931
936
go_on = 1 ;
break ;
941
break ;
break ;
946
break ;
}
g o t o P o s i t i o n ( bin ) ;
forward ( 0 . 1 5 , SPEED_LOW) ;
getID ( id ) ;
951
}
go_on = 0 ;
956
toBasket_done = 1 ;
}
/ / Afleverer det indfangede element
void d e l i v e r ( ) {
p r i n t f ( " \n================\ n d e l i v e r ( ) \n================\n " ) ;
966
int state = 0;
i n t moreItems = 1 ;
enum items old_item = item ;
961
while ( moreItems ) {
971
if (! state ) {
kl_send (& kt , " d r i v e @v0 . 1 \ n " ) ;
}
976
981
986
991
getID ( id ) ;
if (! state ) {
p r i n t f ( " bak , ID=%s\n " , id ) ;
s t a t e ++;
}
break ;
/ / S t a r t b ø r s t e n og k ø r f r e m i g e n
p r i n t f ( " s t a r t b Ã¸r s t e \n " ) ;
forward ( 0 . 0 2 ,SPEED_LOW) ;
p r i n t f ( " fremad\n " ) ;
C.1. HAUPT.C
107
s t a t e ++;
break ;
break ;
996
break ;
1001
break ;
}
p r i n t f ( " s t a t e = %d\n " , s t a t e ) ;
1006
i f ( s t a t e == 2 ) {
s l e e p ( 1 ) ; / / Ven e t ø j e b l i k på a f l e v e r i n g e n
kl_send (& kt , " rs485send 10 9\n " ) ; / / s l u k b ø r s t e m o t o r
p r i n t f ( " s l u k b r s t e \n " ) ;
p r i n t f ( " bak\n " ) ;
1011
p r i n t f ( "AFLEVERET ! ! ! ON = 0\n " ) ;
/ / T j e k om v i h a r f a n g e t n o g e t mere , a f samme t y p e v e l a t mærke
hasItem ( ) ;
1016
moreItems = ( item ! = none && old_item == item ) ? 1 : 0 ;
old_item = item ;
state = 0;
1021
}
}
deliver_done = 1 ;
1026 }
/ / K o n v e r t e r e r v i n k l e r t i l smrdemo ’ s i n t e r v a l f r a −PI t i l PI
1031 f l o a t normAngle ( f l o a t v ) {
r e t u r n v − 2 ∗ PI ∗ round ( v/(2∗ PI ) ) ;
}
1036
/ / Opdaterer de t r e o d o m e t r i v a r i a b l e r
void updateOdometry ( ) {
f l o a t r = 0 . 0 , th = 0 . 0 , t h _ s t a r t = 0 . 0 , th_end = 0 . 0 , l = 0 . 0 ;
float v = 0.0 , f = 0.0 , u = 0.0;
1041
1046
/ / Nye v a r i a b l e r
f l o a t thNorm = 0 . 0 , thSum = 0 . 0 , t h R e a l = 0 . 0 ;
f l o a t rCur = 0 . 0 , r R e a l = 0 . 0 , r O f f s e t = 0 . 0 ;
f l o a t varValue = 0 . 0 ;
char varToUpd [STRING_LENGTH] = " " ;
kl_send (& kt , " flushcmds\n " ) ;
1051
1056
1061
1066
p r i n t f ( " updateOdometry ( ) \n " ) ;
/ / Scan e f t e r l i n j e s t y k k e r og gem p a r a m e t r e
kl_send (& kt , " l a s e r \" s f p o l a r _ w a l l \"\n " ) ;
p r i n t f ( " s v a r : %s\n " , k t . answer ) ;
kl_send (& kt , " e v a l $ l 0 ; $ l 1 ; $ l 2 ; $ l 3 ; $ l 4 \n " ) ;
s s c a n f ( k t . answer , "%f %f %f %f %f " ,& t h _ s t a r t ,& th_end ,& r ,& th ,& l ) ;
p r i n t f ( " Theta−s t a r t : %f \n " , t h _ s t a r t ) ;
getPosition ( ) ;
p r i n t L o c ( curLoc ) ;
/ / Beregner robottens f a k t i s k e o r i e n t e r i n g
thNorm = normAngle ( th ) ;
thSum = normAngle ( curLoc . th+thNorm ) ;
1071
1076
/ / Umulige v i n k l e r . . .
i f ( thNorm == PI || thNorm == −PI || l < WALL_MIN) {
return ;
}
/ / P o s i t i v x−r e t n i n g
108
e l s e i f ( ( thSum > −PI /4) && ( thSum < PI /4) ) {
t h R e a l = −thNorm ;
rCur = ARENA_X − curLoc . x ;
r O f f s e t = ARENA_X;
s p r i n t f ( varToUpd , " $odox " ) ;
}
/ / N e g a t i v y−r e t n i n g
e l s e i f ( ( thSum < −PI /4) && ( thSum > −3∗PI /4) ) {
t h R e a l = −PI /2 − thNorm ;
rCur = curLoc . y ;
rOffset = 0.0;
s p r i n t f ( varToUpd , " $odoy " ) ;
}
/ / P o s i t i v y−r e t n i n g
e l s e i f ( ( thSum > PI /4) && ( thSum < 3∗ PI /4) ) {
t h R e a l = PI /2 − thNorm ;
rCur = ARENA_Y − curLoc . y ;
r O f f s e t = ARENA_Y;
s p r i n t f ( varToUpd , " $odoy " ) ;
}
/ / N e g a t i v x−r e t n i n g
e l s e i f ( ( thSum > 3∗ PI /4 && thSum < PI ) || ( thSum < −3∗PI /4 && thSum > −PI ) || thSum ==
PI || thSum == −PI ) {
i f ( thNorm > 0 ) {
t h R e a l = PI − thNorm ;
}
else {
t h R e a l = −PI − thNorm ;
}
rCur = curLoc . x ;
rOffset = 0.0;
s p r i n t f ( varToUpd , " $odox " ) ;
}
/ / Grænsetilfælde
else {
/ / Intet gøres
return ;
}
1081
1086
1091
1096
1101
1106
1111
/ / B e r e g n e r a f s t a n d e n f r a r o b o t t e n s centrum t i l den d e t e k t e r e d e væg
v = a t a n f ( LASER_X_OFFSET / LASER_Y_OFFSET ) ;
f = s q r t (pow( LASER_X_OFFSET , 2 ) + pow( LASER_Y_OFFSET , 2 ) ) ;
1116
/ / De t o t i l f æ l d e
i f ( thNorm > v ) {
u = thNorm − v ;
rReal = r − f ∗ sin (u) ;
} else {
u = v − thNorm ;
rReal = r + f ∗ sin (u) ;
}
1121
1126
/ / U n d e r s ø g e r om en o p d a t e r i n g e r n ø d v e n d i g
i f ( r R e a l ! = rCur ) {
varValue = ( r O f f s e t > 0 . 0 ) ? r O f f s e t − r R e a l : r R e a l ;
s p r i n t f ( msg , " s e t \"%s \" %f \n " , varToUpd , varValue ) ;
s p r i n t f ( msg , " s e t \" $odoth \" %f \n " , t h R e a l ) ;
}
p r i n t f ( " r R e a l = %f , t h R e a l = %f \n " , rReal , t h R e a l ) ;
1131
1136
}
1141
/ / Henter r o b o t t e n s a k t u e l l e p o s i t i o n
void g e t P o s i t i o n ( ) {
f l o a t x , y , th ;
1146
kl_send (& kt , " e v a l $odox ; $odoy ; $odoth\n " ) ;
s s c a n f ( k t . answer , "%f %f %f " ,&x ,&y,& th ) ;
curLoc . x = x ;
curLoc . y = y ;
curLoc . th = th ;
1151 }
/ / F i n d e r den n æ r m e s t e e l e m e n t e r som s k a l s a m l e s op
1156 void scanIt ems ( i n t n C l o s e s t ) {
f l o a t r = 0 . 0 , th = 0 . 0 , l = 0 . 0 ;
p r i n t f ( " \n================\nscanItems ( ) \n================\n " ) ;
C.1. HAUPT.C
109
1161
1166
1171
/ / K a l d e r l a s e r s c a n n e r e n p a r a m e t r e n e f o r den n ’ t e n æ r m e s t e f l a s k e
s p r i n t f ( msg , " l a s e r \" s f n e a r e s t _ i t e m %d\"\n " , n C l o s e s t ) ;
kl_send (& kt , " e v a l $ l 0 ; $ l 1 ; $ l 2 \n " ) ;
s s c a n f ( k t . answer , "%f %f %f " ,& r ,& th ,& l ) ;
e l l e r dåse
scanItems_done = 1 ;
1176 }
/ / K ø r e r r o b o t t e n f r a den nuværende p o s i t i o n t i l en ny
1181 void g o t o P o s i t i o n ( l o c a t i o n newLoc ) {
int gotoPosition_done = 0 ;
i n t c o r r e c t B a t t e r y = 0 , blocked_count = 0 ;
1186
f l o a t v = 0 . 0 , dx = 0 . 0 , dy = 0 . 0 ;
f l o a t phase = 0 . 0 ;
i n t timeout_count = 0 ;
p r i n t f ( " \n================\n g o t o P o s t i o n ( ) \n================\n " ) ;
1191
1196
while ( ! g o t o P o s i t i o n _ d o n e ) {
/ / Hent nuværende p o s i t i o n
getPosition ( ) ;
p r i n t L o c ( newLoc ) ;
/ / Beregn v i n k e l
dx = newLoc . x − curLoc . x ;
dy = newLoc . y − curLoc . y ;
1201
i f ( dx < 0 ) {
phase = PI ;
} else {
phase = 0 ;
1206
}
v = normAngle ( a t a n f ( dy/dx ) + phase ) ;
1211
alpha = v − curLoc . th ;
p r i n t f ( " v = %f \n " , v ) ;
1216
/ / Drej
turn ( alpha , 1 , SPEED ) ;
getID ( id ) ;
p r i n t f ( " alpha = %f \n " , alpha ) ;
1221
/ / Vent t i l d r e j e t e r u d f ø r t
while (ON) {
i f ( isStopped ( id ,CMD_COMPLETE) ) {
ON = 0 ;
}
}
ON = 1 ;
1226
/ / Hent p o s i t i o n i g e n
getPosition ( ) ;
1231
/ / Beregn a f s t a n d
d = s q r t f ( powf ( ( newLoc . x − curLoc . x ) , 2 ) + powf ( ( newLoc . y − curLoc . y ) , 2 ) ) ;
1236
/ / Kør
forward ( d , SPEED_HIGH) ;
getID ( id ) ;
1241
p r i n t f ( " event = %d\n " , event ) ;
switch ( event ) {
110
1246
blocked_count ++;
i f ( blocked_count > 3 ) { / / T i l l a d kun a t m o t o r b l o k e r i n g t r e
gange
gotoPosition_done = 1 ;
}
break ;
1251
gotoPosition_done = 1 ;
1256
break ;
1261
correctBattery = correctBatteryColor ( ) ;
i f ( c o r r e c t B a t t e r y == 1 ) {
p r i n t f ( " Grapping b a t t e r y \n " ) ;
grabBattery ( ) ;
}
e l s e i f ( c o r r e c t B a t t e r y == 0 ) {
p r i n t f ( " R e j e c t i n g b a t t e r y \n " ) ;
rejectBattery () ;
}
1266
1271
break ;
1276
p r i n t f ( " Dodge . . \ n " ) ;
getPosition ( ) ;
dodgeOpponent ( ) ;
p r i n t f ( " Dodged ! \ n " ) ;
1281
break ;
i f ( timeout_count <3) {
timeout_count ++;
}
else {
gotoPosition_done =1;
1286
1291
}
break ;
}
1296
}
1301
/ / Drej
getPosition ( ) ;
turn ( newLoc . th−curLoc . th , 1 , SPEED ) ;
getID ( id ) ;
while (ON) {
i f ( isStopped ( id ,CMD_COMPLETE) ) {
ON = 0 ;
}
}
1306
ON = 1 ;
getPosition ( ) ;
1311
}
/ / F u n k t i o n e n a f g ø r om i r−s e n s o r e r n e d e t e k t e r e r en f j e n d e
i n t isOpponentDetected ( ) {
float sensor_right = 0.0 , avg_sensor_right = 0 . 0 ;
f l o a t sensor_middle = 0 . 0 , avg_sensor_middle = 0 . 0 ;
1321
float sensor_left = 0.0 , avg_sensor_left = 0 . 0 ;
int i = 0;
int result = 0;
1316
1326
f o r ( i = 1 ; i < 1 0 ; i ++) {
/ / De t r e f r o n t s e n s o r e r b r u g e s
kl_send (& kt , " e v a l $ i r d i s t f r o n t r i g h t ; $ i r d i s t f r o n t m i d d l e ; $ i r d i s t f r o n t l e f t \n " )
;
s s c a n f ( k t . answer , "%f %f %f " ,& s e n s o r _ r i g h t ,& sensor_middle ,& s e n s o r _ l e f t ) ;
C.1. HAUPT.C
111
a v g _ s e n s o r _ r i g h t += s e n s o r _ r i g h t ;
avg_sensor_middle += sensor_middle ;
a v g _ s e n s o r _ l e f t += s e n s o r _ l e f t ;
1331
}
a v g _ s e n s o r _ r i g h t /= ( f l o a t ) ( i −1) ; / / Gennemsnit a f n i v æ r d i e r
avg_sensor_middle /= ( f l o a t ) ( i −1) ;
a v g _ s e n s o r _ l e f t /= ( f l o a t ) ( i −1) ;
1336
getPosition ( ) ;
/ / T a g e r h ø j d e f o r p e r s o n e r o . l i g n . o m k r i n g banen
i f ( curLoc . x > DODGE_OFFSET && curLoc . x < ARENA_X − DODGE_OFFSET && curLoc . y >
DODGE_OFFSET && curLoc . y < ARENA_Y − DODGE_OFFSET) {
r e s u l t = ( a v g _ s e n s o r _ r i g h t < IRRIGHT_THR || avg_sensor_middle < IRMIDDLE_THR
|| a v g _ s e n s o r _ l e f t < IRLEFT_THR ) ? 1 : 0 ;
}
else {
result = 0;
}
1341
1346
}
1351
/ / R o b o t t e n s r e a k t i o n h v i s en f j e n d e e r d e t e k t e r e t
void dodgeOpponent ( ) {
p r i n t f ( " \n================\ndodgeOpponent ( ) \n================\n " ) ;
1356
f l o a t angle = 0 . 0 ;
1361
/ / D o b b e l t−t j e k om f j e n d e n s t a d i g e r d e r
while ( isOpponentDetected ( ) ) {
p r i n t f ( " Opponent s t i l l t h e r e ! \ n " ) ;
getPosition ( ) ;
p r i n t f ( " Current l o c a t i o n : \ n " ) ;
1366
1371
1376
1381
/ / D r e j væk f r a den n æ r m e s t e væg
i f ( curLoc . th > PI / 4 . 0 && curLoc . th < 3 . 0 ∗ PI / 4 . 0 ) {
angle = ( curLoc . x > ARENA_X/ 2 . 0 ) ? PI / 2 . 0 : −PI / 2 . 0 ;
}
e l s e i f ( curLoc . th < −PI / 4 . 0 && curLoc . th > −3.0∗ PI / 4 . 0 ) {
angle = ( curLoc . x > ARENA_X/ 2 . 0 ) ? −PI / 2 . 0 : PI / 2 . 0 ;
}
e l s e i f ( ( curLoc . th >= 3∗ PI /4 && curLoc . th <= PI ) || ( curLoc . th <= −3.0∗ PI / 4 . 0
&& curLoc . th >= −PI ) ) {
angle = ( curLoc . y > ARENA_Y/ 2 . 0 ) ? PI / 2 . 0 : −PI / 2 . 0 ;
}
e l s e i f ( curLoc . th >= −PI /4 && curLoc . th <= PI /4) {
angle = ( curLoc . y > ARENA_Y/ 2 . 0 ) ? −PI / 2 . 0 : PI / 2 . 0 ;
}
else {
angle = PI ;
}
p r i n t f ( " Dodge Angle : %f \n " , angle ) ;
1386
1391
1396
/ / Kør udenom f j e n d e n
kl_send (& kt , " i g n o r e o b s t a c l e s \n " ) ;
turn ( angle , 1 , SPEED ) ;
forward ( 0 . 5 , SPEED ) ;
turn(−angle , 1 , SPEED ) ;
forward ( 0 . 5 , SPEED ) ;
getID ( id ) ;
event = 0 ;
1401
1406
while ( event ! = cmd_complete ) {
p r i n t f ( " Event : %d\n " , event ) ;
switch ( event ) {
getID ( id ) ;
return ;
break ;
112
1411
break ;
break ;
1416
break ;
1421
break ;
}
}
}
1426 }
1431
/ / H å n d t e r i n g a f f e j l og a n d r e e v e n t s
i n t checkStopConditions ( char ∗id ) {
enum e v e n t s event = 0 ;
p r i n t f ( " checkStopConditions\n " ) ;
1436
while ( ! event ) {
// Batteri sidder fast
i f ( isStopped ( id , IRBAT_BLOCKED) ) {
kl_send (& kt , " i d l e \n " ) ;
1441
/ / Vent f o r a t være s i k k e r på a t b a t t e r i e t r e n t f a k t i s k s i d d e r f a s t
p r i n t f ( "IRBAT_BLOCKED\n " ) ;
sleep ( 1 ) ;
p r i n t f ( " s l e e p i n g over\n " ) ;
if ( isBatteryStuck ( ) ) {
p r i n t f ( " f r e e b a t t e r y \n " ) ;
freeBattery () ;
}
1446
1451
event = b a t t e r y _ s t u c k ;
}
1456
/ / Fjenden er d e t e k t e r e t
e l s e i f ( isStopped ( id , IRFRONT_BLOCKED) ) {
event = opponent_detected ;
}
1461
/ / Blokerede motorer
e l s e i f ( strcmp ( k t . answer , " m o t i o n c o n t r o l blocked\n " ) == 0 ) {
p r i n t f ( " Motors blocked\n " ) ;
kl_send (& kt , " i d l e \n " ) ;
kl_send (& kt , " r e s e t m o t o r s \n " ) ;
1466
event = motors_blocked ;
1471
}
1476
/ / Kommandoen e r u d f ø r t
e l s e i f ( isStopped ( id ,CMD_COMPLETE) ) {
event = cmd_complete ;
}
/ / Event timout
e l s e i f ( strcmp ( k t . answer , " e v e n t t i m e o u t\n " ) == 0 ) {
kl_send (& kt , " i d l e \n " ) ;
kl_send (& kt , " i d l e \n " ) ;
p r i n t f ( " Dead by timeout\n " ) ;
1481
event = e v e n t _ t i m e o u t ;
1486
}
}
r e t u r n event ;
}
C.2. KLIENT.H
C.2
klient.h
# i f n d e f KLIENT_DOT_H
# define KLIENT_DOT_H
#
5 #
#
#
#
#
10 #
#
#
#
#
15
#
include
include
include
include
include
include
include
include
include
include
include
< s t d i o . h>
< s t d l i b . h>
< u n i s t d . h>
<sys/t y p e s . h>
<sys/ s o c k e t . h>
< n e t i n e t / i n . h>
<netdb . h>
< f c n t l . h>
< s t r i n g . h>
<pthread . h>
< s i g n a l . h>
define STRING_LENGTH 100
typedef s t r u c t
20 {
/ / User i n p u t :
char ∗hostname ;
int port ;
25
// Internal variables :
char answer [STRING_LENGTH ] ;
char queue [STRING_LENGTH ] [ STRING_LENGTH ] ;
i n t answer_len ;
i n t queue_cnt ;
30
/ / Error value :
int error ;
/ / R e t u r n s a number d i f f e r e n t f r o m 0 i f an e r r o r o c c u r s
35
/ / Output v a l u e :
i n t sockfd ;
} klienttype ;
void
40 void
void
void
void
kl_connect ( klienttype ∗k l i e n t ) ;
kl_disconnect ( klienttype ∗klient ) ;
kl_send ( k l i e n t t y p e ∗ k l i e n t , char ∗cmd) ;
k l _ w a i t ( k l i e n t t y p e ∗ k l i e n t , char ∗event ) ;
kl_cmdwait ( k l i e n t t y p e ∗ k l i e n t , char ∗cmd) ;
45 # endif / / KLIENT_DOT_H
113
114
C.3
klient.c
# include " k l i e n t . h "
void k l i e n t _ i n i t ( k l i e n t t y p e ∗ k l i e n t ) ;
5 void k l _ c o n n e c t ( k l i e n t t y p e ∗ k l i e n t )
{
s t r u c t sockaddr_in serv_addr ;
s t r u c t h o s t e n t ∗host_param ;
i f ( ( k l i e n t −>sockfd = s o c k e t ( AF_INET , SOCK_STREAM, 0 ) ) == −1)
{
f p r i n t f ( s t d e r r , " Can not c r e a t e s o c k e t \n " ) ;
k l i e n t −>e r r o r = 1 ;
return ;
}
10
15
i f ( ( host_param = gethostbyname ( k l i e n t −>hostname ) ) == NULL)
{
f p r i n t f ( s t d e r r , " Can not connect t o %s\n " , k l i e n t −>hostname ) ;
k l i e n t −>e r r o r = 2 ;
return ;
}
20
bzero ( ( char ∗) &serv_addr , s i z e o f ( serv_addr ) ) ;
serv_addr . s i n _ f a m i l y = AF_INET ;
bcopy ( ( char ∗) host_param−>h_addr , ( char ∗) &serv_addr . sin_addr . s_addr , host_param−>h_l engt h
);
serv_addr . s i n _ p o r t = htons ( k l i e n t −>p o r t ) ;
25
i f ( connect ( k l i e n t −>sockfd , ( s t r u c t sockaddr ∗) &serv_addr , s i z e o f ( serv_addr ) ) ! = 0 )
{
f p r i n t f ( s t d e r r , " E r r o r while t r y i n g t o connect t o s o c k e t on : %s , p o r t : %d\n " , k l i e n t −>
hostname , k l i e n t −>p o r t ) ;
k l i e n t −>e r r o r = 3 ;
return ;
}
30
35
klient_init ( klient ) ; / /
k l i e n t −>e r r o r = 0 ;
return ;
Initialize
klient
}
40
void k l _ d i s c o n n e c t ( k l i e n t t y p e ∗ k l i e n t )
{
send ( k l i e n t −>sockfd , " e x i t \n " , 5 , 0 ) ;
45
usleep (100000) ;
c l o s e ( k l i e n t −>sockfd ) ;
k l i e n t −>e r r o r = 0 ;
return ;
50 }
void kl_send ( k l i e n t t y p e ∗ k l i e n t , char ∗cmd)
{
int len ;
55
l e n = send ( k l i e n t −>sockfd , cmd , s t r l e n (cmd) , 0 ) ;
60
i f ( l e n ! = s t r l e n (cmd) )
k l i e n t −>e r r o r = 1 ;
else
k l i e n t −>e r r o r = 0 ;
k l i e n t −>answer_len = r e c v ( k l i e n t −>sockfd , k l i e n t −>answer , STRING_LENGTH, 0 ) ;
k l i e n t −>answer [ k l i e n t −>answer_len ] = ’ \0 ’ ;
65 # i f d e f KLIENT_DEBUG
p r i n t f ( "ANS: l e n = %d || %s " , k l i e n t −>answer_len , k l i e n t −>answer ) ;
# endif
return ;
70 }
void k l _ w a i t ( k l i e n t t y p e ∗ k l i e n t , char ∗event )
{
do
75
{
kl_send ( k l i e n t , " g e t e v e n t 10000\n " ) ;
} while ( strcmp ( k l i e n t −>answer , event ) ! = 0 ) ;
80
k l i e n t −>e r r o r = 0 ;
return ;
C.3. KLIENT.C
}
85
void kl_cmdwait ( k l i e n t t y p e ∗ k l i e n t , char ∗cmd)
{
char i n b u f [STRING_LENGTH] = { 0 } , i d b u f [STRING_LENGTH] = { 0 } ;
kl_send ( k l i e n t , cmd) ;
s t r c p y ( idbuf , k l i e n t −>answer ) ;
i d b u f [ k l i e n t −>answer_len −7] = ’ \0 ’ ;
s t r c a t ( idbuf , " stopcond " ) ;
90
do
{
kl_send ( k l i e n t , " g e t e v e n t 10000\n " ) ;
s t r c p y ( inbuf , k l i e n t −>answer ) ;
i f ( k l i e n t −>answer_len > 1 0 )
i n b u f [ k l i e n t −>answer_len −10] = ’ \0 ’ ;
/ / p r i n t f ( " INBUF : %s || IDBUF : %s \n " , i n b u f , i d b u f ) ;
} while ( strcmp ( idbuf , i n b u f ) ! = 0 ) ;
95
100
k l i e n t −>e r r o r = 0 ;
return ;
}
105 void k l i e n t _ i n i t ( k l i e n t t y p e ∗ k l i e n t )
{
k l i e n t −>answer_len = 0 ;
k l i e n t −>queue_cnt = 0 ;
110
return ;
}
115
116
C.4
4
9
14
19
myfunc.h
/∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗
∗
C o p y r i g h t (C) 2006 by DTU ( C h r i s t i a n A n d e r s e n )
∗
∗
j c a @ o e r s t e d . d t u . dk
∗
∗
∗
∗
T h i s program i s f r e e s o f t w a r e ; you can r e d i s t r i b u t e i t and / o r m o d i f y ∗
∗
i t u n d e r t h e t e r m s o f t h e GNU L e s s e r G e n e r a l P u b l i c L i c e n s e a s
∗
∗
p u b l i s h e d by t h e F r e e S o f t w a r e F o u n d a t i o n ; e i t h e r v e r s i o n 2 o f t h e
∗
∗
L i c e n s e , o r ( a t y o u r o p t i o n ) any l a t e r v e r s i o n .
∗
∗
∗
∗
T h i s program i s d i s t r i b u t e d i n t h e h o p e t h a t i t w i l l b e u s e f u l ,
∗
∗
b u t WITHOUT ANY WARRANTY; w i t h o u t e v e n t h e i m p l i e d w a r r a n t y o f
∗
∗
MERCHANTABILITY o r FITNESS FOR A PARTICULAR PURPOSE . S e e t h e
∗
∗
GNU L e s s e r G e n e r a l P u b l i c L i c e n s e f o r more d e t a i l s .
∗
∗
∗
∗
You s h o u l d h a v e r e c e i v e d a c o p y o f t h e GNU L e s s e r G e n e r a l P u b l i c
∗
∗
L i c e n s e a l o n g w i t h t h i s program ; i f not , w r i t e t o t h e
∗
∗
Free Software Foundation , Inc . ,
∗
∗
59 Temple P l a c e − S u i t e 3 3 0 , B o s t o n , MA 02111 −1307 , USA .
∗
∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗/
/∗ M o d i f i e d by Martin J u h l J Ã¸r g e n s e n
3 May 2007
∗
∗ To u s e f o r t h e E u r o b o t 2007 c o m p e t i t i o n .
∗
∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗/
24 # i f n d e f UFUNC_COG_H
# define UFUNC_COG_H
# include < c s t d l i b >
29 # include <urob4/ufunctioncambase . h>
// / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / /
// / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / /
// / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / /
34
/∗∗ c a l l e d when s e r v e r makes a d l o p e n ( ) ( l o a d s p l u g i n i n t o memory ) ∗/
void libraryOpen ( ) ;
/∗∗ c a l l e d when s e r v e r makes a d l c l o s e ( ) ( u n l o a d s p l u g i n f r o m memory ) ∗/
39 void l i b r a r y C l o s e ( ) ;
/∗∗
Needed f o r c o r r e c t l o a d i n g and l i n k i n g o f l i b r a r y ∗/
void _ _ a t t r i b u t e _ _ ( ( c o n s t r u c t o r ) ) libraryOpen ( void ) ;
44 void _ _ a t t r i b u t e _ _ ( ( d e s t r u c t o r ) ) l i b r a r y C l o s e ( void ) ;
/∗∗
A l l o w s s e r v e r t o c r e a t e o b j e c t ( s ) o f t h i s c l a s s ∗/
e x t e r n "C" UFunctionCamBase ∗ c r e a t e F u n c ( ) ;
/∗∗
49 . . . and t o d e s t r o y t h e c r e a t e d o b j e c t ( s ) ∗/
e x t e r n "C" void d e l e t e F u n c ( UFunctionCamBase∗ p ) ;
54
// / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / /
// / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / /
// / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / /
/∗∗
T e s t camera f u n c t i o n t o d e m o n s t r a t e handling o f one f u n c t i o n
59 COG
w i t h an ROI ( r e g i o n o f i n t e r e s t − x , y , w, h ) , a t r e s h o l d .
The f u n c t i o n a l l o c a t e s an i m a g e i n image−p o o l ,
c o n v e r t s t h e raw i m a g e t o an RGB i m a g e .
T e s t s a l l p i x e l s i n t h e r e q u e s t e d r e g i o n and
64 c a l c u l a t e s t h e c e n t e r o f g r a v i t y o f t h e p i x e l s
that are above the s p e c i f i e d t r e s h o l d ( in the red channel ) .
Bark
Just sends a reply to the c l i e n t
69
@author C h r i s t i a n A n d e r s e n
∗/
c l a s s MyFunction : public UFunctionCamBase
{
74 public :
/∗∗
C o n s t r u c t o r ∗/
MyFunction ( ) ;
/∗∗
79
D e s t r u c t o r ∗/
v i r t u a l ~MyFunction ( ) ;
/∗∗
C.4. MYFUNC.H
Name o f f u n c t i o n
The r e t u r n e d name i s i n t e n d e d a s i n f o r m a t i v e t o c l i e n t s
and s h o u l d i n c l u d e a v e r s i o n number ∗/
v i r t u a l c o n s t char ∗ name ( ) ;
/∗∗
Returns l i s t o f handled f u n c t i o n s in
o n e s t r i n g s e p a r a t e d by a s p a c e .
e . g . r e t u r n "COG" .
The f u n c t i o n s must b e u n i q u e on t h e s e r v e r . ∗/
v i r t u a l c o n s t char ∗ commandList ( ) ;
/∗∗
R e t u r n s l i s t o f p r o v i d e d r e s s o u r c e s by t h i s module
The f u n c t i o n s must b e u n i q u e on t h e s e r v e r . ∗/
v i r t u a l UResBase ∗ g e t R e s s o u r c e ( c o n s t char ∗ resID )
{ r e t u r n NULL; } ;
/∗∗
S h o u l d r e t u r n a s p a c e s e p a r a t e d l i s t o f r e s s o u r c e s t h a t may
b e p r o v i d e d by t h i s f u n c t i o n .
Note ! i f o t h e r f u n c t i o n s e n h a r i t f r o m t h i s c l a s s t h e same r e s s o u r c e may b e
p r o v i d e d by t h a t c l a s s , s o when t h e c o r e a n n o u n c e s r e s s o u r c e u p d a t e , p l e a s e
u s e t h e p r o v i d e d r e s s o u r c e ( t o a v o i d d u p l i c a t e s ) ∗/
v i r t u a l c o n s t char ∗ r e s o u r c e L i s t ( )
{ return " " ; } ;
/∗∗
S e t s h a r e d r e s o u r c e ∗/
v i r t u a l bool s e t R e s o u r c e ( UResBase ∗ r e s o u r c e , bool remove )
{ r e t u r n UFunctionCamBase : : s e t R e s o u r c e ( r e s o u r c e , remove ) ; } ;
/∗∗
Function t e s t s i f a l l shared r e s o u r c e s are loaded
f o r f u l l f u n c t i o n a l i t y ∗/
v i r t u a l bool g o t A l l R e s o u r c e s ( char ∗ missingThese , i n t missingTheseCnt )
{ r e t u r n UFunctionCamBase : : g o t A l l R e s o u r c e s ( missingThese , missingTheseCnt ) ; } ;
/∗∗
Handle incomming command
( i n t e n d e d f o r command s e p a r a t i o n )
Must r e t u r n t r u e i f t h e f u n c t i o n i s h a n d l e d −
o t h e r w i s e t h e c l i e n t w i l l g e t a f a i l e d − r e p l y ∗/
v i r t u a l bool handleCommand ( UServerInMsg ∗ msg , void ∗ e x t r a ) ;
84
89
94
99
104
109
114
119
private :
/∗∗
F u n c t i o n t o h a n d l e t h e b a r k f u n c t i o n ∗/
124
bool doBark ( UServerInMsg ∗ msg ) ;
/∗∗
E x t r a c t m e s s a g e p a r a m e t e r s and c a l l ’ c a l c u l a t e C O G ( ) ’ ∗/
bool c e n t e r O f G r a v i t y ( UServerInMsg ∗ msg , UImage ∗ pushImg ) ;
/∗∗
129
C a l c u l a t e t h e c e n t e r o f g r a v i t y (COG) .
The COG i s c a l c u l a t e d w i t h i n a r e g i o n o f i n t e r e s t ( ROI )
f o r a l l p i x e l w i t h r e d v a l u e s a b o v e t h e s p e c i f i e d t h r e s h o l d . ∗/
bool calculateCOG ( UServerInMsg ∗ msg , / / c l i e n t r e f e r e n c e e t c
UImage ∗ rawImg , / / YUV raw i m a g e
134
i n t roiX , i n t roiY , i n t roiW , i n t roiH , / / ROI
int threshold ) ; / / t h r e s h o l d
/∗∗
E x t r a c t m e s s a g e p a r a m e t e r s and c a l l ’ c a l c B o t t l e O r C a n ( ) ’ ∗/
bool bottleOrCan ( UServerInMsg ∗ msg , UImage ∗ pushImg ) ;
/∗∗
C a l c u l a t e t h e p r o b a b i l i t y t h a t t h e r e i s a can o r a b o t t l e ,
trapped in the brace . I t i s c a l c u l a t e d within a region o f i n t e r e s t
( ROI ) f o r a l l p i x e l w i t h v a l u e s a b o v e t h e s p e c i f i e d t h r e s h o l d . ∗/
bool c a l c B o t t l e O r C a n ( UServerInMsg ∗ msg , / / c l i e n t r e f e r e n c e e t c
UImage ∗ rawImg , / / YUV raw i m a g e
i n t roiX , i n t roiY , i n t roiW , i n t roiH , / / ROI
i n t areaCan , i n t a r e a B o t t l e , / / a r e a t h a t must b e
/ / covered in coloured
// pixels before triggering
i n t thresholdCanU , i n t thresholdCan , / / t h r e s h o l d s Cans
int thresholdBottleU , int thresholdBottleV ) ; / / t h r e s h o l d s b o t t l e s
139
144
149
bool edge ( UServerInMsg ∗ msg , UImage ∗ pushImg ) ;
154
};
# endif
117
118
C.5
myfunc.cpp
/∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗
∗
C o p y r i g h t (C) 2006 by DTU ( C h r i s t i a n A n d e r s e n )
∗
∗
j c a @ o e r s t e d . d t u . dk
∗
∗
∗
∗
T h i s program i s f r e e s o f t w a r e ; you can r e d i s t r i b u t e i t and / o r m o d i f y ∗
∗
i t u n d e r t h e t e r m s o f t h e GNU L e s s e r G e n e r a l P u b l i c L i c e n s e a s
∗
∗
p u b l i s h e d by t h e F r e e S o f t w a r e F o u n d a t i o n ; e i t h e r v e r s i o n 2 o f t h e
∗
8 ∗
L i c e n s e , o r ( a t y o u r o p t i o n ) any l a t e r v e r s i o n .
∗
∗
∗
∗
T h i s program i s d i s t r i b u t e d i n t h e h o p e t h a t i t w i l l b e u s e f u l ,
∗
∗
b u t WITHOUT ANY WARRANTY; w i t h o u t e v e n t h e i m p l i e d w a r r a n t y o f
∗
∗
MERCHANTABILITY o r FITNESS FOR A PARTICULAR PURPOSE . S e e t h e
∗
13 ∗
GNU L e s s e r G e n e r a l P u b l i c L i c e n s e f o r more d e t a i l s .
∗
∗
∗
∗
You s h o u l d h a v e r e c e i v e d a c o p y o f t h e GNU L e s s e r G e n e r a l P u b l i c
∗
∗
L i c e n s e a l o n g w i t h t h i s program ; i f not , w r i t e t o t h e
∗
∗
Free Software Foundation , Inc . ,
∗
18 ∗
59 Temple P l a c e − S u i t e 3 3 0 , B o s t o n , MA 02111 −1307 , USA .
∗
∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗/
/∗ M o d i f i e d by Martin J u h l J Ã¸r g e n s e n
3 May 2007
∗
∗ To u s e f o r t h e E u r o b o t 2007 c o m p e t i t i o n .
∗
∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗/
23
# include " myfunc . h "
3
# include <ugen4/uimage2 . h>
28
//
//
//
//
/////////////////////////////////////////////////
/////////////////////////////////////////////////
/////////////////////////////////////////////////
/////////////////////////////////////////////////
33 void libraryOpen ( void )
{ / / c a l l e d when s e r v e r o p e n s t h i s p l u g i n ( i . e . c a l l s d l o p e n ( ) )
p r i n t f ( " Opening MyFunction L i b r a r y \n " ) ;
/ / add c o d e a s n e e d e d
// for global lo gf il es etc . . .
38 }
// / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / /
void l i b r a r y C l o s e ( void )
43 { / / c a l l e d when s e r v e r u n l o a d s t h i s p l u g i n ( i . e . c a l l s d l c l o s e ( ) )
p r i n t f ( " Clos ing MyFunction l i b r a r y \n " ) ;
/ / add c o d e a s n e e d e d
}
48
// / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / /
// library interface
/ / u s e d by s e r v e r when f u n c t i o n i s l o a d e d
UFunctionCamBase∗ c r e a t e F u n c ( )
53 { / / c r e a t e an o b j e c t o f t h i s t y p e
//
/∗∗ @todo ’ MyFunction ’ w i t h y o u r c l a s s n a m e , a s u s e d i n t h e h e a d e r f i l e ∗/
//
r e t u r n new MyFunction ( ) ;
58 }
void d e l e t e F u n c ( UFunctionCamBase∗ p )
{ / / d e l e t e the object
delete p ;
63 }
68
//
//
//
//
/////////////////////////////////////////////////
/////////////////////////////////////////////////
/////////////////////////////////////////////////
/////////////////////////////////////////////////
MyFunction : : MyFunction ( )
{
/ / i n i t i a l i z a t i o n o f v a r i a b l e s in c l a s s − as needed
73 }
// / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / /
MyFunction : : ~ MyFunction ( )
78 {
/ / p o s s i b l y remove a l l o c a t e d v a r i a b l e s h e r e − i f needed
}
C.5. MYFUNC.CPP
// / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / /
83
c o n s t char ∗ MyFunction : : name ( )
{
r e t u r n " m y _ f i r s t _ p l u g i n −1.53 ( 2 0 0 5 by C h r i s t i a n Andersen ) HaCkEd By JuHl " ;
}
88
// / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / /
93
c o n s t char ∗ MyFunction : : commandList ( )
{
r e t u r n "COG bark canny BOC" ;
}
// / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / /
bool MyFunction : : handleCommand ( UServerInMsg ∗ msg , void ∗ e x t r a )
98 {
/ / message i s unhandled
bool r e s u l t = f a l s e ;
/ / T e s t f o r t h e h a n d l e d commands , and c a l l a f u n c t i o n t o do t h e j o b
/ / t h e tagname i s n o t c a s e s e n s i t i v e − s e e t h e l i b r a r y d o c u m e n t a t i o n f o r
/ / ’ S e r v e r I n M s g ’ and t a g o f t y p e ’ USmlTag ’ − t o g e t a v a i l a b l e f u n c t i o n l i s t .
103
i f ( msg−>t a g . isTagA ( "COG" ) )
r e s u l t = c e n t e r O f G r a v i t y ( msg , ( UImage ∗) e x t r a ) ;
e l s e i f ( msg−>t a g . isTagA ( " bark " ) )
r e s u l t = doBark ( msg ) ;
e l s e i f ( msg−>t a g . isTagA ( " canny " ) )
108
r e s u l t = edge ( msg , ( UImage ∗) e x t r a ) ;
e l s e i f ( msg−>t a g . isTagA ( "BOC" ) )
r e s u l t = bottleOrCan ( msg , ( UImage ∗) e x t r a ) ;
else
sendDebug ( msg , "Command not handled ( by me) " ) ;
113
}
// / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / /
118 bool MyFunction : : doBark ( UServerInMsg ∗ msg )
{ / / send a s h o r t r e p l y back to the c l i e n t r e q u e s t e d the ’ bark ’
c o n s t i n t MRL = 1 0 0 ;
char r e p l y [MRL] ;
bool ask4help ;
123
char helpValue [ 3 0 ] ;
/ / c h e c k f o r p a r a m e t e r s − one p a r a m e t e r i s t e s t e d f o r − ’ h e l p ’
/ / t h e h e l p v a l u e i s i g n o r e d , e . g . i f h e l p =" b a r k " , t h e n
/ / the value " bark " w i l l be in the ’ helpValue ’ s t r i n g .
ask4help = msg−>t a g . g e t A t t V a l u e ( " help " , helpValue , 3 0 ) ;
128
/ / a s k 4 h e l p = f a l s e , i f no ’ h e l p ’ o p t i o n w e r e a v a i l a b l e .
i f ( ask4help )
{ / / c r e a t e t h e r e p l y i n XML−l i k e ( h t m l − l i k e ) f o r m a t
s n p r i n t f ( reply , MRL, "<%s r e p l y = ’ I w i l l not help you t o bark ! ’/ >\n " ,
msg−>t a g . getTagName ( ) ) ;
133
}
else
{ / / c r e a t e t h e r e p l y i n XML−l i k e ( h t m l − l i k e ) f o r m a t
s n p r i n t f ( reply , MRL, "<%s what = ’No reason t o bark a t me. ’/ >\n " ,
msg−>t a g . getTagName ( ) ) ;
138
}
/ / send t h i s s t r i n g as the r e p l y to the c l i e n t
sendMsg ( msg−>c l i e n t , r e p l y ) ;
/ / return t ru e i f the f u n c t i o n i s handled with a p o s i t i v e r e s u l t
return true ;
143 }
// / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / /
bool MyFunction : : c e n t e r O f G r a v i t y ( UServerInMsg ∗ msg , UImage ∗ pushImg )
148 { / / e x t r a c t p a r a m e t e r s f i r s t
/ / decode vars
char attName [MAX_SML_NAME_LENGTH ] ;
c o n s t i n t VBL = 1 0 0 ;
153
char a t t V a l u e [VBL ] ;
int n ;
/ / parameters default value
i n t roi X = 2 0 ;
i n t roi Y = 1 0 ;
158
i n t roiW = 8 0 ;
i n t roiH = 1 0 0 ;
int threshold = 100;
i n t deviceNum = −1;
char posName [MAX_MOUNT_NAME_SIZE] = " " ;
163
/ / c a m e r a and s o u r c e i m a g e v a r i a b l e s
UCamPush ∗ cam = NULL; / / c a m e r a
UImage ∗ img = pushImg ; / / i m a g e
bool ask4help = f a l s e ;
119
120
168
173
178
183
188
193
198
203
208
213
218
223
228
233
238
243
248
c o n s t i n t MRL = 2 0 0 ;
//
/ / extract parameters
/ / t h i s i s o n e o f two m e t h o d t , t h i s o n e t a k e s a l l p r o v i d e d p a r a m e t e r s o n e a t a t i m e
/ / and r e t u r n s i t s name ( attName ) and v a l u e ( a t t V a l u e ) , and you must t e s t f o r t h e name and
/ / e x t r a c t t h e v a l u e from t h e p r o v i d e d s t r i n g .
//
while ( msg−>t a g . g e t N e x t A t t r i b u t e ( attName , a t t V a l u e , VBL) )
{ / / camera d e v i c e
i f ( ( strcasecmp ( attName , " d e v i c e " ) == 0 ) or
( strcasecmp ( attName , " posName " ) == 0 ) )
{ / / e i t h e r number o r s t r i n g
n = s s c a n f ( a t t V a l u e , "%d " , &deviceNum ) ;
i f ( ( n ! = 1 ) and ( s t r l e n ( a t t V a l u e ) > 0 ) )
s t r n c p y ( posName , a t t V a l u e , MAX_MOUNT_NAME_SIZE) ;
}
/ / range o f i n t e r e s t parameters
e l s e i f ( strcasecmp ( attName , " help " ) == 0 )
ask4help = t r u e ;
e l s e i f ( strncasecmp ( attName , " width " , 1 ) == 0 )
s s c a n f ( a t t V a l u e , "%d " , &roiW ) ;
e l s e i f ( strncasecmp ( attName , " h e i g h t " , 1 ) == 0 )
s s c a n f ( a t t V a l u e , "%d " , &roiH ) ;
e l s e i f ( strcasecmp ( attName , " y " ) == 0 )
s s c a n f ( a t t V a l u e , "%d " , &roiY ) ;
e l s e i f ( strcasecmp ( attName , " x " ) == 0 )
s s c a n f ( a t t V a l u e , "%d " , &roiX ) ;
e l s e i f ( strncasecmp ( attName , " t h r e s h o l d " , 1 ) == 0 )
s s c a n f ( a t t V a l u e , "%d " , &t h r e s h o l d ) ;
}
//
i f ( ask4help )
{ / / i t was j u s t a r e q u e s t f o r on−l i n e h e l p
sendMsg ( msg−>c l i e n t , " <help s u b j e c t =\"COG help \">\n " ) ;
sendMsg ( msg−>c l i e n t , "−−−− A v a i l a b l e COG o p t i o n s : \ n " ) ;
sendMsg ( msg−>c l i e n t , " d e v i c e =N
from d e v i c e ’N ’ , i . e . /dev/videoN\n " ) ;
sendMsg ( msg−>c l i e n t , " posName=name from camera ’name ’ , e . g . l e f t f o r l e f t camera\n " ) ;
Top−l e f t p o s i t i o n o f Range Of I n t e r e s t ( ROI ) \n " ) ;
sendMsg ( msg−>c l i e n t , " x=X y=Y
sendMsg ( msg−>c l i e n t , "w=W h=H
Width and h e i g h t o f ROI\n " ) ;
Threshold f o r p i x e l use ( 0 t o 2 5 5 ) \n " ) ;
sendMsg ( msg−>c l i e n t , " t h r e s h o l d =B
sendMsg ( msg−>c l i e n t , "
( r e t u r n s c e n t e r o f g r a v i t y p o s i t i o n f o r a r e a ) \n " ) ;
( Uses image pool img=0 ( with a r e a ) and img=1 ( o r i g i n a l ) ) \n " ) ;
sendMsg ( msg−>c l i e n t , "−−−\n " ) ;
sendMsg ( msg−>c l i e n t , " </help >\n " ) ;
s en d In f o ( msg , " done " ) ;
}
else
{ / / i f s e r v e r e v e n t push , t h e n i m a g e i s a v a i l a b l e i n c a l l
/ / i f not , t h e n g e t an i m a g e f r o m c a m e r a
i f ( img == NULL)
{ / / no image , s o g e t r e q u e s t e d c a m e r a
i f ( g o t A l l R e s o u r c e s ( reply , MRL) )
{
i f ( deviceNum >= 0 )
/ / g e t c a m e r a f r o m d e v i c e number
cam = camPool−>getCam ( deviceNum ) ;
e l s e i f ( s t r l e n ( posName ) > 0 )
/ / e l s e g e t c a m e r a f r o m c a m e r a p o s i t i o n name
cam = camPool−>getCam ( posName ) ;
else
/ / e l s e use d e v i c e 0
cam = camPool−>getCam ( 0 ) ;
r e s u l t = ( cam ! = NULL) ;
if ( result )
{ / / g e t i m a g e b u f f e r f r o m image−p o o l
img = imgPool−>getImage ( cam−>getDeviceNumber ( ) , t r u e ) ;
/ / g e t an i m a g e f r o m c a m e r a
/ / g e t an i m a g e f r o m s t r e a m and l o a d i t i n t o ’ img ’
r e s u l t = cam−>getImageSnapshot ( img , f a l s e ) ;
}
else
f p r i n t f ( s t d e r r , "No camera found\n " ) ;
}
else
f p r i n t f ( s t d e r r , " R e s s o r e c e s a r e missing , ( no %s ) \n " , r e p l y ) ;
}
else
r e s u l t = true ;
//
i f ( not r e s u l t )
/ / s e n d s t a n d a r d XML−p a c k e d r e p l y t o c l i e n t − l i k e :
/ / <COG warning ="No i m a g e a v a i l a b l e "/ >\n
sendWarning ( msg , "No image a v a i l a b l e " ) ;
else
C.5. MYFUNC.CPP
{ / / image i s a v i a l a b l e
calculateCOG ( msg , img , roiX , roiY , roiW , roiH , t h r e s h o l d ) ;
}
253
}
}
258
// / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / /
bool MyFunction : : calculateCOG ( UServerInMsg ∗ msg ,
UImage ∗ rawImg ,
i n t roiX , i n t roiY , i n t roiW , i n t roiH ,
int threshold )
{
UImage ∗ raw2 ;
UImage ∗ img0 ; / / RGB f o r m a t i m a g e
268
UImage ∗ imgBW ; / / RGB f o r m a t i m a g e
double sx = 0 ; / / sum o f x v a l u e s
double sy = 0 ; / / sum o f y v a l u e s
int sxi , syi ;
273
int n = 0;
/ / count o f p i x e l s
int r , c ;
UPixel ∗ pix ; / / p o i n t e r t o a p i x e l
//
c o n s t i n t MRL = 1 0 0 ;
278
char s [ 3 0 ] ;
//
unsigned char ∗y , ∗u , ∗v , m;
263
283
288
293
298
303
308
313
318
323
328
333
//
/ / g e t an i m a g e s t r u c t u r e f r o m t h e i m a g e p o o l − s h a r e d by a l l f u n c t i o n s
/ / i m a g e i s c r e a t e d on f i r s t c a l l ( i f s e c o n d p a r a m e t e r i s t r u e )
//
//
/ / Make a l o w p a s s f i l t e r e d i m a g e u s i n g and openCV f u n c t i o n
//
/ / g e t an i m a g e f r o m p o o l t o s t o r e a work c o p y o f i m a g e
/ / t h i s w i l l b e i m a g e 10 i n i m a g e p o o l−l i s t
img0 = imgPool−>getImage ( 1 0 , t r u e ) ;
/ / g e t a n o t h e r image from p o o l t o s t o r e smoothed image
/ / c o n v e r t raw YUV i m a g e t o RGB ( a c t u a l l y BGR, a s t h i s i s s t a n d a r d i n openCV )
img1−>copy ( rawImg ) ;
img1−>toBGR ( ) ;
img1−>setName ( " img1 " ) ;
/ / f i l t e r i m a g e u s i n g a g a u s s i o n low p a s s f i l t e r
cvSmooth ( img1−>cvArr ( ) , img1−>cvArr ( ) , CV_GAUSSIAN, 3 , 0 , 0 ) ;
//
/ / use f i l t e r e d image
/ / l i m i t a r e a t o w i t h i n image
roi X = maxi ( 0 , mini ( roiX , img1−>width ( ) − 1 ) ) ;
roi Y = maxi ( 0 , mini ( roiY , img1−>h e i g h t ( ) − 1 ) ) ;
i f ( ( ro iX + roiW ) > ( i n t ) img1−>width ( ) )
roiW = img1−>width ( ) − roi X ;
i f ( ( ro iY + roiH ) > ( i n t ) img1−>h e i g h t ( ) )
roiH = img1−>h e i g h t ( ) − roi Y ;
//
//
/ / now c a l c u l a t e t h e COG w i t h i n t h e ROI
f o r ( r = 0 ; r < roiH ; r ++)
{ / / get p o i n t e r to f i r s t p i x e l in area
/ / on l i n e ’ r ’
pix = img1−>g e t P i x R e f ( r + roiY , ro iX ) ;
/ / t h e n c o n t i n u e w i t h t h e n e x t p i x e l s on i m a g e l i n e
f o r ( c = 0 ; c < roiW ; c ++)
{ / / t e s t i f a v e r a g e p i x e l −v a l u e i s a b o v e t r e s h o l d
i f ( ( ( pix−>p1 + pix−>p2 + pix−>p3 ) /3) > t h r e s h o l d )
{ / / summ t h e p i x e l p o s i t i o n
n++;
sx += ( double ) ( roiX + c ) ;
sy += ( double ) ( roiY + r ) ;
/ / change the c o l o r o f t h i s p i x e l
/ / t o mark , t h a t i t i s a b o v e t r e s h o l d .
pix−>p2 = 2 5 5 ; / / g r e e n v a l u e
pix−>p1 = 1 2 8 ; / / b l u e v a l u e
}
/ / advance p i x e l p o i n t e r to next p i x e l
pix ++;
}
}
i f (n > 0)
{ / / c a l c u l a t e Center of Gravity
121
122
338
343
348
353
358
363
368
373
378
383
388
393
398
403
408
413
418
sx /= ( double ) n ;
sy /= ( double ) n ;
}
//
/ / p a i n t a r e c t a n g e l i n image , t o mark t h e t e s t e d a r e a
/ / u s i n g an o p e n CV f u n c t i o n
c v R e c t a n g l e ( img1−>cvArr ( ) ,
/ / image
c v P o i n t ( roiX , roi Y ) ,
/ / one c o r n e r
c v P o i n t ( roi X + roiW , ro iY + roiH ) , / / s e c o n d c o r n e r
CV_RGB( 2 5 5 , 0 , 0 ) ,
/ / c o l o u r ( R , G, B )
1 , 8 , 0) ;
/ / l i n e −width , c o n n e c t i v i t y , o f f s e t
/ / convert result to integer
s x i = roundi ( sx ) ;
s y i = roundi ( sy ) ;
/ / paint cross at center of gravity
cvLine ( img1−>cvArr ( ) , c v P o i n t ( s x i −10, s y i −10) ,
c v P o i n t ( s x i +10 , s y i +10) ,
CV_RGB( 0 , 0 , 2 5 5 ) , 1 , 8 , 0 ) ;
cvLine ( img1−>cvArr ( ) , c v P o i n t ( mini ( img1−>width ( ) −1, s x i +10) , maxi ( 0 , s y i −10) ) ,
c v P o i n t ( maxi ( 0 , s x i −10) , mini ( img1−>h e i g h t ( ) −1, s y i +10) ) ,
CV_RGB( 0 , 0 , 2 5 5 ) , 1 , 8 , 0 ) ;
/ / t e l l i m a g e p o o l t o f l a g i m a g e a s u p d a t e d , ( s o t h a t any p e n d i n g push
/ / commands can b e e f f e c t u a t e d ( a f t e r t h i s f u n c t i o n i s f i n i s h e d ) .
img1−>imgUpdated ( ) ;
/ / s a v e r e s u l t i n bmp f o r m a t − f i l t e r e d i m a g e w i t h r e s u l t m a r k i n g s
rawImg−>saveBMP ( " t e s t i m g−org . bmp" ) ;
img1−>saveBMP ( " t e s t i m g−f i l t . bmp" ) ;
//
/ / send r e p l y
/ / c o n v e r t i m a g e t i m e s t a m p t o a s t r i n g i n f o r m a t hh :mm: s s . ddd ( t o s t r i n g s )
img0−>imgTime . getTimeAsString ( s , t r u e ) ;
/ / f o r m a t r e p l y i n XML s t y l e a s a s i n g l e t a g w i t h a t t r i b u t e s and v a l u e s .
s n p r i n t f ( reply , MRL, " <COG x=\"% f \" y=\"% f \" time=\"%s \"/>\n " , sx , sy , s ) ;
//
/ / t h e r e s t i s j u s t t o d e m o n s t r a t e some i m a g e m a i p u l a t i o n s
//
/ / c o n v e r t t o BW f o r some openCV m a i p u l a t i o n i n BW
imgBW = imgPool−>getImage ( 1 3 , t r u e ) ;
imgBW−>copy ( rawImg ) ;
/ / c o n v e r t t o BW
imgBW−>toBW ( ) ;
/ / f l a g as updated
imgBW−>imgUpdated ( ) ;
/ / copy s u p p o r t d a t a t o image 0
/ / − i n c l u d i n g s i z e i m a g e t i m e , name s e r i a l number e t c .
/ / but not t h e image p i x e l s
img0−>copyMeta (imgBW, t r u e ) ;
img0−>setName ( " img0−edge " ) ;
/ / d e t e c t e d g e s u s i n g a ( openCV ) canny f i l t e r
/ / − and s t o r e s r e s u l t i n img0
cvCanny (imgBW−>cvArr ( ) , img0−>cvArr ( ) , 0 . 5 , 0 . 5 , 3 ) ;
/ / s a v e t h e edge image
img0−>saveBMP ( " t e s t i m g−canny . bmp" ) ;
//
//
/ / some i m a g e m a n i p u l a t i o n p i x e l by p i x e l i n YUV420 f o r m a t
/ / ( i n YUV420 u and v h a s h a l f r e s o l u t i o n o n l y )
/ / r e s u l t i s s a v e d a s t e s t i n g −c o l o u r . bmp
/ / enhances c o l o r in the region :
/ / x=50 ( column ) y=60 ( row ) ( t o p l e f t ) t o ( x =350 y =310 − b o t t o m r i g h t )
//
raw2 = imgPool−>getImage ( 1 2 , t r u e ) ;
raw2−>copy ( rawImg ) ;
raw2−>setName ( " raw2−c o l o r " ) ;
f o r ( r = 6 0 ; r < mini ( raw2−>getHeight ( ) , 3 1 0 ) ; r +=2)
{
y = raw2−>g e t Y l i n e ( r ) ;
u = raw2−>g e t U l i n e ( r /2) ;
v = raw2−>g e t V l i n e ( r /2) ;
f o r ( c = 5 0 ; c < mini ( 3 5 0 , raw2−>getWidth ( ) ) ; c +=2)
{ / / swap U and V
m = ∗v ;
∗v = ∗u ;
∗u = m;
/ / a d v a n c e y p o i n t e r by 2 p i x e l s
y += 2 ;
/ / a d v a n c e U and V by o n e p i x e l
u++;
v ++;
}
}
C.5. MYFUNC.CPP
123
raw2−>imgUpdated ( ) ;
/ / save
raw2−>saveBMP ( " t e s t i n g−c o l o r . bmp" ) ;
//
return true ;
423
}
428
// / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / /
433
438
443
448
453
458
463
468
473
478
483
488
493
498
503
bool MyFunction : : bottleOrCan ( UServerInMsg ∗ msg , UImage ∗ pushImg )
{ / / extract parameters f i r s t
/ / decode vars
c o n s t i n t VBL = 1 0 0 ;
int n ;
i n t roi X = 6 0 ;
i n t roi Y = 0 ;
i n t roiW = 2 2 ;
i n t roiH = 9 4 ;
i n t areaCan = 1 5 0 0 ;
int areaBottle = 750;
i n t thresholdCanU = 1 2 0 ;
i n t thresholdCanV = 1 3 0 ;
int thresholdBottleU = 120;
int thresholdBottleV = 115;
c o n s t i n t MRL = 2 0 0 ;
//
//
}
e l s e i f ( strncasecmp ( attName , " width " , 1 ) == 0 )
s s c a n f ( a t t V a l u e , "%d " , &roiW ) ;
e l s e i f ( strncasecmp ( attName , " h e i g h t " , 1 ) == 0 )
s s c a n f ( a t t V a l u e , "%d " , &roiH ) ;
e l s e i f ( strcasecmp ( attName , " y " ) == 0 )
s s c a n f ( a t t V a l u e , "%d " , &roiY ) ;
e l s e i f ( strcasecmp ( attName , " x " ) == 0 )
s s c a n f ( a t t V a l u e , "%d " , &roiX ) ;
e l s e i f ( strcasecmp ( attName , " areaCan " ) == 0 )
s s c a n f ( a t t V a l u e , "%d " , &areaCan ) ;
e l s e i f ( strcasecmp ( attName , " a r e a B o t t l e " ) == 0 )
s s c a n f ( a t t V a l u e , "%d " , &a r e a B o t t l e ) ;
e l s e i f ( strcasecmp ( attName , " thresholdCanU " ) == 0 )
s s c a n f ( a t t V a l u e , "%d " , &thresholdCanU ) ;
e l s e i f ( strcasecmp ( attName , " thresholdCanV " ) == 0 )
s s c a n f ( a t t V a l u e , "%d " , &thresholdCanV ) ;
e l s e i f ( strcasecmp ( attName , " t h r e s h o l d B o t t l e U " ) == 0 )
s s c a n f ( a t t V a l u e , "%d " , &t h r e s h o l d B o t t l e U ) ;
e l s e i f ( strcasecmp ( attName , " t h r e s h o l d B o t t l e V " ) == 0 )
s s c a n f ( a t t V a l u e , "%d " , &t h r e s h o l d B o t t l e V ) ;
}
//
i f ( ask4help )
sendMsg ( msg−>c l i e n t , " <help s u b j e c t =\" B o t t l e or Can help \">\n " ) ;
sendMsg ( msg−>c l i e n t , "−−−− A v a i l a b l e BOC o p t i o n s : \ n " ) ;
sendMsg ( msg−>c l i e n t , " posName=name
from camera ’name ’ , e . g . l e f t f o r l e f t camera\n
");
Top−l e f t p o s i t i o n o f Range Of I n t e r e s t ( ROI ) \n "
sendMsg ( msg−>c l i e n t , " x=X y=Y
);
124
sendMsg ( msg−>c l i e n t ,
);
s en d In f o ( msg , " done " )
508
513
}
else
{ //
//
if
{
518
523
528
533
538
543
"w=W h=H
" areaCan=Ac
" a r e a B o t t l e =Ab
Width and h e i g h t o f ROI\n " ) ;
Amount o f p i x e l s t h a t should d e f i n e a can\n " ) ;
Amount o f p i x e l s t h a t should d e f i n e a b o t t l e \n "
" thresholdCanU=B
" thresholdCanV=B
" t h r e s h o l d B o t t l e U =B
" t h r e s h o l d B o t t l e V =B
"
( r e t u r n s e i t h e r ’ f a l s e ’ , ’ b o t t l e ’ or ’ can ’ ) \n " ) ;
"
"−−−\n " ) ;
" </help >\n " ) ;
;
i f s e r v e r e v e n t push , t h e n i m a g e i s a v a i l a b l e i n c a l l
i f not , t h e n g e t an i m a g e f r o m c a m e r a
( img == NULL)
/ / no image , s o g e t r e q u e s t e d c a m e r a
{
else
if ( result )
}
else
}
else
}
else
//
else
{ / / image i s a v a i l a b l e
c a l c B o t t l e O r C a n ( msg , img , roiX , roiY , roiW , roiH , areaCan , a r e a B o t t l e , thresholdCanU ,
thresholdCanV , t h r e s h o l d B o t t l e U , t h r e s h o l d B o t t l e V ) ;
}
548
553
558
}
}
563
568
573
578
583
// / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / /
bool MyFunction : : c a l c B o t t l e O r C a n ( UServerInMsg ∗ msg ,
UImage ∗ rawImg ,
i n t roiX , i n t roiY , i n t roiW , i n t roiH ,
i n t areaCan , i n t a r e a B o t t l e ,
i n t thresholdCanU , i n t thresholdCanV ,
int thresholdBottleU , int thresholdBottleV )
{
double sxCans = 0 , s x B o t t l e s = 0 ; / / sum o f x v a l u e s
double syCans = 0 , s y B o t t l e s = 0 ; / / sum o f y v a l u e s
i n t sxiCans , syiCans ;
int sxiBottles , syiBottles ;
i n t nCans = 0 , n B o t t l e s = 0 ;
/ / count o f p i x e l s
int r , c ;
f l o a t pCan = 0 . 0 , p B o t t l e = 0 . 0 ; / / p r o b a b i l i t i e s
UPixel ∗ pix ; / / p o i n t e r t o a p i x e l
//
c o n s t i n t MRL = 1 0 0 ;
char s [ 3 0 ] ;
//
//
C.5. MYFUNC.CPP
588
593
598
603
608
613
618
623
628
//
//
//
/ / g e t a n o t h e r image from p o o l t o s t o r e smoothed image
/ / Copy raw YUV i m a g e
img1−>copy ( rawImg ) ;
img1−>toYUV ( ) ;
img1−>setName ( "BOC" ) ;
/ / f i l t e r i m a g e u s i n g a g a u s s i o n low p a s s f i l t e r
cvSmooth ( img1−>cvArr ( ) , img1−>cvArr ( ) , CV_GAUSSIAN, 3 , 0 , 0 ) ;
//
/ / use f i l t e r e d image
/ / l i m i t a r e a t o w i t h i n image
roi X = maxi ( 0 , mini ( roiX , img1−>width ( ) − 1 ) ) ;
roi Y = maxi ( 0 , mini ( roiY , img1−>h e i g h t ( ) − 1 ) ) ;
i f ( ( ro iX + roiW ) > ( i n t ) img1−>width ( ) )
roiW = img1−>width ( ) − roi X ;
i f ( ( ro iY + roiH ) > ( i n t ) img1−>h e i g h t ( ) )
roiH = img1−>h e i g h t ( ) − roi Y ;
//
//
/ / now c a l c u l a t e t h e BOC w i t h i n t h e ROI
f o r ( r = 0 ; r < roiH ; r ++)
{ / / get p o i n t e r to f i r s t p i x e l in area
/ / on l i n e ’ r ’
pix = img1−>g e t P i x R e f ( r + roiY , ro iX ) ;
/ / c o n v e r t BGR t o YUV p i x e l
/ / ∗ p i x = p i x −>asYUV ( PIX_PLANES_BGR ) ;
/ / t h e n c o n t i n u e w i t h t h e n e x t p i x e l s on i m a g e l i n e
f o r ( c = 0 ; c < roiW ; c ++)
{ / / t e s t i f a v e r a g e p i x e l −v a l u e i s a b o v e t r e s h o l d
i f ( ( pix−>u < thresholdCanU ) && ( pix−>v > thresholdCanV ) )
{ / / sum t h e p i x e l p o s i t i o n
nCans ++;
sxCans += ( double ) ( roiX + c ) ;
syCans += ( double ) ( roiY + r ) ;
pix−>u = 1 9 8 ;
pix−>v = 0 ;
633
}
e l s e i f ( ( pix−>u < t h r e s h o l d B o t t l e U ) && ( pix−>v < t h r e s h o l d B o t t l e V ) )
{ / / sum t h e p i x e l p o s i t i o n
n B o t t l e s ++;
s x B o t t l e s += ( double ) ( roiX + c ) ;
s y B o t t l e s += ( double ) ( roiY + r ) ;
638
pix−>u = 0 ;
pix−>v = 7 0 ;
643
}
/ / advance p i x e l p o i n t e r to next p i x e l
pix ++;
648
}
}
653
658
663
i f ( nCans > 0 )
{
/ / c a l c u l a t e t h e p r o b a b i l i t y o f a can
pCan = ( f l o a t ) nCans / ( f l o a t ) ( areaCan ) ;
/ / c a l c u l a t e Center of Gravity
sxCans /= ( double ) nCans ;
syCans /= ( double ) nCans ;
}
i f ( nBottles > 0)
{
/ / calculate the probability of a b o t t l e
pBottle = ( float ) nBottles / ( float ) ( areaBottle ) ;
/ / c a l c u l a t e Center of Gravity
s x B o t t l e s /= ( double ) n B o t t l e s ;
s y B o t t l e s /= ( double ) n B o t t l e s ;
668
}
//
/ / p a i n t a r e c t a n g e l i n image , t o mark t h e t e s t e d a r e a
/ / u s i n g an o p e n CV f u n c t i o n
c v R e c t a n g l e ( img1−>cvArr ( ) ,
/ / image
125
126
c v P o i n t ( roiX , roiY ) ,
/ / one c o r n e r
c v P o i n t ( roi X + roiW , ro iY + roiH ) , / / s e c o n d c o r n e r
CV_RGB( 2 5 5 , 0 , 0 ) ,
/ / c o l o u r ( R , G, B )
1 , 8 , 0) ;
/ / l i n e −width , c o n n e c t i v i t y , o f f s e t
673
/ / convert result to integer
sxiCans = roundi ( sxCans ) ;
syiCans = roundi ( syCans ) ;
s x i B o t t l e s = roundi ( s x B o t t l e s ) ;
s y i B o t t l e s = roundi ( s y B o t t l e s ) ;
678
683
cvLine ( img1−>cvArr ( ) , c v P o i n t ( sxiCans −10, syiCans −10) ,
c v P o i n t ( sxiCans +10 , syiCans +10) ,
CV_RGB( 2 5 5 , 0 , 0 ) , 1 , 8 , 0 ) ;
cvLine ( img1−>cvArr ( ) , c v P o i n t ( mini ( img1−>width ( ) −1, sxiCans +10) , maxi ( 0 , syiCans −10) ) ,
c v P o i n t ( maxi ( 0 , sxiCans −10) , mini ( img1−>h e i g h t ( ) −1, syiCans +10) ) ,
CV_RGB( 2 5 5 , 0 , 0 ) , 1 , 8 , 0 ) ;
688
cvLine ( img1−>cvArr ( ) , c v P o i n t ( s x i B o t t l e s −10, s y i B o t t l e s −10) ,
c v P o i n t ( s x i B o t t l e s +10 , s y i B o t t l e s +10) ,
CV_RGB( 0 , 2 5 5 , 0 ) , 1 , 8 , 0 ) ;
cvLine ( img1−>cvArr ( ) , c v P o i n t ( mini ( img1−>width ( ) −1, s x i B o t t l e s +10) , maxi ( 0 , s y i B o t t l e s −10) ) ,
c v P o i n t ( maxi ( 0 , s x i B o t t l e s −10) , mini ( img1−>h e i g h t ( ) −1, s y i B o t t l e s +10) ) ,
693
698
CV_RGB( 0 , 2 5 5 , 0 ) , 1 , 8 , 0 ) ;
/ / t e l l i m a g e p o o l t o f l a g i m a g e a s u p d a t e d , ( s o t h a t any p e n d i n g push
/ / commands can b e e f f e c t u a t e d ( a f t e r t h i s f u n c t i o n i s f i n i s h e d ) .
703
/ / s a v e r e s u l t i n bmp f o r m a t − f i l t e r e d i m a g e w i t h r e s u l t m a r k i n g s
/ / send r e p l y
s n p r i n t f ( reply , MRL, " < v i s i o n v i s 0 =\"% f \" v i s 1 =\"% f \"/>\n " , pCan , p B o t t l e ) ;
708
713
//
//
718
s n p r i n t f ( r e p l y , MRL, "<BOC pCan=\"% f \" p B o t t l e =\"% f \" t i m e=\"%s \"/ >\n " , pCan , p B o t t l e , s ) ;
return true ;
}
723
728
733
738
743
748
753
// / / / / / / / / / / / / / / / /
// / / / / / / / / / / / / / / / /
// / / / / / / / / / / / / / / / /
bool MyFunction : : edge ( UServerInMsg ∗ msg , UImage ∗ pushImg )
{
/ / decode vars
c o n s t i n t VBL = 1 0 0 ;
int n ;
int sobel = 5;
double t h r e s h o l d 1 = 0 . 5 ;
double t h r e s h o l d 2 = 6 0 0 0 ;
c o n s t i n t MRL = 2 0 0 ;
//
//
C.5. MYFUNC.CPP
758
763
768
773
778
783
788
793
798
803
808
813
818
823
828
833
838
127
}
e l s e i f ( strncasecmp ( attName , " s o b e l " , 1 ) == 0 )
s s c a n f ( a t t V a l u e , "%d " , &s o b e l ) ;
e l s e i f ( strncasecmp ( attName , " t h r e s h o l d 1 " , 1 ) == 0 )
s s c a n f ( a t t V a l u e , "%l f " , &t h r e s h o l d 1 ) ;
e l s e i f ( strncasecmp ( attName , " t h r e s h o l d 2 " , 1 ) == 0 )
s s c a n f ( a t t V a l u e , "%l f " , &t h r e s h o l d 2 ) ;
}
//
i f ( ask4help )
sendMsg ( msg−>c l i e n t , " <help s u b j e c t =\"Canny edge d e t e c t i o n help \">\n " ) ;
sendMsg ( msg−>c l i e n t , "−−−− A v a i l a b l e Canny o p t i o n s : \ n " ) ;
sendMsg ( msg−>c l i e n t , " posName=name
from camera ’name ’ , e . g . l e f t f o r l e f t camera\n " ) ;
sendMsg ( msg−>c l i e n t , " s o b e l =S
The S o b e l f a c t o r { 1 , 3 , 5 , 7 } \ n " ) ;
Threshold one\n " ) ;
sendMsg ( msg−>c l i e n t , " t h r e s h o l d 1 =B
sendMsg ( msg−>c l i e n t , " t h r e s h o l d 2 =B
Threshold two\n " ) ;
sendMsg ( msg−>c l i e n t , "−−−\n " ) ;
sendMsg ( msg−>c l i e n t , " </help >\n " ) ;
s en d In f o ( msg , " done " ) ;
}
else
{ / / i f s e r v e r e v e n t push , t h e n i m a g e i s a v a i l a b l e i n c a l l
/ / i f not , t h e n g e t an i m a g e f r o m c a m e r a
i f ( img == NULL)
{ / / no image , s o g e t r e q u e s t e d c a m e r a
{
else
if ( result )
}
else
}
else
}
else
//
else
{ / / image i s a v a i l a b l e
UImage ∗ imgBW ; / / RGB f o r m a t i m a g e
//
c o n s t i n t MRL = 1 0 0 ;
char s [ 3 0 ] ;
//
//
//
//
//
/ / g e t an i m a g e f r o m p o o l t o s t o r e a work c o p y o f i m a g e
128
/ / c o n v e r t t o BW f o r some openCV m a i p u l a t i o n i n BW
imgBW = imgPool−>getImage ( 1 5 , t r u e ) ;
imgBW−>copy ( img ) ;
/ / c o n v e r t t o BW
imgBW−>toBW ( ) ;
imgBW−>imgUpdated ( ) ;
/ / copy s u p p o r t d a t a t o image 0
/ / − i n c l u d i n g s i z e i m a g e t i m e , name s e r i a l number e t c .
/ / but not t h e image p i x e l s
img0−>copyMeta (imgBW, t r u e ) ;
img0−>setName ( " canny edge " ) ;
/ / d e t e c t e d g e s u s i n g a ( openCV ) canny f i l t e r
/ / − and s t o r e s r e s u l t i n img0
cvCanny (imgBW−>cvArr ( ) , img0−>cvArr ( ) , t h r e s h o l d 1 , t h r e s h o l d 2 , s o b e l ) ;
/ / send r e p l y
s n p r i n t f ( reply , MRL, " <Canny time=\"%s \"/>\n " , s ) ;
843
848
853
858
863
}
}
868
}
C.6. DEFINITIONS.H
C.6
129
Definitions.h
# include <avr/ i o . h>
# include <avr/ s i g n a l . h>
/∗ s i g n a l ( i n t e r r u p t ) v e c t o r h a n d l i n g ∗/
# include <avr/ i n t e r r u p t . h> /∗ i n t e r r u p t f l a g s e . g . TIMSK ∗/
5 /∗
D e f i n i t i o n s and m a c r o s u s e d i n t h e AtmelMega16 program
∗/
/∗ Make b o o l e a n more r e a d a b l e ∗/
10 # define t r u e 1
# define f a l s e 0
# define e n a b l e _ r a i s i n g t r u e
/ / Used
# define
# define
# define
# define
20 # define
# define
# define
# define
15
ports
PORT_SERVOS
DDR_SERVOS
PORT_BUTTONS
DDR_BUTTONS
PORT_SWITCHES
DDR_SWITCHES
PORT_LEDS
DDR_LEDS
PORTA
DDRA
PINC
DDRC
PINB
DDRB
PORTC
DDRC
25 # define PIN_DC
PORTA7
/ / N o t i c e t h a t PORTD t a k e s c a r e o f t h e c o m m u n i c a t i o n w i t h t h e c o m p u t e r and s h o u l d n ’ t b e
/ / u s e d i n t h i s program
30 enum s e r v o s { RAISE_SERVO , REJECT_SERVO , DELIVER_SERVO , RESERVE_SERVO } ;
/ / t h e v e l o c i t i e s o f t h e 8 s e r v o s l a r g e v a l u e means f a s t e r
int velocity [4]={1 ,20 ,20 ,20}; / / int v e l o c i t y [4]={20 ,20 ,20 ,20};//{10 ,2 ,5 ,5 ,1 ,2 ,3 ,3};
/ / t h e v e l o c i t i e s o f t h e 8 s e r v o s s m a l l v a l u e means f a s t e r
35 i n t v e l o c i t y 2 [ 4 ] = { 2 , 1 , 1 , 1 } ; / / i n t v e l o c i t y 2 [ 4 ] = { 1 , 1 , 1 , 1 } ; / / { 1 , 2 , 1 , 1 , 2 0 , 2 , 2 0 , 2 0 } ;
/ / macros
# define s e r v o _ s e t ( x )
# define s e r v o _ c l e a r ( x )
40 # define s e r v o _ i n v e r t ( x )
PORT_SERVOS |= _BV ( x )
PORT_SERVOS &= ~_BV ( x )
PORT_SERVOS ^= _BV ( x )
# define l e d _ o f f ( x )
# define led_on ( x )
# define l e d _ i n v e r t ( x )
PORT_LEDS |= _BV ( x )
PORT_LEDS &= ~_BV ( x )
PORT_LEDS ^= _BV ( x )
# define kick_dc_on ( )
# define k i c k _ d c _ o f f ( )
PORT_SERVOS |= _BV ( PIN_DC )
PORT_SERVOS &= ~_BV ( PIN_DC )
45
/ / made s o t h a t t h e macro r e t u r n s t r u e when t h e s w i t c h i s p r e s s e d ( v a l u e =0)
50 # define b u t t o n _ p r e s s e d ( x )
( ( PORT_BUTTONS & _BV ( x ) ) ==0? t r u e : f a l s e )
# define r e a d y _ t o _ r a i s e ( )
( ( PORT_SWITCHES & _BV ( 0 ) ) ==0? f a l s e : t r u e )
/ / D i f f e r e n t a n g l e s u s e d i n t h e s o r t i n g mechanism and p l a c e m e n t box
55
/ / Specific angles
# define SERVO_0
1
# define SERVO_45 0 x31
# define SERVO_90 0x4E
60 # define SERVO_135 0 x69
# define SERVO_180 127
/ / angles
65 # define RAISE_ATTACH_ARM 124
# define RAISE_READY 80
# define RAISE_GRAP 64
# define RAISE_TOP 127
70 # define REJECT_CLOSED 31
# define REJECT_OPEN 65
# define DELIVER_CLOSED 124
# define DELIVER_OPEN 70
130
C.7
Communication.h
1 void i n i t _ u s a r t ( void ) ;
C.8
Communication.c
/∗
The c o m m u n i c a t i o n module f o r t h e Atmel mega16 c i r c u i t ,
which i s u s e d t o c o n t r o l t h e s e r v o s , l a m p s and r e c e i v e
d i f f e r e n t i n t e r r u p t s f r o m t h e smr .
4
∗/
9 # include <avr/ i o . h>
# include <avr/ i n t e r r u p t . h>
# include <avr/ s i g n a l . h>
# include " Communication . h "
# include " S e r v o s t y r i n g . h "
14
# define t r u e 1
# define f a l s e 0
# define TRANSMIT_ENABLE 0 x04
19 char
char
char
char
char
24 void
void
input = 0 x00 ;
msg [ 4 ] = { 0 } ;
msg_cnt = 0 ;
msg_len = 0 ;
command = 0 ;
r e c v _ r o u t i n e ( char input ) ;
a n s _ r o u t i n e ( char ∗msg , char nbytes ) ;
char out [ 4 ] = { 0 } ;
char o u t _ c n t = 0 ;
29 char o u t _ l e n = 0 ;
extern
extern
extern
34 e x t e r n
extern
extern
extern
extern
39 e x t e r n
extern
char
char
char
char
char
char
char
char
char
char
start_flag ;
raise_ready_flag ;
raise_grap_flag ;
raise_top_flag ;
reject_closed_flag ;
reject_open_flag ;
deliver_closed_flag ;
deliver_open_flag ;
kick_dc_on_flag ;
kick_dc_off_flag ;
SIGNAL( SIG_UART_TRANS )
44 {
i f ( out_cnt != out_len )
{
o u t _ c n t ++;
UDR = out [ o u t _ c n t ] ;
49
}
else
{
out_cnt = 0 ;
PORTD &= ~TRANSMIT_ENABLE ;
54
}
}
SIGNAL( SIG_UART_RECV )
{
59
input = UDR;
r e c v _ r o u t i n e ( input ) ;
}
64 void i n i t _ u s a r t ( void )
{
/ / S e t s b a u d r a t e t o 115200 ( i f f _ c l k = 1 4 , 7 4 5 6 MHz) :
UBRRH = ( unsigned char ) 0 x00 ;
UBRRL = ( unsigned char ) 0 x07 ;
69
/ / E n a b l e s t r a n s m i t / r e c e i v e and t r a n s m i t / r e c e i v e i n t e r r u p t s :
UCSRB = (1 < <RXCIE ) | (1 < <TXCIE ) | (1 < <RXEN) | (1 < <TXEN) ;
/ / S e t s f r a m e f o r m a t t o 8 d a t a b i t s and 2 s t o p b i t s :
UCSRC = (1 < <URSEL ) | (1 < <USBS ) | (1 < <UCSZ1 ) | (1 < <UCSZ0 ) ;
74
}
C.8. COMMUNICATION.C
131
void r e c v _ r o u t i n e ( char input )
{
79
i f ( msg_cnt < 4 )
msg [ msg_cnt ] = input ;
i f ( msg_cnt == 0 )
msg_len = msg [ 0 ] & 0 x0F ;
84
/ / Store message length
i f ( msg_cnt == msg_len )
{
/ / Act on t h e r e q u e s t i f i t h a s t h e r i g h t a d d r e s s :
i f ( msg_cnt == 1 && ( ( msg [ 1 ] & 0 x0F ) == 0x0A ) ) / / t h e a d r e s s o f t h e Atmel i s A
{
89
command = ( msg [ 1 ] & 0 xF0 ) >> 4 ;
/ / t h i s i s were t h e message i s a n a l y s e d
94
switch ( command ) {
c a s e 0 x01 :
c a s e 0 x02 :
c a s e 0 x03 :
c a s e 0 x04 :
c a s e 0 x05 :
c a s e 0 x06 :
c a s e 0 x07 :
c a s e 0 x08 :
c a s e 0 x09 :
}
99
104
r a i s e _ r e a d y _ f l a g = 1 ; break ;
r a i s e _ g r a p _ f l a g = 1 ; break ;
r a i s e _ t o p _ f l a g = 1 ; break ;
r e j e c t _ c l o s e d _ f l a g = 1 ; break ;
r e j e c t _ o p e n _ f l a g = 1 ; break ;
d e l i v e r _ c l o s e d _ f l a g = 1 ; break ;
d e l i v e r _ o p e n _ f l a g = 1 ; break ;
k i c k _ d c _ o n _ f l a g = 1 ; break ;
k i c k _ d c _ o f f _ f l a g = 1 ; break ;
/∗
o u t [ 0 ] = 0 x01 ;
o u t [ 1 ] = 0x0A | ( ( command +7) << 4 ) ;
a n s _ r o u t i n e ( out , o u t [ 0 ] ) ; ∗/
109
}
/ / Reset buffer counter :
msg_cnt = 0 ;
114
}
else
msg_cnt ++;
119
}
void a n s _ r o u t i n e ( char ∗msg , char nbytes )
{
124
o u t _ l e n = nbytes ;
PORTD |= TRANSMIT_ENABLE ;
UDR = out [ 0 ] ;
}
/ / s e n d command + 7 b a c k
132
C.9
Servostyring.h
void r a i s e _ r e a d y ( void ) ;
void r a i s e _ g r a p ( void ) ;
3 void r a i s e _ t o p ( void ) ;
void
void
void
8 void
r e j e c t _ c l o s e d ( void ) ;
r e j e c t _ o p e n ( void ) ;
d e l i v e r _ c l o s e d ( void ) ;
d e l i v e r _ o p e n ( void ) ;
C.10
Servostyring.c
# include <avr/ i o . h>
2 # include <avr/ s i g n a l . h>
/∗ s i g n a l ( i n t e r r u p t ) v e c t o r h a n d l i n g ∗/
# include <avr/ i n t e r r u p t . h> /∗ i n t e r r u p t f l a g s e . g . TIMSK ∗/
# include " D e f i n i t i o n s . h "
# include " Communication . h "
# include " S e r v o s t y r i n g . h "
7
# define debouncing_nr 100
/∗ P r o t o t y p e o f u s e d f u n c t i o n s ∗/
void updateAngle ( i n t servo ) ;
12 void i n i t _ t i m e r 0 ( void ) ;
void s t a r t _ t i m e r 0 ( char countvalue ) ;
void s t o p _ t i m e r 0 ( void ) ;
void i n i t _ t i m e r 1 ( void ) ;
void s t a r t _ t i m e r 1 ( void ) ;
17 void i n i t _ p o r t s ( void ) ;
void s k i t t l e _ r a i s e ( void ) ;
/ / c h a r p r e s e n t A n g l e [ 4 ] = {SORT_HOLD,BOX_M, 0 , 0 } ;
/ / c h a r newAngle [ 4 ] = {SORT_HOLD,BOX_M, 0 , 0 } ;
char presentAngle [ 4 ] = {RAISE_ATTACH_ARM, REJECT_CLOSED , DELIVER_CLOSED , 0 } ;
char newAngle [ 4 ] = {RAISE_ATTACH_ARM, REJECT_CLOSED , DELIVER_CLOSED , 0 } ;
i n t servocount [ 4 ] = { 0 , 0 , 0 , 0 } ;
char s h o u l d F l o a t [ 4 ] = { 0 , 0 , 0 , 0 } ;
27 char p r e s e n t S e r v o ;
i n t timecount = 0 ;
char update_complete = f a l s e ;
char s k i t t l e _ o u t = 1 ;
i n t tmp ;
32
/ / Flags
char r a i s e _ r e a d y _ f l a g = 0 ;
char r a i s e _ g r a p _ f l a g = 0 ;
char r a i s e _ t o p _ f l a g = 0 ;
37
char r e j e c t _ c l o s e d _ f l a g = 0 ;
char r e j e c t _ o p e n _ f l a g = 0 ;
22
char d e l i v e r _ c l o s e d _ f l a g = 0 ;
42 char d e l i v e r _ o p e n _ f l a g = 0 ;
char k i c k _ d c _ o n _ f l a g = 0 ;
char k i c k _ d c _ o f f _ f l a g = 0 ;
47 char s t a r t _ f l a g = 0 ;
char s t a r t _ c n t = 0 ;
i n t main ( i n t argn , char ∗ argv [ ] )
{
52
char b u t t o n s [ 8 ] ;
char i = 0 ;
long debounce_counter ;
char s k i t t l e _ c n t = 0 ;
57
// initialisation
init_ports () ;
init_timer0 () ;
init_timer1 () ;
init_usart () ;
62
/∗ e n a b l e a l l t h e i n t e r r u p t s ∗/
/ / HUSK AT SÆTTE TIL IGEN
sei () ;
67
while ( t r u e )
C.10. SERVOSTYRING.C
133
{
72
/ / Check a l l t h e b u t t o n s t o s e e w h e t h e r t h e y a r e p r e s s e d
f o r ( i = 0 ; i < 7 ; i ++) {
b u t t o n s [ i ]= f a l s e ;
debounce_counter = 0 ;
while ( b u t t o n _ p r e s s e d ( i ) ) {
debounce_counter ++;
i f ( debounce_counter >debouncing_nr ) {
b u t t o n s [ i ]= t r u e ;
break ;
}
}
}
77
82
/ / The g i v e n a c t i o n s h o u l d t a k e p l a c e i f t h e c o r r e s p o n d i n g s w i t c h i s p r e s s e d
/ / o r t h e c o r r e s p o n d i n g f l a g i s s e t by t h e c o m m u n i c a t i o n module
87
if
if
if
if
if
if
if
if
if
92
97
( /∗ b u t t o n s [ 0 ] = = t r u e
( /∗ b u t t o n s [ 1 ] = = t r u e
( /∗ b u t t o n s [ 2 ] = = t r u e
( /∗ b u t t o n s [ 3 ] = = t r u e
( /∗ b u t t o n s [ 4 ] = = t r u e
( /∗ b u t t o n s [ 5 ] = = t r u e
( /∗ b u t t o n s [ 6 ] = = t r u e
( kick_dc_on_flag )
( kick_dc_off_flag )
||
||
||
||
||
||
||
∗/ r a i s e _ r e a d y _ f l a g )
{ raise_ready ( ) ; }
∗/ r a i s e _ g r a p _ f l a g )
{ raise_grap ( ) ; }
∗/ r a i s e _ t o p _ f l a g )
{ raise_top ( ) ; }
∗/ r e j e c t _ c l o s e d _ f l a g )
{ reject_closed () ; }
∗/ r e j e c t _ o p e n _ f l a g )
{ reject_open ( ) ; }
∗/ d e l i v e r _ c l o s e d _ f l a g ) { d e l i v e r _ c l o s e d ( ) ; }
∗/ d e l i v e r _ o p e n _ f l a g )
{ deliver_open ( ) ; }
{ kick_dc_on ( ) ; k i c k _ d c _ o n _ f l a g = 0 ; }
{ kick_dc_off ( ) ; kick_dc_off_flag = 0 ; }
//
when
the
mech
swit
is
pres
}
return 0 ;
102 }
/ / T h i s i n t e r u p t i s c a l l e d e v e r y 20 ms
SIGNAL (SIG_OUTPUT_COMPARE1A)
{
107
int i ;
timecount ++;
i f ( timecount == 3 0 0 0 ) timecount = 0 ;
112
//
/ / r e s e t t h i s c o u n t e r e v e r y 60 s e c .
/ / s e n d t h e new a n g l e s t o t h e d i f f e r e n t s e r v o s
f o r ( i = 0 ; i < 4 ; i ++) {
f o r ( i = 0 ; i < 2 ; i ++) {
update_complete = f a l s e ;
117
updateAngle ( i ) ;
/ / w a i t a w h i l e t o make s u r e t h e u p d a t e A n g l e command h a s b e e n e x e c u t e d
sei () ;
while ( update_complete== f a l s e ) __asm__ ( " nop " ) ;
122
}
}
127
void updateAngle ( i n t servo ) {
char countvalue ;
/ / c h a r tempAngle ;
132
137
servocount [ servo ] + + ;
i f ( servocount [ servo ] = = 2 5 5 ) servocount [ servo ] = 0 ;
//
//
//
//
if
c a l c u l a t e the angle t h a t the servo should uptain using infomation o f
t h e p r e s e n t a n g l e o f t h e s e r v o a s w e l l a s t h e wanted a n g l e ( n e w _ a n g l e )
t o c o n t r o l t h e v e l o c i t y o f t h e s e r v o s movement a n e w a n g l e s h o u l d n ’ t b e s e n t
immediatly but i n s t e a d d i v i d e d i n t o s e p e r a t e a n g l e s
( newAngle [ servo ] > 0 ) {
i f ( servocount [ servo ]% v e l o c i t y 2 [ servo ] = = 0 )
134
142
{
147
/ / a new a n g l e s h o u l d b e g i v e n
i f ( ( ( presentAngle [ servo ]+ v e l o c i t y [ servo ] ) >newAngle [ servo ] ) && ( presentAngle [
servo]− v e l o c i t y [ servo ] <newAngle [ servo ] ) )
presentAngle [ servo ] = newAngle [ servo ] ;
else {
i f ( newAngle [ servo ] > presentAngle [ servo ] ) presentAngle [ servo ] +=
v e l o c i t y [ servo ] ;
i f ( newAngle [ servo ] < presentAngle [ servo ] ) presentAngle [ servo ] −=
v e l o c i t y [ servo ] ;
}
/ / p r e s e n t A n g l e [ s e r v o ]= tempAngle ;
}
/ / p r e s e n t A n g l e [ s e r v o ] = newAngle [ s e r v o ] ;
/ / update the angle of the given servo
152
p r e s e n t S e r v o = servo ;
157
s e r v o _ s e t ( servo ) ;
countvalue = presentAngle [ servo ] ;
i f ( countvalue >SERVO_180 ) countvalue = SERVO_180 ;
i f ( countvalue <SERVO_0 ) countvalue = SERVO_0 ;
s t a r t _ t i m e r 0 ( countvalue ) ;
162
}
e l s e { s e r v o _ c l e a r ( servo ) ; update_complete= t r u e ; }
}
SIGNAL (SIG_OUTPUT_COMPARE0) {
/ / this timer controls the length of the pulse
167
servo_clear ( presentServo ) ;
/ / stop this timer
stop_timer0 ( ) ;
update_complete= t r u e ;
}
172
void i n i t _ p o r t s ( void ) {
/∗ S e t t h e d a t a−d i r e c t i o n o f d i f f e r e n t p o r t s .
1 = output
0 = input
177
∗/
DDR_SERVOS
= 0xFF ;
/ / Servos
DDR_SWITCHES = 0 x00 ;
/ / b u t t o n s ( i n t h e r a i s i n g mechanisme )
DDR_BUTTONS = 0 x00 ;
/ / From t h e s t k 5 0 0 b o a r d
DDR_LEDS
= 0xFF ;
/ / LEDS
182
DDRD = 0 x07 ;
PORTB |= 0 x03 ;
/∗ S e t t h e i n i t i a l v a l u e s f o r t h e o u t p u t p o r t s ∗/
PORT_LEDS = 0xFF ;
187
}
void i n i t _ t i m e r 0 ( void ) {
// Possible prescaler settings
192
/ / Timer0 (8− b i t c o u n t e r )
//
/ / CS02 CS01 CS00
//
0
0
0
= Timer0 s t o p p e d
//
0
0
1 = clk
197
//
0
1
0
= clk /8
//
0
1
1
= c l k /64
//
1
0
0
= c l k /256
//
1
0
1
= c l k /1024
//
202
/ / e . g . TCCR0 = _BV ( CS00 ) | _BV ( CS02 ) ; / / c l k / 1 0 2 4
/ / n o t e t h a t _BV ( CS02 ) e q u a l s 0 b00000010
/ / TCCR0 =
_BV ( CS02 ) | _BV ( CS00 ) | _BV (WGM01) ;
/ / Count v a l u e
TCNT0 = 0 x00 ;
207
/ / The s t a r t v a l u e
}
212
void s t a r t _ t i m e r 0 ( char countvalue ) {
OCR0 = countvalue ;
TCNT0 = 0 x00 ;
/ / The s t a r t v a l u e
TCCR0 = _BV ( CS02 ) | _BV (WGM01) ;
TIMSK |= _BV ( OCIE0 ) ;
}
217
void s t o p _ t i m e r 0 ( void ) {
TCCR0 = 0 x00 ;
TIMSK &= ~_BV ( OCIE0 ) ;
}
222
C.10. SERVOSTYRING.C
void i n i t _ t i m e r 1 ( void ) {
TCNT1L = 0 x00 ;
TCNT1H = 0 x00 ;
227
OCR1AH = 0 x90 ;
OCR1AL = 0 x00 ;
/ / TCCR1A = _BV (WGM12) ;
TCCR1B = _BV ( CS11 ) | _BV (WGM12) ;
232
TIMSK |= _BV ( OCIE1A ) ;
}
237
void delay ( i n t time ) {
/ / d e l a y = t i m e ∗ 20ms
timecount = 0 ;
while ( timecount < time ) ;
}
/ / The d i f f e r e n t a c t i o n s
242 void r a i s e _ r e a d y ( void ) {
led_on ( 0 ) ;
newAngle [ RAISE_SERVO]= RAISE_READY ;
velocity [0] = 1;
velocity2 [0] = 2;
247
raise_ready_flag = 0;
led_off (0) ;
}
void r a i s e _ g r a p ( void ) {
led_on ( 1 ) ;
newAngle [ RAISE_SERVO]= RAISE_GRAP ;
velocity [0] = 1;
velocity2 [0] = 2;
raise_grap_flag = 0;
257
led_off (1) ;
}
void r a i s e _ t o p ( void ) {
led_on ( 2 ) ;
velocity [0] = 1;
262
velocity2 [0] = 2;
newAngle [ RAISE_SERVO]= RAISE_TOP ;
raise_top_flag = 0;
led_off (2) ;
}
267
void r e j e c t _ c l o s e d ( void ) {
/ / led_on (0) ;
newAngle [ REJECT_SERVO]= REJECT_CLOSED ;
reject_closed_flag = 0;
272
// led_off (0) ;
}
252
void r e j e c t _ o p e n ( void ) {
/ / led_on (0) ;
277
newAngle [ REJECT_SERVO]= REJECT_OPEN ;
reject_open_flag = 0;
// led_off (0) ;
}
void d e l i v e r _ c l o s e d ( void ) {
282
/ / led_on (0) ;
newAngle [ DELIVER_SERVO]= DELIVER_CLOSED ;
deliver_closed_flag = 0;
// led_off (0) ;
}
287
void d e l i v e r _ o p e n ( void ) {
/ / led_on (0) ;
newAngle [ DELIVER_SERVO]= DELIVER_OPEN ;
deliver_open_flag = 0;
292
// led_off (0) ;
}
135
www.oersted.dtu.dk
Ørsted·DTU
Automation
Danmarks Tekniske Universitet
Elektrovej
Bygning 326
2800 Kgs. Lyngby
Tel: (+45) 45 25 35 50
Fax: (+45) 45 88 12 95
E-mail: info@oersted.dtu.dk

Robot Recycling Rally

Transcription

Similar documents

Kabul City Map

Nu bor vi her - Tiderne Skifter

prVI 99-10

Sådan skal robotter være Enkle Fleksible Billige

Hvorfor skal man bruge følere/afbrydere?

Bådelaugets dåbsattest

Revy-hold i topform

ESR datavask og migrering

LokalAvisen Nyborg kort CMYK

Kort 1