'=======================================================================================
'チーム高浜 安達良
'http://teamtakahama.web.fc2.com
'すべての著作権はチーム高浜にあります。

'フォワード for 2007
'========================================================================================

Dim speed1 As Byte
Dim speed2 As Byte

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 mag As Byte
Dim mag_s As Word

Dim mode As Word
Dim pmmode As Word

Dim sou_12 As Word
Dim cou As Word
Dim ampm As Word

Dim irfin As Word
Dim sarcou As Word
Dim pycou As Word
Dim ucou As Word
Dim spcoul As Word
Dim spcour As Word

Dim acou As Word

Dim px As Word
Dim py As Word
Dim pm As Word

Dim gra As Word
Dim grb As Word
Dim grc As Word

Dim py_2 As Word

Dim pir As Word

Dim smag As Byte
Dim scou As Word

Dim kickmode As Word

Dim dat As Word
Dim adcon0 As Byte
Dim adcon0_1 As Byte
Dim adcon1 As Byte

Dim hanten As Byte

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

Read &h00,gra,grb,grc					'E2PROMからのgrの値

Gosub pwmset

mode=10
pycou=0
kickmode=0 							'キッカー正常＝０、キッカー故障＝１
hanten=0							'黒攻め＝０、白攻め＝１
'==============================================================================メインはじめ
Low rb.4
main:
Toggle rb.4
Gosub adin
Gosub maget
'Debug gra," ",grb," ",grc," ",gr," ",py_2
'Debug px,py_2," ",usonicr," ",usonicl," ",mag," ",irr+irl+irc
'Debug " irr=",irr," irc=",irc," irl=",irl," ",mode," 合計",irr+irl+irc
'Debug pir," ",mode," ",px,py_2," ",py," ",mag," ",pm
'Sleep 10
'Serout pb115200,px,py,"  mag-",mag,"  mode-",mode,"  pir-",pir,CHR$(&H0a0d)
'------------------------------------------------------------------------------mag確認
'If mag=0 Then:
'Gosub led
'Else
'Gosub stop
'Endif
'------------------------------------------------------------------------------X座標の判断
px=0									'縦列真中
If usonicr < 80 Then px=2				'縦列右側
If usonicl < 80 Then px=3				'縦列左側
If usonicr<80 And usonicl<80 Then px=1	'エラー時
'------------------------------------------------------------------------------Y座標の判断
If hanten=0 Then:						'黒攻め用
	py_2=1								'py=1(コート前方)
	If gr > gra Then py_2=2				'py=2(コート中央　前方)
	If gr > grb Then py_2=3				'py=3(コート中央　後方)
	If gr > grc Then py_2=4				'py=4(コート後方)
Endif

If hanten=1 Then:						'白攻め
	py_2=4								'py=4(コート後方)
	If gr > gra Then py_2=3				'py=3(コート中央　後方)
	If gr > grb Then py_2=2				'py=2(コート中央　前方)
	If gr > grc Then py_2=1				'py=1(コート前方)
Endif
'------------------------------------------------------------------------------irセンサーの目安
pir=0
If irr+irl+irc > 350 Then pir=2
If irr+irl+irc > 1000 Then pir=1
'------------------------------------------------------------------------------magの目安値
pm=0
If mag >=0 Then pm=12
If mag >=4 Then pm=9
If mag >=11 Then pm=6
If mag >=14 Then pm=3
'------------------------------------------------------------------------------ボールの有無
'================================================ボールを捜すときの方向誘導（回る方向転換）
If rb.3=0 Then irfin=0					'irfin=0にする（294行あたりで使う）回る方向切り替えのため
If rb.2=0 Then irfin=1					'irfin=1にする（294行あたりで使う）回る方向切り替えのため


If mode=10 Then:
	sarcou=sarcou+1
	speed1=40							'後輪スピード設定
	speed2=40							'前輪スピード設定
	Gosub pwmon							'PWMをONにする
	If irfin=1 Then:					'最後に見つけたのが左側のセンサーだったら
		rd=&B11100101
	Else								'最後に見つけたのが右側のセンサーだったら（そうでなかったら）
		rd=&B11011010
	Endif
