TÁVVEZÉRLÉS SOROS KOMMUNKÁCIÓVAL ESZKÖZÖKÖN Készítette: X. Y. 12.F Konzulens tanár: W. Z.
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. MESTER: II. Megvalósítás: 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 CON %0000 ' reset paracs a HM55B Measure CON %1000 ' felmeres megkezdese parancs Report CON %1100 ' status/tengely ertekenek lekerese parancs Ready CON %1100 ' 11 -> kesz, 00 -> nincs hiba NegMask CON %1111100000000000 ' 11-bit negtivbol 16 bitbe x VAR Word ' x-tengely y VAR Word ' y-tengely status VAR Nib ' Status flag ek angle VAR Word ' elmozdulas felmeresenek eltarolasa a VAR Word ' seged valtozo r VAR Word ' relativ elmozdulas - 2 -
' --------------------------------------------------[ Main Routine ]--------------------------------------------------------- DO ' fo hurok a = angle GOSUB Compass_Get_Axes angle = x ATN -y angle = angle */ 360 r = a-angle DEBUG HOME, "angle = ", DEC angle, " fok", CLREOL, CR, CR, "r=", SDEC r, CLREOL ' x es y ertekenek lekerese ' elmozdulas kiszamitasa ' elmozdulas atvaltasa fokokba ' relativ elmozdulas kiszamitasa ' 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 -
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 -
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 PIN 13 ' bal szervo motor RMotor PIN 12 ' 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 ] DEBUG HOME, "relativ forgasszog = ", SDEC r, CLREOL, CR ' reltiv forgasszog fogadasa ' relativ forgasszog kiirasa m = r + 360 IF m>370 THEN ' ha r kisseb mint 0 m = r FOR i = 1 TO r 'forgasido megadasa PULSOUT LMotor, v NEXT ENDIF IF m<350 THEN FOR i = 0 TO ABS r PULSOUT RMotor, v 'forgasido megadasa NEXT - 5 -
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 -