'Dennis's grabber program 2.0
'This program should make the grabber rotate clockwise and counter clockwise.
'The change from 1.0 is that this one works!!!!!!!!

'-------------------Constants----------------------------------------------
'Global
ON			con	1
OFF			con	0	
MOTOR_STOP		con	127	'Sets the motor to idle

'Local
C_GR_SWITCH_UP	con	1
C_GR_SWITCH_DOWN	con	0
C_GR_REV		con	30	'Sets the motor to reverse
C_GR_FWD		con	225	'Sets the motor to forward

'------------------Inputs and Outputs-----------------------------------------------
'Inputs
inp_gr_tog_switch		var	bit	'This part stores if the switch is up or down   (port 1 pin 14)
inp_gr_sensor_up		var	bit	'This part stores if the sensor up is on    (digital 1)
inp_gr_sensor_down	var	bit	'This part stores if the sensor down is on   (digital 4)

'Outputs
out_gr_motor		var	byte	'This part stores what the motor does   (pwm5)

'-------------------------Grabber Main Program-----------------------------------------------------
grabber_main:
	if (inp_gr_tog_switch = C_GR_SWITCH_UP) then grabber_up	'This part checks if the switch is up or down

	if (inp_gr_sensor_down = OFF) then grabber_move_rev		'This part checks if sensor down senses anything
	out_gr_motor = MOTOR_STOP
	return

grabber_up:
	if (inp_gr_sensor_up = OFF) then grabber_move_fwd		'This part checks if sensor up senses anything
	out_gr_motor = MOTOR_STOP
	return

grabber_move_rev:
	out_gr_motor = C_GR_REV		'This part runs the motor reverse
	return

grabber_move_fwd:
	out_gr_motor = C_GR_FWD		'This part runs the motor forward
	return