'================================================ボールを探す時の方向誘導（回る位置移動）

	If sarcou > 50 Then:				'sarcou > 100（sarcouが100以上）になったら
		If py_2 <> 2 Then:				'py <> 2(pyが2以外)
			mode=11
		Else
			mode=10
		Endif
	Endif

	If mode=10 Then:
		If pir=1 Or pir=2 Then:			'ボール追跡モードへの確認
			mode=20						'ボール追跡モードへ（mode=20)
			sarcou=0
		Endif
	Endif
Endif

If mode=11 Then:
'================================================現在方位が1〜7だったら
	If pm=9 Or pm=12 Then:
		speed1=40						'後輪スピード設定
		speed2=40						'前輪スピード設定
		Gosub pwmon						'PWMをONにする
		If irfin=1 Then:				'最後に見つけたのが左側のセンサーだったら
			rd=&B11100101
		Else							'最後に見つけたのが右側のセンサーだったら（そうでなかったら）
			rd=&B11011010
		Endif
	Endif
'================================================'現在方位が8〜15だったら
	If pm=3 Or pm=6 Then:
		speed1=40						'後輪スピード設定
		speed2=40						'前輪スピード設定
		Gosub pwmon						'PWMをONにする
		If irfin=1 Then:				'最後に見つけたのが左側のセンサーだったら
			rd=&B11100101
		Else							'最後に見つけたのが右側のセンサーだったら（そうでなかったら）
			rd=&B11011010
		Endif
	Endif
'================================================現在方向が0だったら
	If mag = 0 Then:					'方位が0だったら
		Gosub pwmoff					'PWMをOFFにして
		rd=&B11111111					'止まる
		Sleep 50
		mode=12
		sarcou=0
	Endif
	If mode=11 Then:
		If pir=1 Or pir=2 Then:			'ボール追跡モードへの確認
			mode=20						'ボール追跡モードへ（mode=20）
			sarcou=0
		Endif
	Endif

Endif

If mode=12 Then:
	sarcou=sarcou+1
		If sarcou > 100 Then:
			sarcou=0
			mode=10
		Endif
Endif

If mode=12 Then:
	If py_2=1 Then:
		speed1=200
		speed2=200

		If px=3 Then:
			Gosub pwmon
			rd=&B11011101
		Endif

		If px=2 Then:
			Gosub pwmon
			rd=&B11101011
		Endif

		If px=1 Or px=0 Then:
			Gosub pwmon
			rd=&B11111001
		Endif
	Endif

	If py_2=3 Then:					'py=3(pyが３{コート後方《味方側》})だったら
		speed1=200
		speed2=200
		If px=3 Then:
			Gosub pwmon
			rd=&B11010111
		Endif

		If px=2 Then:
			Gosub pwmon
			rd=&B11101110
		Endif

		If px=1 Or px=0 Then:
			Gosub pwmon
			rd=&B11110110
		Endif
	Endif

	If py_2=4 Then:						'py=4(pyが４{コート後方《味方側》})だったら
		speed1=200
		speed2=200
		Gosub pwmon
		rd=&B11000110
	Endif

	If py_2=2 Then:
		mode=10
		sarcou=0
	Endif

	If mag<>0 Then:
		speed1=25						'後輪スピード設定
		speed2=25						'前輪スピード設定
'================================================現在方位が1〜7だったら
		If pm=9 Or pm=12 Then:
			Gosub pwmon					'PWMをONにする
			rd=&B11010010				'時計周りに回る
		Endif
'================================================現在方位が8〜15だったら
		If pm=3 Or pm=6 Then:
			Gosub pwmon					'PWMをONにする
			rd=&B11100101
		Endif
'================================================現在方向が0だったら
		If mag = 0 Then:				'方位が0だったら
			Gosub pwmoff				'PWMをOFFにして
			rd=&B11111111				'止まる
			Sleep 10
			mode=12
		Endif
	Endif
		If pir=1 Or pir=2 Then:			'ボール追跡モードへの確認
			mode=20						'ボール追跡モードへ（mode=20）
			sarcou=0
		Endif
Endif
'------------------------------------------------------------------------------ボールの追跡（ボール遠）
If mode=20 Then:						'ボール追跡モード（mode=2）
	If pir=2 Then:						'ボールが遠くにあったら
