'Konstanten define chkcnt 2 's muß der Starttaster gedrückt sein. define howlong 10 'minuten laufzeit bis Neustart des Programms define drehtime 10 'Drehwinkel beim Wenden; 66= 180grad 10 = 45 grad define ruecktime 16 'Rücklaufstrecke beim Wenden; 16= 10 cm define brk_time 5 'Bremszeit in #stop define brk_force 220 'Stärke der Bremse define wendetime 66 'wird ersetzt durch "intelligentes wenden" indem man dreht, bis frei ist. define spin_min 80 'in Hz; 150 = 9000 1/min 'define spin_err 100 'Untere Grenze für Not-Aus define gnd_dist 23 ' Inv. Distanz, ab der Boden erkannt wird define react_dist 55 ' Inv. Distanz, ab der reagiert wird define turn_dist 73 ' Inv. Distanz, ab der gewendet wird define r_intens 9 'Multiplikator für Objektempfindlichkeit beim drehen define turnspeed 200 'Drehgeschw. beim Boden suchen define setspeed 220 define sens_ch_dir 15 'empfindlichkeit für drehrichtungswechsel define v word 'aktuelle geschwindigkeit 0=0 2 define r word 'aktuelle rotationsgeschwindigkeit 0=mitte, - ist links; + ist rechts 4 define lastread word 'Timerstand des letzten Datenempfangs 6 define cnt byte ' some counter (recyclable) 7 define chek byte 'schalter-gedrückt-timer beim start-warten *** 8 define vs byte 'gesetzte Vorwärtsgeschwindigkeit 'define ir_dist_l byte 'Abstand zum nächsten Objekt 'define ir_dist_m byte ' 'define ir_dist_r byte ' 'Serielle kommunikation von der Fernsteuerung define rx_bef byte ' 12 define rx_befc byte ' 13 define rx_val byte ' 14 define rx_csum byte ' 15 'define r_spin byte define start_ok bit[192] '**** PORTS **** ' I's define l_bump port[5] 'linker bumper o define m_bump port[6] 'mittlerer bumper o define r_bump port[7] 'rechter bumper o define startbutt port[8] ' Startknopf o define ir_d_al ad[1] 'IR- Abstandssensor o define ir_d_ml ad[2] 'IR- Abstandssensor o define ir_d_mr ad[3] 'IR- Abstandssensor o define ir_d_ar ad[4] 'IR- Abstandssensor o 'define L_ant ad[x] 'Feldstärke der Antenne links X 'define R_ant ad[x] 'Feldstärke der Antenne rechts X define l_spin freq1 define r_spin freq2 ' O's ' links=A rechts=B define l_fwd port[3] 'linker Motor vorwärts o define l_bck port[4] 'linker Motor rückwärts o define r_fwd port[2] 'rechter Motor vorwärts o define r_bck port[1] 'rechter Motor rückwärts o define maehwerk port[9] 'Mähwerk über Relais an o define rel2 port[10] 'Relais define rel3 port[11] 'Relais define sirene port[12] 'Sirene o define l_sp da[1] 'speed linker Motor o define r_sp da[2] 'speed rechter Motor o put 14 print "programmstart "; BAUD R9600 'preset vs=setspeed #start 'and inizialise sth ? gosub stop maehwerk = off sirene= off start_ok = off chek = 0 'no button pressed within waitloop v=0:r=0 gosub drive 'stehenbleiben. year = 0 : month = 0 : day = 0 : hour = 0 : minute = 0 : second = 0 randomize timer pause 5 if startbutt=on then goto start put 15 put 17 print "waiting "; beep 453,15,0 beep 837,15,0 beep 362,15,0 rel2 = on pause 3 rel2 = off pause 3 rel3 = on pause 3 rel3 = off 'slowmode on #waitstart if rxd then goto rxstart if not (startbutt=on or second >= 30) then goto waitstart 'slowmode off if second >= 30 then goto start for cnt = 1 to chkcnt if startbutt = on then chek =chek+1 'beep 120,25,0 rel2 = on pause 17 rel2 = off pause 17 next if chek < chkcnt then goto start 'wenn der taster nicht lang genug gedrückt, zurück. 'hier geht's los! put 15 print "starting "; minute=0 : second=0 'reset counter if l_bump or r_bump or m_bump then goto start ' looktab ir_20_tab,ir_volt_m,ir_dist_m ' looktab ir_120_tab,ir_volt_r,ir_dist_r ' looktab ir_120_tab,ir_volt_l,ir_dist_l 'if (ir_dist_l<30 or ir_dist_m<30 or ir_dist_r<30) then goto start 'weg klar? sirene=on pause 50 sirene=off maehwerk= on pause 150 'if l_spin < spin_min or r_spin_m < spin_min*2 then goto start'nach 1 s check, ob Drehzahl erreicht. ' **************************************************************************************************************** start_ok = on put 15 print "mainloop "; 'Print "Datalog:" 'Print "ir-L ir-M ir-R v r l_spin r_spin" #loop 'r_spin = r_spin_m / 2 if ir_d_al= howlong then goto start '*** activate when running on MC 'if l_spin < spin_err or r_spin < spin_err then goto notaus if l_bump then gosub turnright if r_bump then gosub turnleft if m_bump then gosub turn180 'if l_bump then goto start 'if r_bump then goto start 'if m_bump then goto start if not startbutt then goto inloop gosub stop goto start #inloop r= (max(react_dist,max(ir_d_al,ir_d_ml)) - max(react_dist,max(ir_d_mr,ir_d_ar))) * r_intens if ir_d_al > turn_dist and ir_d_ar > turn_dist then gosub turn180 if ir_d_ml > turn_dist and ir_d_mr > turn_dist then gosub turn180 v= vs-(abs(r)/2) 'geschwindigkeit Relativ zum Drehbefehl reduzieren. 'XXX 'v=255-(abs(r)/2)-abs((gnd_dist*4) - (ir_d_al+ir_d_ar+ir_d_ml+ir_d_mr ))*v_intens 'v=min(max(v,-255),255) 'limitieren der Werte auf zulässiges Maß. 'XXX 'r=min(max(r,-255),255) 'limitieren der Werte auf zulässiges Maß. 'XXX 'vs= min(min(l_spin,r_spin)*2,255) put 14 print "L:";l_spin;" R:";r_spin;" "; 'print ir_d_al;" ";ir_d_ml;" ";ir_d_mr;" ";ir_d_ar;" "; gosub drive goto loop 'end of main loop ' **************************************************************************************************************** #findground '(uses cnt) (gosub aus main-loop) gosub stop '(gosub-level 2) if max(ir_d_al,ir_d_ml) > max(ir_d_mr,ir_d_ar) then r = -turnspeed else r = turnspeed v=0 #groundloop 'Finde Boden durch drehen if rxd then return if minute >= howlong then return '*** activate when running on MC 'if l_spin < spin_err or r_spin < spin_err then goto notaus if l_bump then return if r_bump then return if m_bump then return if startbutt then return if max(ir_d_al,ir_d_ml) > max(ir_d_mr,ir_d_ar) then cnt = 0 else cnt=2 if abs(max(ir_d_al,ir_d_ml)-max(ir_d_mr,ir_d_ar)) sens und die gewünschte drehrichtung anders, dann neu berechnen gosub stop '(gosub-level 2) if max(ir_d_al,ir_d_ml) > max(ir_d_mr,ir_d_ar) then r = -turnspeed else r = turnspeed cnt = cnt = sgn(max(sens_ch_dir,max(ir_d_al,ir_d_ml)) - max(sens_ch_dir,max(ir_d_mr,ir_d_ar)))+1 'Drehrichtung merken #contfindground gosub drive if not startbutt then goto inloop2 gosub stop goto start #inloop2 if ir_d_al 25 and start_ok = on then goto loop if timer - lastread > 25 and start_ok = off then goto start if rx_bef=1 then v= (rx_val-127) *2 ' 1= set V if rx_bef=2 then r= (rx_val-127) *2 ' 2= set R gosub drive goto rx_loop #rx_dataread rx_bef =0 : rx_val=0 : rx_befc=0 : rx_csum=0 'init if rxd then get rx_bef 'Befehl lesen #rx_read_again 'Einsprung, um noch ein Byte zu lesen , wenn Sync. verloren if rxd then get rx_befc 'Befehl invertiert lesen if ((rx_bef xor rx_befc) = 255) then goto rx_bef_ok 'prüfen, ob check des Befehls ok rx_bef = rx_befc 'wenn check nicht ok, 2. Wert als ersten hernehmen und if rxd then goto rx_read_again else goto rx_noread 'neu versuchen, wenn noch daten da sind. #rx_bef_ok 'Einsprung, wenn Befehl ok if rxd then get rx_val 'Wert lesen if rxd then get rx_csum 'Prüfsumme lesen if rx_csum <> (rx_bef xor rx_val) then goto rx_noread 'Prüfsumme prüfen, wenn nicht ok, alles null. lastread = timer 'merken, wann letztes mal gelesen return 'Return ins Programm, wenn alles OK #rx_noread 'Einsprung bei Fehler rx_bef =0 : rx_val=0' 'alles Null return 'Return ins Programm #turnleft gosub stop v=-vs 'zurück gosub drive pause ruecktime if r_bump then goto start 'Ende, wenn nach Rückwärtsfahrt der Schalter noch gedrückt ist. v=0 r=-vs 'drehen linksrum gosub drive pause drehtime gosub stop return #turnright gosub stop v=-vs 'zurück gosub drive pause ruecktime if l_bump then goto start 'Ende, wenn nach Rückwärtsfahrt der Schalter noch gedrückt ist. v=0 r=vs 'drehen rechtsrum gosub drive pause drehtime gosub stop return #turn180 gosub stop v=-vs 'zurück gosub drive pause ruecktime 'for cnt = 1 to ruecktime 'if l_bump then return 'if r_bump then return 'if m_bump then return 'if startbutt then return 'next if m_bump then goto start 'Ende, wenn nach Rückwärtsfahrt der Schalter noch gedrückt ist. v=0 r=sgn(rand)*vs 'drehen in beliebige Richtung gosub drive pause wendetime gosub stop return #drive 'l_sp=0 'strom weg 'r_sp=0 if (v+r)>=0 then l_fwd=on else l_fwd=off 'drehrichtung setzen l_bck = not l_fwd if (v-r)>=0 then r_fwd=on else r_fwd=off r_bck = not r_fwd l_sp=min(abs(v+r),255) 'speed setzen r_sp=min(abs(v-r),255) return #stop put 14 print "Stop "; r=0 v=0 l_fwd=on l_bck=on r_fwd=on r_bck=on l_sp=brk_force r_sp=brk_force pause brk_time l_sp=0 r_sp=0 l_bck = off r_bck = off return #notaus put 15 'Zeile 2 'put 16 'Licht an print "Notaus "; maehwerk = off sirene= off chek = 0 'no button pressed within waitloop gosub stop 'stehenbleiben. sirene=on pause 100 sirene=off #notauswait Print "."; pause 10 goto notauswait end 'table ir_20_tab 'ergibt den Abstand aus dem Spannungswert des IR-Sensors. '255 255 218 164 131 109 93 82 73 65 59 54 50 47 44 41 38 36 34 33 31 30 28 '27 26 25 24 23 22 22 21 20 20 19 19 18 18 17 17 16 16 16 15 15 14 14 14 14 '13 13 13 13 12 12 12 12 11 11 11 11 11 11 10 10 10 10 10 10 9 9 9 9 9 9 9 '9 8 8 8 8 8 8 8 8 8 8 7 7 7 7 7 7 7 7 7 7 7 7 7 7 6 6 6 6 6 6 6 6 6 6 6 6 '6 6 6 6 6 6 5 5 5 5 5 5 5 5 5 5 5 5 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 '0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 '0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 '0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 'tabend 'table ir_120_tab '255 255 255 255 208 168 141 121 105 93 83 75 69 63 58 54 50 47 44 42 39 37 '35 34 32 31 29 28 27 26 25 24 23 23 22 21 20 20 19 19 18 18 17 17 16 16 15 '15 15 14 14 14 13 13 13 13 12 12 12 12 11 11 11 11 11 10 10 10 10 10 10 9 9 '9 9 9 9 9 8 8 8 8 8 8 8 8 8 7 7 7 7 7 7 7 7 7 7 7 7 6 6 6 6 6 6 6 6 6 6 6 6 '6 6 6 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 '0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 '0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 '0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 'tabend