DEVICE SX28, OSCHS3, TURBO, STACKX, OPTIONX
FREQ 50_000_000


'===========# VARIABLES FOR GENERAL USE #================================================================

LED1 		PIN RC.2 OUTPUT
LED2 		PIN RC.3 OUTPUT
LED3 		PIN RC.4 OUTPUT
LED4 		PIN RC.5 OUTPUT
IsOn 		CON 1
IsOff 		CON 0
Yes 		CON 1
No 		CON 0


'===========# VARIABLES FOR SONIC #=======================================================================

Ping_g 		PIN RC.7 			'green label
Ping_b 		PIN RC.6 			'black label
Trigger 	CON 1 				'10 uS trigger pulse
Scale 		CON 10 				'raw x 10.00 = uS
RawToCm 	CON 2257 			'1 / 29.034 (with **)
RawToMm 	CON 22596
Green_Sonic 	CON 1				'1 for green, 0 for black
Black_Sonic 	CON 0
ball_position 	VAR Word 			'final position, resulting from switching btw. distance_1 and _2
active_sonic 	VAR BIT 			'1 for green, 0 for black
whereisit	var word			'ADDED FOR THIS MODULE


'-----------# INITIALIZE SONAR #----------------------------------------------------------------------------

active_sonic = 1 


'===========# MAIN #======================================================================================

PROGRAM Start
i VAR BYTE
GET_SONAR 		FUNC 2, 1 	
GET_BALL_POSITION 	FUNC 2, 0 		
Start:	


'led check
Led1 = ison
Led2 = ison
Led3 = ison
Led4 = ison

PAUSE 2000

Led1 = isoff
Led2 = isoff
Led3 = isoff
Led4 = isoff



DO

	whereisit = get_ball_position
	\ watch whereisit,16, udec		'ASSEMBLY CODE
    	\ BREAK					'ASSEMBLY CODE

LOOP



'-----------# GET_BALL_POSITION #-----------------------------------------------------------------------------------

FUNC GET_BALL_POSITION

temp_pos 	VAR WORD
result_pos 	VAR WORD


FOR i = 1 TO 3 					'reuse i to safe variable space

	IF active_sonic = 1 THEN 		'if green ultrasonic was the active one in the last loop..

		IF ball_position < 325 THEN 	'..and the ball position is < 325cm, then remain the active one
			temp_pos = Get_Sonar 1
		ELSE 				'else make the black one the active
			temp_pos = Get_Sonar 0
		ENDIF

	ELSE 					'if black ultrasonic was the active one in the last loop..

		IF ball_position > 225 THEN 	'..and the ball position is > 225cm, then remain the active one
			temp_pos = Get_Sonar 0
		ELSE 				'else make the green one the active
			temp_pos = Get_Sonar 1
		ENDIF

	ENDIF

						'analyze values and create results

	IF temp_pos > 350 THEN 			'sonic switches at max. 325. everthing measured above is 
						'wrong and is not to be considered
	
		temp_pos = 0
	ENDIF

	IF i = 1 THEN

		result_pos = temp_pos 		'store first value

	ELSE
		
		result_pos = result_pos + temp_pos 'add 2nd and 3rd value

	ENDIF

NEXT


result_pos = result_pos / 3 			'calc average value for three measurements

RETURN result_pos

result_pos = 0
temp_pos = 0

ENDFUNC



'-----------# GET_SONAR #-----------------------------------------------------------------------------------------
FUNC 	Get_Sonar

temp_gs 	var bit
temp_gs = 	__param1

IF temp_gs = Green_Sonic THEN

	Led4 = IsOn
	Led3 = IsOff
	active_sonic = Green_Sonic
	ping_g = IsOFF 				'set port to low
	PULSOUT ping_g, trigger 		'set port high for width of trigger variable (5). 
						'creates the pulse that I will later receive. 
						'after the pulse, port gets back to initial status (low).
	PAUSEUS 450 				'Pause 450 microseconds
	PULSIN ping_g, IsON, ball_position 	'measure the width of an incoming pulse. IsON
						'sets the transition of the measured pulse (Flankenerkennung und 
						'Triggerung: port war auf low gesetzt. nun
						'warte ich auf eine Veraenderung auf HIGH durch einfallenden Puls, den ich mit 
						'PULSOUT gesendet habe.
						'PULSIN triggert also auf steigende Flanke. Der einfallende Puls wird als 
						'highpulse bezeichnet, dessen
						'Laenge gemessen).The length of the measured pulse is stored in the word "distance_"

ELSE 						'Black Sonic
	
	Led4 = IsOff
	Led3 = IsOn
	active_sonic = Black_Sonic
	ping_b = IsOFF
	PULSOUT ping_b, trigger
	PAUSEUS 450
	PULSIN ping_b, IsON, ball_position

ENDIF

ball_position = ball_position * scale 		'adjust by calculating with floating number
ball_position = ball_position / 2 		'divide by 2, as measured distance of signal is there and back again, I only need one way.
ball_position = ball_position ** RawToMm 	'and convert to millimeters
ball_position = ball_position + 5 		'-20mm offset, +25mm to calc center of the ball == + 5
RETURN ball_position 				'quits the subroutine, if called with GOSUB. jumps back to the main program (here back to the line 
						'after: GOSUB get_sonar) gives back the distance_

ENDFUNC

 