'Debug"遠い"
'================================================守り側正面
		If irr+irl < irc*10 Then:
			'speed1=255-(irc/3)
			'If irc<>0 Then:
			'If 255-(irc/3)<=100 Then speed1=50
			'If 255-(irc/3)>=700 Then speed1=700
			'speed2=100					'前輪スピード設定
			'Endif
			speed1=250
			speed2=250
			Gosub pwmon					'PWMをONにする
			rd=&b10000110
			irfin=0						'irfin=0にする（294行あたりで使う）回る方向切り替えのため
		Endif
'================================================守り側右方
		If irc+irl < irr Then:
		spcour=spcour+1
			speed1=70					'後輪スピード設定
			speed2=70					'前輪スピード設定
			Gosub pwmon					'PWMをONにする
			rd=&B10101110
			irfin=0						'irfin=0にする（294行あたりで使う）回る方向切り替えのため
		Endif
'================================================守り側左方
		If irc+irr < irl Then:
		spcoul=spcoul+1
			speed1=70					'後輪スピード設定
			speed2=70					'前輪スピード設定
			Gosub pwmon					'PWMをONにする
			rd=&B10010111
			irfin=1						'irfin=1にする（294行あたりで使う）回る方向切り替えのため
		Endif

	Endif
Endif


'------------------------------------------------------------------------------ボールの追跡（ボール近）
If mode=20 Then:						'ボール追跡モード（mode=2）
	If pir=1 Then:
'================================================守り側正面
		If irr+irl < irc*4 Then:
			'speed1=255-(irc/3)
			'If irc<>0 Then:
			'If 255-(irc/3)<=100 Then speed1=50
			'If 255-(irc/3)>=700 Then speed1=700
			'speed2=100					'前輪スピード設定
			'Endif
			speed1=100
			speed2=100
			Gosub pwmon					'PWMをONにする
			rd=&b10000110
			irfin=0						'irfin=0にする（294行あたりで使う）回る方向切り替えのため
		Endif
'================================================守り側右方
		If irl+irc < irr Then:
		spcour=spcour+1
			speed1=70
			speed2=70
			Gosub pwmon					'PWMをONにする
			rd=&B10101110
			irfin=0						'irfin=0にする（294行あたりで使う）回る方向切り替えのため
		Endif
'================================================守り側左方
		If irr+irc < irl Then:
		spcoul=spcoul+1
			speed1=70					'後輪スピード設定
			speed2=70					'前輪スピード設定
			Gosub pwmon					'PWMをONにする
			rd=&B10010111
			irfin=1						'irfin=1にする（294行あたりで使う）回る方向切り替えのため
		Endif
	Endif
Endif

If mode=20 Then:
	If spcoul Or spcour >= 20 Then:
		If acou >= 70 Then:				'acouが７０以上（ロボが何らかの故障で動かなくなったら）
			speed1=200
			speed2=200
			If spcoul >= 20 Then:
				Gosub pwmon				'PWMをONにする
				rd=&B11100100			'反時計回りに回る
				Sleep 200
				spcoul=0
			Endif
			If spcour >= 20 Then:
				Gosub pwmon
				rd=&B11010010			'時計周りに回る
				Sleep 200
				spcour=0
			Endif
			mode=10
		Endif
	Endif
Endif


If mode=20 And smag=mag Then acou=acou+1'magをsmagに入れる

If mode=20 And smag <> mag Then acou=0	'smagがmagでなったら

If mode <> 20 Then acou=0				'mode=20以外になっら、カウントリセット

If mode=20 Then smag=mag				'magをsmagに入れる

'------------------------------------------------------------------------------ボール、ホールド確認
If mode=20 Then:						'ホールドしたら
	If rb.5=0 Then:						'rb.5=0だったら
		Sleep 50
		rd=&B10111111
		Sleep 100

		If py=1 Or py=4 Then:
			If py=1 Then:
				If pm=12 Or pm=3 Then:
					Sleep 50
					rd=&B10111111
					Sleep 50
					speed1=100			'後輪スピード設定
					speed2=100			'前輪スピード設定
					Gosub pwmon
					rd=&B10001001
					Sleep 500
				Endif
			Endif

			If py=4 Then:
				If pm=6 Or pm=9 Then:
					Sleep 50
					rd=&B10111111
					Sleep 50
					speed1=100			'後輪スピード設定
					speed2=100			'前輪スピード設定
					Gosub pwmon
					rd=&B10001001
					Toggle rb.4			'CPU暴走防止に使用している
					Sleep 200
				Endif
			Endif
		Endif
	mode=30								'mode=30へ（方向誘導）
	Endif
