Készítette: X. Y. 12.F. Konzulens tanár: W. Z

TÁVVEZÉRLÉS SOROS KOMMUNKÁCIÓVAL PARALLAX SUMOBOT ESZKÖZÖKÖN

Készítette: X. Y. 12.F Konzulens tanár: W. Z.

TÁVVEZÉRLÉS SOROS KOMMUNKÁCIÓVAL PARALLAX SUMOBOT EZKÖZÖKÖN

I. Cél: A MESTER robot mozgatáskor compass modul segítségével kiszámítja, hány fokot fordult el előző helyzetéhez képest, ezt egy kábelen keresztül elküldi SZOLGA robotnak aki a kapott fok értékből kiszámítja merre és mennyit kell forognia.

II. Megvalósítás: MESTER: Forráskód '=============================================================================== ' {$STAMP BS2} ' {$PBASIC 2.5} ' ================================MESTER========================================= ' ----------------------------------------------[ I/O DEFINICIOK ]----------------------------------------------------------DinDout PIN 6 ' Din Dout | Clk PIN 5 ' CLK |--> compass modul portjai En PIN 4 ' EN | '=========================[ VALTOZOK ; KONSTANSOK ]=============================== Reset Measure Report Ready NegMask

CON CON CON CON CON

%0000 %1000 %1100 %1100 %1111100000000000

' reset paracs a HM55B ' felmeres megkezdese parancs ' status/tengely ertekenek lekerese parancs ' 11 -> kesz, 00 -> nincs hiba ' 11-bit negtivbol 16 bitbe

x y status angle

VAR VAR VAR VAR

Word Word Nib Word

' x-tengely ' y-tengely ' Status flag ek ' elmozdulas felmeresenek eltarolasa

a r

VAR VAR

Word Word

' seged valtozo ' relativ elmozdulas

-2-

TÁVVEZÉRLÉS SOROS KOMMUNKÁCIÓVAL PARALLAX SUMOBOT EZKÖZÖKÖN

' --------------------------------------------------[ Main Routine ]--------------------------------------------------------DO

' fo hurok

a = angle GOSUB Compass_Get_Axes

' x es y ertekenek lekerese

angle = x ATN -y angle = angle */ 360

' elmozdulas kiszamitasa ' elmozdulas atvaltasa fokokba

r = a-angle

' relativ elmozdulas kiszamitasa

DEBUG HOME, "angle = ", DEC angle, " fok", CLREOL, CR, CR, "r=", SDEC r, CLREOL

' elmozdulas kiirasa 'relativ elmozdulas kiirasa

SEROUT 8, 16468, [ "!", r.HIGHBYTE, r.LOWBYTE] 'relativ elmozdulas ertekenek elkuldese a SZOLGAnak LOOP

' fo hurok ismetlese

' ---------------------------------------------------[ SUBRUTINOK ]-------------------------------------------------------Compass_Get_Axes:

' Compass module subroutine

HIGH En: LOW En ' reset parancs kuldese a HM55B -re (compass modul) SHIFTOUT DinDout,clk,MSBFIRST,[Reset\4] HIGH En: LOW En ' HM55B felmeres parancs megkezdese SHIFTOUT DinDout,clk,MSBFIRST,[Measure\4] status = 0 ' elozo status flag ek torlese DO ' status flag elenorzo hurok HIGH En: LOW En ' felmeres status parancs (Measurement status command) SHIFTOUT DinDout,clk,MSBFIRST,[Report\4] SHIFTIN DinDout,clk,MSBPOST,[Status\4] ' Status lekerese LOOP UNTIL status = Ready ' kilepes a hurokbol ha a status kesz SHIFTIN DinDout,clk,MSBPOST,[x\11,y\11] ' x es y tengely eretkenek lekerese HIGH En ' modul deaktivalasa IF (y.BIT10 = 1) THEN y = y | NegMask IF (x.BIT10 = 1) THEN x = x | NegMask

' 11 bit tarolasa elojeles szo kent (signed word) ' megismetles a masik tengelyen

RETURN '=============================================================================== -3-

TÁVVEZÉRLÉS SOROS KOMMUNKÁCIÓVAL PARALLAX SUMOBOT EZKÖZÖKÖN

