'*********************************
'* Programma di base per Minimus *
'*********************************
' Variabili sulle porte
GPDX_Dta	VAR PORTA.6
GPDX_Clk	VAR PORTA.7
GPSX_Dta	VAR PORTB.2
GPSX_Clk	VAR PORTB.3

Led		VAR PORTB.0

Motore_sx_a	VAR PORTB.6
Motore_sx_b	VAR PORTB.7

Motore_dx_a	VAR PORTB.4
Motore_dx_b	VAR PORTB.5

Bordo_sx	VAR CMCON.6
Bordo_dx	VAR CMCON.7

' Variabili in memoria
i	 		VAR BYTE
Agg			VAR BYTE
GPDX		VAR BYTE
GPSX		VAR BYTE
Dir			VAR BYTE


' Inizio : Attendo 5 secondi prima di partire
	For i = 1 TO 5
		High Led
		Pause 600
		Low Led
		Pause 600
	Next
	Pause 200
	GoSub m_indietro
	Pause 500

' Inizializzo il Generatore di Tensione ed il Comparatore
	VRCON = %10101100
	CMCON = %00000010

	Agg = 0
	Dir = 0

lmain:
'Leggo gli Sharp
	Toggle Led

	GoSub Radar

	SerOut 1,6,["*",#GPDX,"-",#GPSX,13,10]


'Se	ho davanti il mio avversario l'ho agganciato
	IF (GPDX > 120) AND (GPSX > 120) Then
		Agg = 1
	Else
		Agg = 0
	EndIF

' Se non ho agganciato giro nella direzione in cui l'ho visto muoversi
	IF Agg = 0 Then
		IF Dir = 0 Then
			GoSub m_dx
		Else
			GoSub m_sx
		EndIF
	Else
		GoSub m_Avanti
	EndIF

' Sta svicolando a sx
	IF (GPDX < 120) AND (GPSX > 120) Then
		Dir = 1
	EndIF

' Sta svicolando a sx
	IF (GPDX > 120) AND (GPSX < 120) Then
		Dir = 0
	EndIF


' se bordo a sx giro a dx
	IF (Bordo_sx = 1) AND (Bordo_dx = 0) Then
		Dir = 0
	EndIF
' se bordo a dx giro a sx
	IF (Bordo_sx = 0) AND (Bordo_dx = 1) Then
		Dir = 1
	EndIF
' se sono con il muso sul bordo... retro
	IF (Bordo_sx = 1) AND (Bordo_dx = 1) Then
		GoSub m_Indietro
		Pause 500
	EndIF

	Pause 50
GoTo lmain

'****************************
'* Routines SHARP GD12D02   *
'****************************
Radar:
	Low GPDX_Clk
rad1:
	IF GPDX_Dta = 0 Then rad1
	ShiftIn GPDX_Dta, GPDX_Clk,2,[GPDX]
	High GPDX_Clk
	Pause 2

	Low GPSX_Clk
rad2:
	IF GPSX_Dta = 0 Then rad2
	ShiftIn GPSX_Dta, GPSX_Clk,2,[GPSX]
	High GPSX_Clk
	Pause 2

	Return

'****************************
'* Routines gestione motori *
'****************************
m_Avanti:
	GoSub m_dx_s
	GoSub m_sx_s
	GoSub m_dx_a
	GoSub m_sx_a
	Return

m_Indietro:
	GoSub m_dx_s
	GoSub m_sx_s
	GoSub m_dx_i
	GoSub m_sx_i
	Return

m_Sx:
	GoSub m_dx_s
	GoSub m_sx_s
	GoSub m_sx_i
	GoSub m_dx_a
	Return

m_Dx:
	GoSub m_dx_s
	GoSub m_sx_s
	GoSub m_sx_a
	GoSub m_dx_i
	Return

m_Sx_perno:
	GoSub m_dx_s
	GoSub m_sx_s
	GoSub m_dx_a
	Return

m_Dx_perno:
	GoSub m_dx_s
	GoSub m_sx_s
	GoSub m_sx_a
	Return

M_sx_i:
	Low  Motore_sx_a
	High Motore_sx_b
	Return

M_sx_a:
	High Motore_sx_a
	Low  Motore_sx_b
	Return

M_sx_s:
	High Motore_sx_a
	High Motore_sx_b
	Return

M_dx_i:
	Low  Motore_dx_a
	High Motore_dx_b
	Return

M_dx_a:
	High Motore_dx_a
	Low  Motore_dx_b
	Return

M_dx_s:
	High Motore_dx_a
	High Motore_dx_b
	Return