Endif

If mode=20 And pir=0 Then mode=10		'ボールが離れてしまったら

If mode=20 Then py=py_2


'------------------------------------------------------------------------------タッチセンサー確認
If mode=30 Then:
	speed1=60
	speed2=120
	Gosub pwmon
 		If rc.5=0 Then:					'タッチセンサー左確認
			rd=&b10010101
			Sleep 200
			mode=30
		Endif

		If rc.4=0 Then:					'タッチセンサー右確認
			rd=&b10101010
			Sleep 200
			mode=30
		Endif
Endif

'------------------------------------------------------------------------------方向誘導（mag=0にする）
If mode=30 And smag=mag Then scou=scou+1'magをsmagに入れる

If mode=30 And smag <> mag Then scou=0	'smagがmagでなったら

If mode <> 30 Then scou=0				'mode=30以外になっら、カウントリセット

If mode=30 And scou > 20 Then:			'scouが２０以上（ロボが何らかの故障で動かなくなったら）
	speed1=100
	speed2=100
	Gosub pwmon
	rd=&b10001001
	Sleep 100
	scou=0
Endif

If mode=30 Then:						'mode=3だったら（ボールをホールドしたら）
'================================================現在方位が1〜7だったら
	If pm=9 Or pm=12 Then:
		speed1=100					'後輪スピード設定
		speed2=100					'前輪スピード設定
		Gosub pwmon						'PWMをONにする
		rd=&B10010010					'時計周りに回る
	Endif
Endif
'================================================'現在方位が8〜15だったら
If mode=30 Then:						'mode=30だったら（ボールをホールドしたら）
	If pm=3 Or pm=6 Then:
		speed1=100					'後輪スピード設定
		speed2=100	 				'前輪スピード設定
		Gosub pwmon						'PWMをONにする
		rd=&B10100100					'反時計回りに回る
	Endif
Endif

'================================================現在方向が0だったら
If mode=30 Then:						'mode=3だったら（ボールをホールドしたら）

	If mag = 0 Then:					'方位が0だったら
		Gosub pwmoff					'PWMをoffにする
		rd=&B10111111
		Sleep 100
	Endif

	If mag=0 And px=0 Then mode=50

Endif

If mode=30 And pir=0 Then mode=10		'ボールが離れてしまったら

If mode=30 And mag=0 Then cou=cou+1		'mode=3（ボールをホールドしたら）でmag=0だったら

If mode=30 And mag<>0 Then cou=0		'mode=3（ボールをホールドしたら）でmag=0以外だったら

If mode=30 And cou > 1 Then:			'mode=3（ボールをホールドしたら）で一定の時間mag=0だったら
	mode=40								'mode=4にする（ボールをホールドしていてmag=0だったら）
	Gosub pwmon						'PWMをONにする
	rd=&B10111111
	Sleep 100
Endif


If mode=30 Then smag=mag				'magをsmagに入れる

'------------------------------------------------------------------------------シュート方向への角度あわせ
If mode=40 Then:						'mode=4にする（ボールをホールドしていてmag=0だったら）
	If px=1 And py=1 Then:				'(11)コート前方のエラー時
		speed1=80						'後輪スピード設定
		speed2=80						'前輪スピード設定
		Gosub pwmon						'PWMをONにする
		rd=&B10001001
		Sleep 700
		rd=&B10111111
		Sleep 100
		mode=50
	Endif

	If px=1 Then:
		mode=50
	Endif

'================================================コート右より
	If px=2 Then:						'コート右より（超音波{y軸}）
		rd=&b10111111
		Sleep 5
		speed1=50						'後輪スピード設定
		speed2=50						'前輪スピード設定
		Gosub pwmon						'PWMをONにする

		If py=4 Then:					'（24）コート後方　右より
			rd=&B10011010				'時計周りに回る
			Sleep 20
			mode=50
		Endif

		If py=3 Then:					'（23）コート中央（後方）右より
			rd=&B10011010				'時計周りに回る
			Sleep 30
			mode=50
		Endif

		If py=2 Then:					'（22）コート中央（前方）右より）
			rd=&B10011010				'時計周りに回る
			Sleep 40
			mode=50
		Endif

		If py=1 Then:					'（21）コート前方右より
			rd=&B10011010				'時計周りに回る
			Sleep 400
			mode=50
		Endif
	Endif