Magyarázat A Compass_Get_Axes: szubrutin kiszámíttatja az iránytűmodullal a MESTER égtájakhoz viszonyított irányát fokokban, és a MESTER Északhoz viszonyított szögét (angles). A program először eltárolja az a változóba az angle értékét (ami alaphelyzetben 0) és csak utána számítja ki a jelenlegi értéket. A program ezek után átszámítja az angle értékét fokokba, majd kivonja a tárolt értéket a jelenlegiből, ezzel kiszámítva a relatív elmozdulást, ami az előző helyzethez képesti elmozdulást jelenti, és ezt elmenti az r változóba (r = a-angle). Az angle és az r kiíródik a képernyőre. Végül a MESTER továbbítja az r értékét a 8-as porton keresztül 16468-os bitrátával a SZOLGÁNAK. Ez két részletben történik, mert a robot egyszerre csak 1 byte adatot tud elküldeni, az r visszont kettőben van tárolva. Először a felső majd az alsó byte kerül elküldésre. A jel egy vezérjellel kezdődik ami az adás megkezdését jelzi (SEROUT 8, 16468, [ "!", r.HIGHBYTE, r.LOWBYTE]). Összeállítás Compass modul bekötési ábrája

 Az 1. és 2. lábat a P6-os foglalatba kell kötni. Ez a két láb felel a kommunikációért a robottal.  A 3. láb a föld.  A 4. lábat a P5-ös foglalatba kell kötni. Ez biztosítja az órajelet.  Az 5. lábat a P4-es foglalatba kell kötni. Ez kapcsolja be a modult, engedélyezi annak használatát.  A 6. láb a tápellátást biztosítja. Kommunikáció:  A kommunikációs kábelt a P8 -as foglalatba kell kötni.

-4-

TÁVVEZÉRLÉS SOROS KOMMUNKÁCIÓVAL PARALLAX SUMOBOT EZKÖZÖKÖN

SZOLGA Forráskód '{$STAMP BS2} '{$PBASIC 2.5} '====================================SZOLGA ROBOT=============================== ' 'soros kommunikacio MESTER es SZOLGA kozott 'szolga koveti a meser forgasat a kapott adatok alapjan 'b alapjan kiszamitja a forga idotartalmat es iranyat '==================================== I/O DEFINICIOK ============================== LMotor RMotor

PIN PIN

13 12

' bal szervo motor ' jobb szervo motor

'=============================== VALTOZOK ; KONSTANSOK =========================== m VAR Word ' mozgas r VAR Word ' relativ forgasszog i VAR Word ' impulzusok v CON 770 ' motor sebessege '==================================== FO PROGRAM =============================== DO SERIN 7, 16468, [ WAIT("!"), r.HIGHBYTE, r.LOWBYTE ]

' reltiv forgasszog fogadasa

DEBUG HOME, "relativ forgasszog = ", SDEC r, CLREOL, CR

' relativ forgasszog kiirasa

m = r + 360 IF m>370 THEN m=r FOR i = 1 TO r PULSOUT LMotor, v

' ha r kisseb mint 0 'forgasido megadasa

NEXT ENDIF IF m<350 THEN FOR i = 0 TO ABS r PULSOUT RMotor, v

'forgasido megadasa

NEXT -5-

TÁVVEZÉRLÉS SOROS KOMMUNKÁCIÓVAL PARALLAX SUMOBOT EZKÖZÖKÖN

ENDIF LOOP

Magyarázat A SZOLGA a 7-es portján 16468-as bitrátával fogadja a Mestertől kapott értékeket, ha előtte megkapja a vezérjelet (SERIN 7, 16468, [ WAIT("!"), r.HIGHBYTE, r.LOWBYTE ]). A relatív forgásszög értékéhez hozzáad 360-at mert a későbbi függvények nem tudják az előjeles számokat értelmezni. A kapott értéket eltárolja m változóban( m = r + 360). Ha az m értéke nagyobb 370-nél akkor a bal oldali motor fog forogni, ha pedig kisseb 350-nél akkor a jobb oldali. Van 20 fok ráhagyás hogy ne következzen be túlvezérlés. A motornak leadott sebessége 770-re lett állítva, mert ezzen az értéken 1° eltérést 1 darab impulzussal lehet elérni. Az impulzusok egy for ciklussal vannak számolva. A ciklus annyiszor fut le amennyi az r abszolút értéke.

Összeállítás A kommunikácios kábelt a P7-es foglalatba kell kötni.

III. Megjegyzések: Mindkét SumoBot-nak rákötve kell lennie a számítógépre soros kábelükön keresztül, hogy működjön a program.

-6-