'======================================================================================= 'チーム高浜 杉浦光 'http://teamtakahama.web.fc2.com 'すべての著作権はチーム高浜にあります。 '定形文 for 2007 Japan open '======================================================================================== Dim speed1 As Byte '前PWMのスピード設定 Dim speed2 As Byte '後PWMのスピード設定 Dim irr As Word '右赤外線センサー Dim irl As Word '左赤外線センサー Dim irc As Word '中赤外線センサー Dim gr As Word 'グレースケール Dim usonicr As Word '右超音波センサー Dim usonicl As Word '左超音波センサー Dim grr As Word 'グレースケール待機位置 Dim px As Byte 'X座標 Dim grpre As Word '待機位置の誤差 Dim grline As Word '見えない壁のライン Dim grlinea As Word '見えない壁の誤差 Dim grg As Word 'ゴール直前 Dim irlinedisa As Word Dim mag As Byte 'Lookup後の方向センサー Dim mag_s As Word '方向センサーのアナログ値 Dim mag_h As Byte 'Lookup前の方向センサー Dim mode As Word 'モード Dim hanten As Word '白黒反転 Dim irdis As Word 'IRを発見する値 Dim irdisa As Word 'IRを見失う値 Dim holdcou As Word 'ホールドカウンタ Dim time As Word '1DTMF Dim adcon0 As Byte Dim adcon0_1 As Byte Dim adcon1 As Byte Dim dat As Word Output rd.0 Output rd.1 Output rd.2 Output rd.3 Output rd.4 Output rd.5 Output rd.6 Output rd.7 Output rc.1 Output rc.2 Gosub pwmset hanten=0 '白黒反転 1なら黒守り 0なら白守り ' (白攻め) (黒攻め) irdis=300 irdisa=50 irlinedisa=400 If hanten=1 Then: '白攻めの時のグレー定数をE2PROMから読み込む Read &h00,grg Read &h02,grr Read &h04,grline Gosub dtmf Sleep 100 Gosub stop grpre=6 '待機位置の誤差(帯域) grlinea=10 '見えない壁の誤差(帯域) Endif If hanten=0 Then: '黒攻めの時のグレー定数をE2PROMから読み込む Read &h06,grg Read &h08,grr Read &h10,grline grpre=20 '待機位置の誤差(帯域) grlinea=30 '見えない壁の誤差(帯域) Endif mode=1 '========================================================================================= Low rb.4 'WDTをスタート(初期化) main: 'メインループ開始 Toggle rb.4 'WDTを更新 Gosub adin 'ADの取り込み Gosub maget 'MAGを取得 If hanten=0 Then gr=1000-gr '白攻めの時にグレーの反転をする 'Debug mode," ",grr," ",gr," ",usonicr," ",usonicl,"----grr ",gr," grline= ",grline," grline-grlinea= ",grline-grlinea," grline+grlinea= ",grline+grlinea 'Debug mode," rb.0=",rb.0," rb.2=",rb.2," rb.3=",rb.3," re.2=",re.2 'Debug " ",grr," ",grg," ",grline," ",gr Debug mag '-----------------------------------------------------------------------------------横座標 If usonicr<120 Then px=2 If usonicl<120 Then px=3 If usonicr>120 And usonicl>120 Then px=1 If usonicr<80 And usonicl<80 Then px=4 '----------------------------------------------------------------------------------------- If mode=1 And mag<>0 Then Gosub magmatch0 If mode=1 Then: Gosub kickre Debug "kuou" Endif If mode=1 And mag=0 Then: 'グレーが待機位置で超音波で横に動く If gr < grr And gr > grr-grpre Then: If usonicl+30 < usonicr Then: speed1=60 speed2=120 Gosub pwmon rd=&B11011001 Endif If usonicr+30 < usonicl Then: speed1=60 speed2=120 Gosub pwmon rd=&B11100110 Endif If usonicr+30 > usonicl Then: If usonicl+30 > usonicr Then: rd=&B11111111 Endif Endif Endif Endif If mode=1 Then: '待機位置よりも前でMAGが0だったら斜めに下がる If gr > grr And mag=0 Then: If px=3 Then: speed1=200 speed2=200 Gosub pwmon rd=&B11011101 Endif If px=2 Then: speed1=200 speed2=200 Gosub pwmon rd=&B11100111 Endif If px=1 Or px=4 Then: speed1=150 speed2=150 Gosub pwmon rd=&B11110101 Endif Endif If gr < grr-grpre And mag=0 Then: '待機位置より後ろだったら speed1=50 speed2=50 Gosub pwmon rd=&B11111010 Endif Endif '========================================================================追いかけ If irr+irl+irc > irdis And mode<>50 Then: If mode<>29 Then mode=20 'IRの合計が発見判断基準よりも大きかったら Endif If mode=20 And irr+irl+irc irr+irl Then: '中のIRが一番大きかったら speed1=250 speed2=250 Gosub pwmon rd=&B10111010 Endif If irl*2 > irc+irr Then: '左のIRが一番大きかったら speed1=250 speed2=250 Gosub pwmon rd=&B10011011 Endif If irr*2 > irc+irl Then: '右のIRが一番大きかったら speed1=200 speed2=200 Gosub pwmon rd=&B10101110 Endif If rb.5=0 Then mode=50 Endif If mode=25 Then: Gosub kickre If irc*2 > irr+irl Then: '中のIRが一番大きかったら speed1=80 speed2=80 Gosub pwmon rd=&B10111010 Endif If irl*2 > irc+irr Then: '左のIRが一番大きかったら speed1=80 speed2=80 Gosub pwmon rd=&B10011011 Endif If irr*2 > irc+irl Then: '右のIRが一番大きかったら speed1=80 speed2=80 Gosub pwmon rd=&B10101110 Endif If rb.5=0 Then mode=50 Endif If mode=25 And irr+irl+irc < irdisa Then: mode=1 Endif If mode=20 And gr >= grline Then mode=29 '-------------------------------------------------------------------中立点 If mode=29 And mag=0 Then: If gr >= grline Then: speed1=50 speed2=50 Gosub pwmon rd=&B10110101 Endif If gr <= grline-grlinea Then mode=1 If gr > grline-grlinea And gr < grline Then: If irc*2 > irr+irl Then: Gosub pwmoff rd=&B10111111 Debug "中立点でボールが真ん中にある" Endif If irl*2 > irr+irc Then: speed1=128 speed2=255 Gosub pwmon rd=&B10011001 Debug "中立点でボールが左にある" Endif If irr*2 > irl+irc Then: speed1=128 speed2=255 Gosub pwmon rd=&B10100110 Debug "中立点でボールが右にある" Endif Endif If irr+irl+irc < irdisa Then mode=1 If rb.5=0 Then mode=50 Endif If mode=29 And mag<>0 Then: If mag>=8 And mag<>15 Then: speed1=120 speed2=120 Gosub pwmon rd=&B10010110 mode=1 Endif If mag=15 Then: speed1=60 speed2=60 Gosub pwmon rd=&B10010110 Endif If mag<8 And mag<>1 Then: speed1=120 speed2=120 Gosub pwmon rd=&B10101001 Endif If mag=1 Then: speed1=60 speed2=60 Gosub pwmon rd=&b10101001 Endif Endif '------------------------------------------------------------------デジタルIR If gr < grr And mode<>50 Then: If rb.2=0 And mag=0 Then mode=32 '後にボールがあるとき Endif If gr < grg And mode<>50 Then: If re.2=0 Then mode=33 If rc.5=0 Then mode=34 If rb.0=0 And rb.3=1 Then mode=30 '斜め左前にボールが有るとき If rb.3=0 And rb.0=1 Then mode=31 '斜め右前にボールが有るとき Endif If gr > grg And mode<>50 Then: If re.2=0 Then mode=42 If rc.5=0 Then mode=43 If rb.0=0 Then mode=40 If rb.3=0 Then mode=41 Endif If gr > grg And mode<>50 Then: If rb.2=0 And px=1 Then mode=44 If rb.2=0 And px=2 Then mode=44 If rb.2=0 And px=3 Then mode=45 If rb.2=0 And px=4 Then mode=45 Endif If mode=30 Then: '斜め左前にボールが有るとき speed1=250 speed2=250 Gosub pwmon rd=&B10010110 Sleep 10 Endif If mode=31 Then: '斜め右前にボールが有るとき speed1=250 speed2=250 Gosub pwmon rd=&B10101001 '斜め右に進む Sleep 10 Endif If mode=32 Then: speed1=80 speed2=80 Gosub pwmon rd=&B11111010 Sleep 500 speed1=100 speed2=100 Gosub pwmon rd=&B10101001 Sleep 600 mode=25 Endif If mode=34 Then: speed1=255 speed2=255 Gosub pwmon rd=&B11110101 Endif If mode=33 Then: speed1=255 speed2=255 Gosub pwmon rd=&B11110101 Endif If mode-9 >= 21 And mode <= 39 Then: If rb.0=1 And rb.3=1 Then: If re.2=1 And rc.5=1 Then: If rb.2=1 Then mode=1 Endif Endif Endif If mode-9 >= 31 And mode <= 49 Then: If rb.0=1 And rb.3=1 Then: If re.2=1 And rc.5=1 Then: If rb.2=1 Then mode=1 Endif Endif Endif If mode=40 And mag=0 Then: If gr > grg Then: speed1=250 speed2=250 Gosub pwmon rd=&B10100111 Endif Endif If mode=41 And mag=0 Then: If gr > grg Then: speed1=250 speed2=250 Gosub pwmon rd=&B10011101 Endif Endif If mode=42 And mag=0 Then: If gr > grg Then: speed1=250 speed2=250 Gosub pwmon rd=&B11110101 Endif Endif If mode=43 And mag=0 Then: If gr > grg Then: speed1=250 speed2=250 Gosub pwmon rd=&B11110101 Endif Endif If mode=44 And mag=0 Then: speed1=250 speed2=250 Gosub pwmon rd=&B11100111 Endif If mode=45 And mag=0 Then: speed1=250 speed2=250 Gosub pwmon rd=&B11011101 Endif If mode-9 = 39 And mode < 49 Then: If rb.0=1 And rb.3=1 Then: If re.2=1 And rc.5=1 Then: If rb.2=1 Then Gosub magmatch0 Endif Endif Endif If mode-9 = 49 And mode < 59 Then: If rb.0=1 And rb.3=1 Then: If re.2=1 And rc.5=1 Then: If rb.2=1 Then Gosub magmatch0 Endif Endif Endif '------------------------------------------------------------------------------蹴る If mode=50 Then: Gosub dtmf If mag=0 Then: Gosub kick speed1=50 speed2=50 Gosub pwmon rd=&b01111010 Sleep 50 Gosub stop speed1=255 speed2=255 Gosub pwmon Sleep 400 Gosub kickre Sleep 200 mode=1 holdcou=0 Endif If mag <> 0 Then: If mag>=8 And mag<>15 Then: speed1=120 speed2=120 Gosub pwmon rd=&B10010110 mode=1 Endif If mag=15 Then: speed1=60 speed2=60 Gosub pwmon rd=&B10010110 Endif If mag<8 And mag<>1 Then: speed1=120 speed2=120 Gosub pwmon rd=&B10101001 Endif If mag=1 Then: speed1=60 speed2=60 Gosub pwmon rd=&b10101001 Endif Endif If rd.5=1 Then holdcou=holdcou+1 If rd.5=0 Then holdcou=0 If holdcou > 100 Then mode=1 Endif Goto main '========================================================================================= dtmf: '----------3bit-8lineデコーダに接続されたDTMFモジュールでDTMFのCを発信する。 Low ra.4 Low rc.0 High rc.3 Debug "dtmf(dec001)" Return '--------------------------------------------------------------------------- kick: '----------3bit-8lineデコーダに接続されたキックデバイスで蹴る Low ra.4 Low rc.0 Low rc.3 Return '--------------------------------------------------------------------------- kickre: '----------3bit-8lineデコーダに接続されたキックデバイスで蹴ったキッカーを戻す High ra.4 Low rc.0 Low rc.3 Return '--------------------------------------------------------------------------- led: '-----------3bit-8lineデコーダに接続されたLEDを点灯する Low ra.4 High rc.0 High rc.3 Return '--------------------------------------------------------------------------- stop: '-----------現在使用されている3bit-8lineデコーダに接続されたデバイスを停止する High ra.4 High rc.0 High rc.3 Return '--------------------------------------------------------------------------- maget: '----------変数magに現在のA/Dで入力された値を0〜15に変換し、代入する Adc 6,0,mag_s If mag_s >= 665 Then mag_h=10 :Goto magend If mag_s >= 650 Then mag_h=9 :Goto magend If mag_s >= 570 Then mag_h=8 :Goto magend If mag_s >= 520 Then mag_h=7 :Goto magend If mag_s >= 480 Then mag_h=11 :Goto magend If mag_s >= 470 Then mag_h=12 :Goto magend If mag_s >= 390 Then mag_h=13 :Goto magend If mag_s >= 380 Then mag_h=14 :Goto magend If mag_s >= 280 Then mag_h=4 :Goto magend If mag_s >= 260 Then mag_h=3 :Goto magend If mag_s >= 200 Then mag_h=5 :Goto magend If mag_s >= 190 Then mag_h=6 :Goto magend If mag_s >= 90 Then mag_h=2 :Goto magend If mag_s >= 80 Then mag_h=1 :Goto magend If mag_s >= 40 Then mag_h=15 :Goto magend If mag_s >= 0 Then mag_h=0 :Goto magend magend: ' Lookup mag_h,mag,0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15 ' Lookup mag_h,mag,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,0 ' Lookup mag_h,mag,2,3,4,5,6,7,8,9,10,11,12,13,14,15,0,1 ' Lookup mag_h,mag,3,4,5,6,7,8,9,10,11,12,13,14,15,0,1,2 ' Lookup mag_h,mag,4,5,6,7,8,9,10,11,12,13,14,15,0,1,2,3 ' Lookup mag_h,mag,5,6,7,8,9,10,11,12,13,14,15,0,1,2,3,4 Lookup mag_h,mag,6,7,8,9,10,11,12,13,14,15,0,1,2,3,4,5 ' Lookup mag_h,mag,7,8,9,10,11,12,13,14,15,0,1,2,3,4,5,6 ' Lookup mag_h,mag,8,9,10,11,12,13,14,15,0,1,2,3,4,5,6,7 ' Lookup mag_h,mag,9,10,11,12,13,14,15,0,1,2,3,4,5,6,7,8 ' Lookup mag_h,mag,10,11,12,13,14,15,0,1,2,3,4,5,6,7,8,9 ' Lookup mag_h,mag,11,12,13,14,15,0,1,2,3,4,5,6,7,8,9,10 ' Lookup mag_h,mag,12,13,14,15,0,1,2,3,4,5,6,7,8,9,10,11 ' Lookup mag_h,mag,13,14,15,0,1,2,3,4,5,6,7,8,9,10,11,12 ' Lookup mag_h,mag,14,15,0,1,2,3,4,5,6,7,8,9,10,11,12,13 ' Lookup mag_h,mag,15,0,1,2,3,4,5,6,7,8,9,10,11,12,13,14 Return '--------------------------------------------------------------------------- pwmset: '----------PWMの初期設定をする Poke &h92,250 '周波数基礎 speed1=255 speed2=135 Poke &h15,speed1 'デューティー比 (PWM ONモーター3,4) Poke &h1b,speed2 'デューティー比 (PWM ONモーター1,2) Poke &h12,&b110 '1k 周波数(プリスケラー) Poke &h17,&b1100 Poke &h1d,&b1100 Return'------------------------------------------------------------------------------------ pwmon: '----------speed1,speed2の値をモーター1,2と3のPWMに適用する Poke &h15,speed1 'デューティー比 (PWM ONモーター3,4) Poke &h1b,speed2 'デューティー比 (PWM ONモーター1,2) Poke &h17,1100 'PWM ON Poke &h1d,1100 Return '---------------------------------------------------------------------------- pwmoff: Poke &h17,&b0000 'PWM OFF Poke &h1d,&b0000 Return adin: adcon1=&h80+(0 & &h0f)'     ADCON1 adcon0=&h81+((0 & &h07)<<3)'  ADCON0 adcon0_1=&h85+((0 & &h07)<<3) Gosub pp_adc'            558μS datに測定データが格納されます。 irr=dat adcon1=&h80+(0 & &h0f)'     ADCON1 adcon0=&h81+((1 & &h07)<<3)'  ADCON0 adcon0_1=&h85+((1 & &h07)<<3) Gosub pp_adc'            558μS datに測定データが格納されます。 irc=dat adcon1=&h80+(0 & &h0f)'     ADCON1 adcon0=&h81+((2 & &h07)<<3)'  ADCON0 adcon0_1=&h85+((2 & &h07)<<3) Gosub pp_adc'            558μS datに測定データが格納されます。 irl=dat adcon1=&h80+(0 & &h0f)'     ADCON1 adcon0=&h81+((3 & &h07)<<3)'  ADCON0 adcon0_1=&h85+((3 & &h07)<<3) Gosub pp_adc'            558μS datに測定データが格納されます。 usonicr=dat adcon1=&h80+(0 & &h0f)'     ADCON1 adcon0=&h81+((4 & &h07)<<3)'  ADCON0 adcon0_1=&h85+((4 & &h07)<<3) Gosub pp_adc'            558μS datに測定データが格納されます。 usonicl=dat adcon1=&h80+(0 & &h0f)'     ADCON1 adcon0=&h81+((5 & &h07)<<3)'  ADCON0 adcon0_1=&h85+((5 & &h07)<<3) Gosub pp_adc'            558μS datに測定データが格納されます。 gr=dat Return pp_adc: Poke &h9F,adcon1'        ADCON1 Poke &h1F,adcon0'        ADCON0 Poke &h1F,adcon0_1'       ADCON0 dat=Peek(&h1E) dat=(dat << 8)+Peek(&h9E) Return magmatch0: If mag>=8 And mag<>15 Then: speed1=60 speed2=60 Gosub pwmon rd=&B11010110 Endif If mag=15 Then: speed1=30 speed2=30 Gosub pwmon rd=&B11010110 Endif If mag<8 And mag<>1 Then: speed1=60 speed2=60 Gosub pwmon rd=&B11101001 Endif If mag=1 Then: speed1=30 speed2=30 Gosub pwmon rd=&b11101001 Endif Return End