Endif

'================================================コート左より

If mode=40 Then:						'mode=4にする（ボールをホールドしていてmag=0だったら）
	If px=3 Then:						'コート左より（超音波{y軸}）
		rd=&b10111111
		Sleep 5
		speed1=50						'後輪スピード設定
		speed2=50						'前輪スピード設定

		Gosub pwmon						'PWMをONにする

		If py=4 Then:					'（34）コート後方　左より
			rd=&B10100101				'反時計回りに回る
			Sleep 20
			mode=50
		Endif

		If py=3 Then:					'（33）コート中央（後方）左より
		rd=&B10100101					'反時計回りに回る
			Sleep 30
			mode=50
		Endif

		If py=2 Then:					'（32）コート中央（前方）左より
			rd=&B10100101				'反時計回りに回る
			Sleep 40
			mode=50
		Endif

		If py=1 Then:					'（31）コート前方　左より
			rd=&B10100101				'反時計回りに回る
			Sleep 400
			mode=50
		Endif
	Endif
Endif


'================================================その他の場合

If px=0 And mode=40 Then mode=50		'真中のライン

If mode=40 And rb.5=1 Then cou=cou+1	'ボールが離れてしまったら（ボールをホールドしていてmag=0だったら）

If mode=40 And rb.5 = 0 Then cou=0		'ボールがもとに戻ったら

If mode=40 And cou > 5 Then mode=10		'ボールが離れてしまったら

'------------------------------------------------------------------------------ボールキックモード
If mode=50 Then:
	If kickmode=0 Then:
		rd=&b11111111
		Gosub kick
		Sleep 400
		Gosub kickre
		mode=10
	Else
		speed1=250					'後輪スピード設定
		speed2=250					'前輪スピード設定
		Gosub pwmon						'PWMをONにする
		rd=&B01000110
		Sleep 750
		rd=&b11111111
		Sleep 1000
		mode=10
	Endif
Endif

If rb.7=1 Then:
	If mag<>0 Then:
		speed1=50						'後輪スピード設定
		speed2=50						'前輪スピード設定
'================================================現在方位が1〜7だったら
		If pm=9 Or pm=12 Then:
			Gosub pwmon					'PWMをONにする
			rd=&B11010010				'時計周りに回る
		Endif
'================================================現在方位が8〜15だったら
		If pm=3 Or pm=6 Then:
			Gosub pwmon					'PWMをONにする
			rd=&B11100101
		Endif
'================================================現在方向が0だったら
		If mag = 0 Then:				'方位が0だったら
			Gosub pwmoff				'PWMをOFFにして
			rd=&B11111111				'止まる
			Sleep 100
			speed1=250
			speed2=250
			Gosub pwmon
			rd=&B00001001
			Sleep 500
			mode=30
		Endif
	Endif
Endif

Goto main
'=========================================================================================

dtmf:									'3bit-8lineデコーダに接続されたDTMFモジュールでDTMFのCを発信する。
	Low ra.4
	Low rc.0
	High rc.3
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で入力された値を０〜15に変換し、代入する
	Adc 6,0,mag_s
		If mag_s >= 665 Then  mag=10 :Goto magend
		If mag_s >= 650 Then  mag=9 :Goto magend
		If mag_s >= 570 Then  mag=8 :Goto magend
		If mag_s >= 520 Then  mag=7 :Goto magend 
		If mag_s >= 480 Then  mag=11 :Goto magend
		If mag_s >= 470 Then  mag=12 :Goto magend
		If mag_s >= 390 Then  mag=13 :Goto magend
		If mag_s >= 380 Then  mag=14 :Goto magend
		If mag_s >= 280 Then  mag=4 :Goto magend
		If mag_s >= 260 Then  mag=3 :Goto magend
		If mag_s >= 200 Then  mag=5 :Goto magend
		If mag_s >= 190 Then  mag=6 :Goto magend
		If mag_s >= 90 Then   mag=2 :Goto magend
		If mag_s >= 80 Then   mag=1 :Goto magend
		If mag_s >= 40 Then   mag=15 :Goto magend
		If mag_s >= 0 Then    mag=0 :Goto magend
	magend:
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

End