'		Version 1.0- Added basic code
'		Version 2.0- Added storing variables in EEPROM


'------------- CONSTANTS -----------------------------
DRV_BUFFER	CON	10

'---------- GLOBAL CONSTANTS -------------------------
ON		CON	1
OFF		CON	0
MOTOR_STOP	CON	127

'---------- Auto Calibration Variables ---------------
calibrate_button	VAR	bit
joy_1_min		VAR	byte
joy_1_max		VAR	byte
joy_2_min		VAR	byte
joy_2_max		VAR	byte
joy_1_rest		VAR	byte
joy_2_rest		VAR	byte
inp_joy_1		VAR	byte
inp_joy_2		VAR	byte




'===================== Perform Operations =============
'============== AUTO-CALIBRATION ======================
calibrate_main:
	if (calibrate_button = OFF) then begin_wait
	joy_1_min	= 127						'Reset the motors to 127 for calibrating
	joy_1_max	= 127
	joy_2_min	= 127
	joy_2_max	= 127
	joy_1_rest	= inp_joy_1
	joy_2_rest	= inp_joy_2


calibrate_status_reset:
	Serin COMA\COMB, INBAUD, [oi_swA,oi_swB,rc_swA,rc_swB,inp_joy_1,PB_mode,inp_joy_2]	'Input the switches and joysticks
	toggle 8													'Toggle light
	toggle 7

calibrate_1_axis:													'Calibrate the 1st axis
	if(inp_joy_1 > joy_1_max) then set_1_max			'If the input is greater than the current max then reset it
	if(inp_joy_1 < joy_1_min) then set_1_min			'If the input is less than the current min then reset it
	goto calibrate_2_axis

set_1_max:
	joy_1_max = inp_joy_1						'Put the current 1st axis input in joy_1_max
	goto calibrate_2_axis						'Go calibrate second axis

set_1_min:
	joy_1_min = inp_joy_1						'Put the current 1st axis input in joy_1_min

calibrate_2_axis:								'Calibrate the 2nd axis
	if(inp_joy_2 > joy_2_max) then set_2_max			'If the input is greater than the current max then reset it
	if(inp_joy_2 < joy_2_min) then set_2_min			'If the input is less than than the current min then reset it
	goto check_end

set_2_max:
	joy_2_max = inp_joy_2						'Put the current 2nd axis input in joy_2_max
	goto check_end

set_2_min:	
	joy_2_min = inp_joy_2						'Put the current 2nd axis input in joy_2_max

check_end:
	Serout USERCPU, OUTBAUD, [255,255,MOTOR_STOP,relayA,MOTOR_STOP,relayB,MOTOR_STOP,MOTOR_STOP,MOTOR_STOP,MOTOR_STOP,MOTOR_STOP,MOTOR_STOP,MOTOR_STOP,MOTOR_STOP,MOTOR_STOP,MOTOR_STOP,MOTOR_STOP,MOTOR_STOP,MOTOR_STOP,MOTOR_STOP]
	if (calibrate_button = ON) then calibrate_main		'Check to see if the calibrate button is on, if so, then go back again
	WRITE 0, joy_1_min						'The following section directly writes to the EEPROM
	WRITE 1, joy_1_max						'the first number is the EEPROM location split by a comma while
	WRITE 2, joy_2_min						'the second is the data to be stored in that location
	WRITE 3, joy_2_max						'For example store 234 in location: 6 WRITE 6, 234
	WRITE 4, joy_1_rest
	WRITE 5, joy_2_rest
	goto begin_wait