'---------- Operator Interface - Digital Inputs ------------------------------

oi_swA		VAR byte	'OI Digital Switch Inputs 1 thru 8
oi_swB		VAR byte	'OI Digital Switch Inputs 9 thru 16


'---------- Robot Controller - Digital Inputs --------------------------------

rc_swA		VAR byte	'RC Digital Inputs 1 thru 8
rc_swB		VAR byte	'RC Digital Inputs 9 thru 16


'---------- Robot Controller - Analog Outputs --------------------------------

io_bc_motorlower		VAR byte	'Lower ball handling motor #1
io_bc_motorhigher		VAR byte	'Higher ball handling motor #1



'---------- Ball Handling ----------------------------------------------------
bc_previous_state_l 	VAR bit	
bc_previous_state_h 	VAR bit
bc_counterl 		VAR bit
bc_counterh 		VAR bit

'---------- Aliases for each OI switch input ---------------------------------
'  Below are aliases for the digital inputs located on the Operator Interface.
'  Ports 1 & 3 have their inputs duplicated in ports 4 & 2 respectively.  The 
'  inputs from ports 1 & 3 may be disabled via the 'Disable' dip switch 
'  located on the Operator Interface.  See Users Manual for details.

bc_switchl		VAR oi_swA.bit0
bc_switchh		VAR oi_swA.bit1
bc_motorlower     VAR oi_swA.bit2
bc_motorhigher    VAR oi_swA.bit4



'========== BALL COLLECTION & DELIVERY CONSTANTS =============================

'c_bc_ if the prefix for the constants for the ball collection and delivery team
C_BC_ONFWDL			CON	225
C_BC_ONFWDH			CON	225
C_BC_ONBWD			CON	30
C_BC_SWITCH_BWD		CON	0
C_BC_SWITCH_NEUTRAL	CON	1
C_BC_SWITCH_FWD		CON	2
C_BC_COUNTERLOOP		CON   20

bc_previous_state_l = C_BC_SWITCH_NEUTRAL	
bc_previous_state_h = C_BC_SWITCH_NEUTRAL
bc_counterl = C_BC_COUNTERLOOP
bc_counterh = C_BC_COUNTERLOOP

'---------- Global Constants ------------------------------------------------

MOTOR_STOP		CON 127	'Value at which a motor will stop
ON			CON 1		'Value for turning on a relay
OFF			CON 0		'Value for turning off a relay



'=============== BALL COLLECTION & DELIVERY OPERATIONS =======================
'=============================================================================
ball_handling:
	gosub bc_lower_assembly
	gosub bc_upper_assembly
	return

bc_lower_assembly:
	if (bc_previous_state_l = bc_switchl) then bc_motor_statusl
	if (bc_counterl > 0) then bc_counterl_dec

	bc_previous_state_l = bc_switchl

'intentional that this drops into next label area
bc_motor_statusl:
	bc_counterl = C_BC_COUNTERLOOP

	if (bc_switchl = C_BC_SWITCH_NEUTRAL) then bc_motorlower_neutral
	if (bc_switchl = C_BC_SWITCH_FWD) then bc_motorlower_fwd
	
	bc_motorlower = C_BC_ONBWD
	return

bc_counterl_dec:
	bc_counterl = bc_counterl - 1

'intentional that this drops into next label area
bc_motorlower_neutral:
	bc_motorlower = MOTOR_STOP
	return

bc_motorlower_fwd:
	bc_motorlower = C_BC_ONFWDL
	return


bc_upper_assembly:
	if (bc_previous_state_h = bc_switchh) then bc_motor_statush
	if (bc_counterh > 0) then bc_counterh_dec

	bc_previous_state_h = bc_switchh

'intentional that this drops into next label area
bc_motor_statush:
	bc_counterh = C_BC_COUNTERLOOP

	if (bc_switchh = C_BC_SWITCH_NEUTRAL) then bc_motorhigher_neutral
	if (bc_switchh = C_BC_SWITCH_FWD) then bc_motorhigher_fwd

	bc_motorhigher = C_BC_ONBWD
	return

bc_counterh_dec:
	bc_counterh = bc_counterh - 1

'intentional that this drops into next label area
bc_motorhigher_neutral:
	bc_motorhigher = MOTOR_STOP
	return

bc_motorhigher_fwd:
	bc_motorhigher = C_BC_ONFWDH
	return