'Written by NROSoft										v4.03
'02/16/02
'
'         N       N   RRRRRRR       OOOOO       SSSSS                  ff	 t 
'         NN      N   R      R     O     O     S     S                f  f     t
'         N N     N   R       R   O       O   S                       f        t
'         N  N    N   R       R   O       O    S                      f        t
'         N   N   N   RRRRRRRR    O       O     SSSSS      ooooo    ffffff   ttttt
'         N    N  N   R    R      O       O          S    o     o     f        t
'         N     N N   R     R     O       O           S   o     o     f        t
'         N      NN   R      R     O     O    S      S    o     o     f        t t
'         N       N   R       R     OOOOO      SSSSSS      ooooo      f         t
'
'						  |
'						  \->A division of the GearGrinders (Team 112)
'
'		Version Updates:
'			1.00 Beginning, got to start somewhere.
'			1.01 Adding Ball Handler code
'			2.00 Combined Ball Handling code and goal grabber code
'			2.01 Fixed minor issues with the ball handling and goal grabber code
'			3.00 Combined drive code
'			3.01 Added some pins and corrected some constant's names
'				 Added auto-calibration code
'			3.02 Replaced Drive code with new drive code. Made SERIN and SEROUT
'			3.03 Took out auto-calibration too many variables. :-(
'				Updated drive code
'				Added caster code
'			3.04 Added Caster and grabber debugs
'				Added auto-calibration in again
'				Learned what an alias was! No longer too many variables :-)
'			3.05 Totally changed drive code so it fits with the auto-calibration
'			3.06 Finished changing drive constants so they fit with auto-calibration
'				Learned that debug statements take up EEPROM :-(
'				  It will be fixed by commenting out any unused debugs during that specific test :-)
'			3.07 Reviewed by Chris and changed LEDs on robot controller to constants
'				  One week to go and we still don't have a finished robot :-(
'			3.08 Added code to deploy caster when drive is at an extreme
'				  modified above modification
'				  corrected drive constants
'				  added drive commenting for the caster routines
'				  A couple days to go and no robot :-(
'			3.09 Started Testing Grabber and Caster and ball handling are done
'			3.10 Commented out drive code made unneccessary by going with one joystick
'			4.00 Tested the inputs and outputs made them right. (Big jump) 
'			4.01 Added the caster pot
'
'{$STAMP BS2sx}

'================================================================================================
' 					==== BASIC IDEA ====
' The drive team is assigned the variable and constant prefix "drv_"
' The grabber team is assigned the variable and constant prefix "gr_"
' The caster team is assigned the variable and constant prefix "cast_"
' The ball collector and ball delivery teams are assigned "bc_" and "bd_" respectively
' Any variables or constants that are needed for the rangefinder will have the "ra_" prefix
'
'---------- Operator Interface (INP) - Analog Inputs --------------------------
'******Commented out variables are for a two joystick system
inp_joy_1			VAR byte	'1st joystick axis
inp_joy_2			VAR byte	'2nd joystick axis

'---------- 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 - Digital Outputs -------------------------------
relayA			VAR byte
'relayB			VAR byte

'---------- Robot Controller (OUT) - Analog Outputs --------------------------------
out_bc_motorlower		VAR byte	'Lower ball handling motor (pwm 6) Fisherprice
out_bc_motorhigher	VAR byte	'Higher ball handling motor (pwm 8) Chipiua
out_cast			VAR byte	'Output value for the caster Globe motor
out_drive_left		VAR byte	'Left drive motor output value
out_drive_right		VAR byte	'Right drive motor output value
out_servo_left		VAR byte	'Output value for gear shifting servo
out_servo_right		VAR byte	'Output value for gear shifting servo
out_gr_motor		VAR byte	'This part stores what the motor does   (pwm5) Window motor

'---------- Misc. ------------------------------------------------------------
PB_mode	VAR byte		'the mode the program is in. can disable outputs
'packet_num	VAR byte		'the current packet number
'delta_t	VAR byte		'is the time in 40 loops

'========== DEFINE ALIASES ===================================================
'=============================================================================
'  Aliases are variables which are sub-divisions of variables defined
'  above.  Aliases don't require any additional RAM.

'=============================================================================
'---------- 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.

inp_drv_fc		  VAR oi_swA.bit0			'Drive fine control
inp_turbo		  VAR oi_swA.bit1			'TURBO!!!!!
inp_start		  VAR oi_swA.bit2			'Start switch so robot doesn't mangle people
inp_gr_tog_switch  VAR oi_swA.bit3			'Grabber toggle switch
inp_bc_sw          VAR oi_swA.highnib		'Ball handling switches
inp_cast		  VAR oi_swB.bit0			'Caster switch
inp_calibrate	  VAR oi_swB.bit1			'Calibrate button
inp_shift		  VAR oi_swB.bit2			'Shift lever
inp_joy_sw         VAR oi_swB.bit3			'Number of joysticks switchs

inp_caster_pot		VAR	byte			'Analog 2
'---------- Aliases for each RC switch input ---------------------------------
'  Below are aliases for the digital inputs located on the Robot Controller.
inp_gr_sensor_up		VAR rc_swA.bit0	'This part stores if the sensor up is on   
inp_gr_sensor_down	VAR rc_swA.bit4	'This part stores if the sensor down is on 

'---------- Aliases for each RC Relay outputs ---------------------------------
'  Below are aliases for the relay outputs located on the Robot Controller.
out_mars_light		VAR relayA.bit0

'---------- Aliases for the Pbasic Mode Byte (PB_mode) -----------------------
'  The last bit of the PB_mode byte (aliased as comp_mode) indicates the status
'  of the Competition Control, either Enabled or Disabled.  This indicates the
'  starting and stopping of rounds at the competitions.  Comp_mode is the same
'  as the "Disabled" LED on the Operator Interface.
'  Comp_mode = 1 for Enabled, 0 for Disabled.
comp_mode		VAR PB_mode.bit7

'========== DECLARE PROGRAM VARIABLES ========================================
'=============================================================================
'---------- Ball Handling Variables ------------------------------------------
bc_previous_state 	VAR 	nib	'The previous state of the ball handling switch
bc_counterl 		VAR 	nib	'The counter for the lower assembly
bc_counterh 		VAR 	nib	'The counter for the higher assembly

'---------- Drive Variables --------------------------------------------------
drv_n 			VAR	byte
drv_prev_shift 		VAR	bit

'---------- Auto- Calibration ------------------------------------------------
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

'========== DEFINE CONSTANTS =================================================
'=============================================================================
c_joy_x			CON	1
c_io_joy_y			CON	1
c_joy_x_2			CON	0
c_io_joy_y_2		CON	0
c_p2_x			CON	0
c_p2_y			CON	0
c_p4_x			CON	0
c_p4_y			CON	0
c_rc_swB			CON	1
c_rc_swA			CON	1
c_oi_swB			CON	1
c_oi_swA			CON	1
c_sensor1			CON	1	'Caster pot	
c_sensor2			CON	0
c_sensor3			CON	0
c_sensor4			CON	0	'Rangefinder1: previously named inp_rf1
c_sensor5			CON	0	'Rangefinder2: previously named inp_rf2
c_sensor6			CON	0	'Rangefinder3: previously named inp_rf3
c_sensor7			CON	0	'Rangefinder4: previously named inp_rf4
c_p1_wheel			CON	0
c_p2_wheel			CON	0
c_p3_wheel  		CON	0
c_p4_wheel			CON	0
c_p1_aux			CON	0
c_p2_aux 			CON	0
c_p3_aux  			CON	0
c_p4_aux 			CON	0
c_batt_volt			CON	0
c_delta_t  			CON	0
c_res01 			CON	0
c_packet_num		CON	0
c_PB_mode			CON	1

'========== DRIVE CONSTANTS ==================================================
'c_drv_
DRV_STOP_CON			CON	5
DRV_SHIFT_CON			CON	10
DRV_MOTOR_STOP 			CON	127
DRV_JOY_MAX_RANGE 		CON	128
DRV_JOY_MIN_RANGE			CON 	127
DRV_JOY_CENTER 			CON 	127
DRV_SHIFT_HIGH 			CON 	1
DRV_SHIFT_LOW 			CON 	0
DRV_ONE_JOYSTICK 			CON 	0
DRV_TWO_JOYSTICK 			CON 	1
DRV_TURBO_OFF 			CON 	0
DRV_TURBO_ON 			CON 	1
DRV_R_SERVO_HIGH 			CON 	70
DRV_R_SERVO_LOW 			CON 	200
DRV_L_SERVO_HIGH			CON	240
DRV_L_SERVO_LOW			CON	70
DRV_FINE_ON 			CON 	1
DRV_FINE_OFF 			CON 	0
DRV_FINE_CON_MUL 			CON 	1
DRV_FINE_CON_DIV 			CON 	2
DRV_SCAL_MUL 			CON 	4
DRV_SCAL_DIV 			CON 	5
DRV_BUFFER 				CON 	10
DRV_CON_DIFF_FC			CON	DRV_FINE_CON_DIV - DRV_FINE_CON_MUL
DRV_DIV_MUL_CENTER_FC		CON	DRV_CON_DIFF_FC * DRV_JOY_CENTER
DRV_CON_DIFF_SCALE		CON	DRV_SCAL_DIV - DRV_SCAL_MUL
DRV_DIV_MUL_CENTER_SCALE	CON	DRV_CON_DIFF_SCALE * DRV_JOY_CENTER
DRV_SHIFTING_SPEED		CON	15
DRV_JOY_MIN_EXTREME_BUFFER	CON	DRV_JOY_CENTER - DRV_JOY_MIN_RANGE + 20
DRV_JOY_MAX_EXTREME_BUFFER	CON	DRV_JOY_CENTER + DRV_JOY_MAX_RANGE - 20

'========== GRABBER CONSTANTS ================================================
'c_gr_
C_GR_SWITCH_UP	CON	1	'These are the constants that I use to say the difference between the two sensors on the grabber
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

'========== CASTER CONSTANTS =================================================
C_CAST_SWITCH_UP		CON	1	'These are the constants that I use to say the difference between the two switch position on the caster
C_CAST_SWITCH_DOWN	CON	0
C_CAST_REV			CON	250	'Sets the motor to up
C_CAST_FWD			CON	1	'Sets the motor to down
C_CAST_UP			CON	84
C_CAST_DOWN			CON	243
 
'========== BALL COLLECTION & DELIVERY CONSTANTS =============================
'c_bc_
'c_bd_
C_BC_ONFWDL			CON	250
C_BC_ONFWDH			CON	190
C_BC_ONBWD			CON	 30
C_BC_COUNTERLOOP		CON    15
C_BC_HIGHER             CON %1100
C_BC_LOWER              CON %0011
C_BC_HIGHER_REV         CON %0100
C_BC_HIGHER_FWD         CON %1000
C_BC_LOWER_REV          CON %0010
C_BC_LOWER_FWD          CON %0001
C_BC_NEUTRAL            CON %0000

'---------- 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

'========= LED CONSTANTS =====================================================
LED_BASIC_RUN CON 7
LED_RESET	  CON 8
LED_PWM1_RED  CON 9
LED_PWM2_GRN  CON 10
LED_PWM2_RED  CON 11
LED_RLY1_RED  CON 12
LED_RLY1_GRN  CON 13
LED_RLY2_RED  CON 14
LED_RLY2_GRN  CON 15

'========= DEFINE CONSTANTS FOR INITIALIZATION ===============================
'=============================================================================
'  The initialization code is used to select the input data used by PBASIC.
'  The Master micro-processor (uP) sends the data you select to the BS2SX
'  PBASIC uP.  You may select up to 26 constants, corresponding
'  to 26 variables, from the 32 available to you.  Make sure that you have 
'  variables for all the bytes recieved in the serin command.
'
'  The constants below have a "c_" prefix, as compared to the variables that 
'  they will represent.
'
'  Set the Constants below to 1 for each data byte you want to recieve.
'  Set the Constants below to 0 for the unneeded data bytes.

'---------- Set the Initialization constants you want to read ----------------
'---------- Initialization Constant VOLTAGE - USER DEFINED -------------------
'  This is the 'Low Battery' detect voltage.  The 'Low Battery' LED will
'  blink when the voltage drops below this value.
'  Basically set VOLTAGE = INT ((DESIRED FLASH VOLTAGE + 0.4) * 16.3)
'  Example, for a 10 Volt trigger, set Voltage eq 170.

dataInitVolt	CON 153	'9.0 Volts

'========== DEFINE CONSTANTS (DO NOT CHANGE) =================================
'=============================================================================
' Baud rate for communications with User CPU
OUTBAUD		CON	20	' (62500, 8N1, Noninverted)
INBAUD		CON	20	' (62500, 8N1, Noninverted)
USERCPU		CON 	4
FPIN			CON 	1
COMA			CON 	1
COMB			CON 	2
COMC			CON 	3

'========== MAIN PROGRAM =====================================================
'=============================================================================
'---------- Input & Output Declarations --------------------------------------
Output		COMB
Input			COMA
Input			COMC

Output	LED_BASIC_RUN	'define Basic Run LED on RC => out7
Output      LED_RESET		'Define Robot Feedback LED => out8	=> PWM1 Green
Output      LED_PWM1_RED	'define Robot Feedback LED => out9	=> PWM1 Red
Output      LED_PWM2_GRN	'define Robot Feedback LED => out10	=> PWM2 Green
Output      LED_PWM2_RED	'define Robot Feedback LED => out11 => PWM2 Red
Output      LED_RLY1_RED	'define Robot Feedback LED => out12 => Relay1 Red
Output      LED_RLY1_GRN	'define Robot Feedback LED => out13 => Relay1 Green
Output      LED_RLY2_RED	'define Robot Feedback LED => out14 => Relay2 Red
Output      LED_RLY2_GRN	'define Robot Feedback LED => out15 => Relay2 Green

'---------- Initialize Inputs & Outputs --------------------------------------
Out7  = 1		'Basic Run LED on RC
Out8  = 0		'PWM1 LED - Green
Out9  = 0		'PWM1 LED - Red
Out10 = 0		'PWM2 LED - Green
Out11 = 0		'PWM2 LED - Red
Out12 = 0		'Relay1 LED - Red
Out13 = 0		'Relay1 LED - Green
Out14 = 0		'Relay2 LED - Red
Out15 = 0		'Relay2 LED - Green

'========== PBASIC - MASTER uP INITIALIZATION ROUTINE ========================
'=============================================================================
'  DO NOT CHANGE THIS!  DO NOT MOVE THIS!
'  The init routine sends 5 bytes to the Master uP, defining which data bytes to receive.
'  1)  Collect init.
'  2)  Lower the COMA line, which is the clk line for the shift out command.
'  3)  Lower COMB line to tell pic that we are ready to send init data.
'  4)  Wait for pic to lower the COMC line, signaling pic is ready for data.
'  5)  Now send out init dat to pic, all 5 bytes.
'  6)  Now set direction and levels for the COMA and COMB pins.
' The following is some shifting and basically it doesn't get out of wait_init at the bottom until
'  everything is all ready, then it sends all the data out in the correct order.

tempA		CON	c_joy_x_2 <<1 + c_p4_x <<1 + c_joy_x <<1 + c_p2_x <<1 + c_rc_swB
dataInitA	CON	tempA <<1 + c_rc_swA <<1 + c_oi_swB <<1 + c_oi_swA
tempB		CON	c_sensor4 <<1 + c_sensor3 <<1 + c_io_joy_y <<1 + c_p2_y <<1 + c_sensor2
dataInitB	CON	tempB <<1 + c_sensor1 <<1 + c_packet_num <<1 + c_PB_mode
tempC		CON	c_batt_volt <<1 + c_sensor7 <<1 + c_p1_wheel <<1 + c_p2_wheel <<1 + c_sensor6
dataInitC	CON	tempC <<1 + c_sensor5 <<1 + c_io_joy_y_2 <<1 + c_p4_y
tempD		CON	c_res01 <<1 + c_delta_t <<1 + c_p3_aux <<1 + c_p4_aux <<1 + c_p1_aux
dataInitD	CON	tempD <<1 + c_p2_aux  <<1 + c_p3_wheel <<1 + c_p4_wheel

Output	COMA
low		COMA
low		COMB

bc_previous_state = C_BC_NEUTRAL
bc_counterl = C_BC_COUNTERLOOP
bc_counterh = C_BC_COUNTERLOOP
out_mars_light = ON
drv_prev_shift = DRV_SHIFT_HIGH
out_servo_right = DRV_R_SERVO_HIGH
out_servo_left = DRV_L_SERVO_HIGH
drv_n = 0

Wait_init:
	if IN3 = 1 then Wait_init:

Shiftout	COMB,COMA,1, [dataInitA,dataInitB,dataInitC,dataInitD,dataInitVolt]
Input		COMA
high		COMB

'------wait for calibrate button to be pressed
Serout USERCPU, OUTBAUD, [255,255,out_drive_right,relayA,out_drive_left,0,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]

'******the robot needs to wait for the start button to be pressed before the robot actually runs.
wait_check:
	Serin COMA\COMB, INBAUD, [oi_swA,oi_swB,rc_swA,rc_swB,inp_joy_1,PB_mode, inp_caster_pot,inp_joy_2]
'	debug "grab switch ", dec inp_gr_tog_switch ,cr
'	debug " grab sensor up ", dec inp_gr_sensor_up
'	debug " grab sensor down ", dec inp_gr_sensor_down
'	debug "cast switch ", dec inp_cast
'	debug " cast sensor ", dec inp_caster_pot,cr
'	debug "Ball Handling: ", bin4 inp_bc_sw,cr
'	debug "Calibrate: ", dec inp_calibrate,cr
'	debug "J1Max: ", dec joy_1_max,cr
'	debug "J1Min: ", dec joy_1_min,cr
'	debug "J1Rest: ", dec joy_1_rest,cr
'	debug "J2Max: ", dec joy_2_max,cr
'	debug "J2Min: ", dec joy_2_min,cr 
'	debug "J2Rest: ", dec joy_2_rest,cr
'	debug " j sw: ", dec inp_joy_sw
'	debug " j1y: ", dec inp_joy_1
'	debug " j2: ", dec inp_joy_2
'	debug "	fine control switch: ", dec inp_drv_fc ', cr
'	debug "	turbo switch: ", dec inp_turbo, cr
'	debug bin8 oi_swA, " ", bin8 oi_swB, cr
'	debug "counting constant: ", dec drv_n, cr
'	debug "shifting position: ", dec inp_shift, cr
'	debug "start: ", dec inp_start, cr
'	debug "	turbo switch: ", dec inp_turbo, cr
'	debug "Pot: ", dec inp_caster_pot,cr
	if (inp_start = ON) then Pre_State_Master

	toggle LED_RESET
	toggle LED_BASIC_RUN
Serout USERCPU, OUTBAUD, [255,255,MOTOR_STOP,relayA,MOTOR_STOP,0,out_servo_right,out_servo_left,MOTOR_STOP,MOTOR_STOP,MOTOR_STOP,MOTOR_STOP,MOTOR_STOP,MOTOR_STOP,MOTOR_STOP,MOTOR_STOP,MOTOR_STOP,MOTOR_STOP,MOTOR_STOP,MOTOR_STOP]
goto wait_check

'-----------------THE STATE MASTER!!!----------------------------------------

Pre_State_Master:
Out8 = ON
gosub cal_check_joy_read
'debug "1min: ", dec joy_1_min, " 1r: ", dec joy_1_rest, " 1max: ", dec joy_1_max, " 2min: ", dec joy_2_min, " 2r: ", dec joy_2_rest, " 2max: ", dec joy_2_max, cr
'intentionally falls through

State_Master:

'---------- Serin Command - Get Data from Master uP --------------------------
'  Construct the "serin" command using the following rules:
'  1) There must be one variable for every input defined in the "Define Constants for Init" section.
'  2) The order must match the order in the EXAMPLE SERIN COMMAND below.
'  3) The total number of all variables may not exceed 26.
'  4) Only use one "Serin" command.
'  5) The Serin command must occupy one line.
'
'  If you see a BASIC INIT ERR on the Robot Controller after programming and pressing RESET, then
'  there is a problem with the Serin command below.  Check the number of variables.  A BASIC INIT ERR
'  will not occur if you have the variables in the wrong order, however your code will not work correctly.
'
'  EXAMPLE SERIN COMMAND
'  This example exceed the 26 variable limit and is not on one line:
'
'  Serin COMA\COMB, INBAUD, [oi_swA,oi_swB,rc_swA,rc_swB,p2_x,p1_x,p4_x,p3_x,PB_mode,packet_num,sensor1,
'				     sensor2,p2_y,p1_y,sensor3,sensor4,p4_y,p3_y,sensor5,sensor6,p2_wheel,p1_wheel,
'				     sensor7,sensor8,p4_wheel,p3_wheel,p2_aux,p1_aux,p4_aux,p3_aux,delta_t,res01]
' 
  	Serin COMA\COMB, INBAUD, [oi_swA,oi_swB,rc_swA,rc_swB,inp_joy_1,PB_mode,inp_caster_pot, inp_joy_2]
'	debug "Ball Handling: ", bin4 inp_bc_sw
'	debug " sw: ", dec inp_joy_sw, " 1min: ", dec joy_1_min, " 1r: ", dec joy_1_rest, " 1max: ", dec joy_1_max, " 2min: ", dec joy_2_min, " 2r: ", dec joy_2_rest, " 2max: ", dec joy_2_max
'	debug "grab switch ", dec inp_gr_tog_switch
'	debug "cast sensor ",dec inp_caster_pot
'	debug "cast switch ", dec inp_cast
'	debug "	fine control switch: ", dec inp_drv_fc , cr
'	debug "	turbo switch: ", dec inp_turbo, cr
'	debug bin oi_swA, " ", bin oi_swB, cr
'	debug "counting constant: ", dec drv_n, cr
'	debug "shifting position: ", dec inp_shift, cr
'	debug "start: ", dec inp_start, cr
'	debug "1: ", dec inp_joy_1, " 2: ", dec inp_joy_2
	if (inp_calibrate = ON) then state_master_skip_calibrate
'	debug " calibrating"
	gosub calibrate_main
	goto State_Master

state_master_skip_calibrate:
	gosub drv_main
	gosub ball_handling_main
	gosub grabber_main
	gosub	caster_main

'---------- Blink BASIC RUN LED ----------------------------------------------
	Toggle LED_BASIC_RUN			'Basic Run LED on the RC is toggled ON/OFF every loop.

'	debug "caster motor", dec out_cast,cr
'	debug "grabber motor ", dec out_gr_motor,cr
'	debug "servo position right: ", dec out_servo_right, "	servo position left: ", dec out_servo_left
'	debug " dr: ", dec out_drive_right, " dl: ", dec out_drive_left,cr
'	debug " n: ", dec drv_n, cr
'	debug "Ball Handling: ", bin inp_bc_sw
'	debug " LC: ", dec bc_counterl, " HC: ", dec bc_counterh
'	debug " LB: ", dec out_bc_motorlower, " HB: ", dec out_bc_motorhigher,cr
'	debug "J1Min: ", dec joy_1_min,cr' " J1Max: ", dec joy_1_max, " J1Rest: ", dec joy_1_rest
'	debug "J2Min: ", dec joy_2_min,cr
'	debug "J2Max: ", dec joy_2_max,cr
'	debug "J2Rest: ", dec joy_2_rest

'========== OUTPUT DATA ======================================================
'=============================================================================
'  The Serout line sends data to the Output uP.  The Output uP passes this to each PWM 1-16
'  and Relay 1-8.  The Output uP will not output data if there is no communication with the
'  Operator Interface or if the Competition Mode is Disabled.  Do not delete any elements
'  from the Serout array.  Set unused PWM outputs to 127.  Set unused relay outputs to 0.
'
'	Serout USERCPU, OUTBAUD, [255,255, (PWM1), relayA,(PWM2), relayB, (PWM3), (PWM4), (PWM5), (PWM6), (PWM7), (PWM8), (PWM9), (PWM10), (PWM11), (PWM12), (PWM13), (PWM14), (PWM15), (PWM16)   ]
'	SEROUT USERCPU, OUTBAUD, [255,255, out_drive_right, relayA, out_drive_left, 0, out_servo_right, out_servo_left, out_gr_motor, out_bc_motorlower, out_cast, out_bc_motorhigher, MOTOR_STOP,MOTOR_STOP,MOTOR_STOP,MOTOR_STOP,MOTOR_STOP,MOTOR_STOP,MOTOR_STOP,MOTOR_STOP]
	SEROUT USERCPU, OUTBAUD, [255,255, out_drive_right, relayA, out_drive_left, 0, out_servo_right, out_servo_left, out_gr_motor, out_bc_motorlower, out_cast, out_bc_motorhigher, MOTOR_STOP,MOTOR_STOP,MOTOR_STOP,out_bc_motorhigher,MOTOR_STOP,MOTOR_STOP,MOTOR_STOP,out_bc_motorlower]

Goto State_Master

'========== PERFORM OPERATIONS ===============================================
'=============================================================================

'====================== DRIVE OPERATIONS =====================================
'=============================================================================

'----------gear shift----------
'inorder to shift you must make sure the motors are stopped through a loop,
'start shift either high or low, move motors slowly to engage it
'if not have to shift and is stop then allows drive

drv_main:
	if inp_shift = drv_prev_shift then drv_drive_allow_determine
	if drv_n >= DRV_STOP_CON then drv_start_shift
	out_drive_right = DRV_MOTOR_STOP
	out_drive_left = DRV_MOTOR_STOP
	drv_n	= drv_n + 1
	return

drv_drive_allow_determine:
	if drv_n = 0 then drv_allow_drive
	drv_prev_shift = ~inp_shift
	out_drive_right = DRV_MOTOR_STOP
	out_drive_left = DRV_MOTOR_STOP
	drv_n = 1
	return

drv_start_shift:
	if inp_shift = DRV_SHIFT_HIGH then drv_set_servo_high
	out_servo_right = DRV_R_SERVO_LOW
	out_servo_left = DRV_L_SERVO_LOW

'intentionally falls through

drv_start_shift_back:
	out_drive_left = DRV_MOTOR_STOP + DRV_SHIFTING_SPEED
	out_drive_right = DRV_MOTOR_STOP - DRV_SHIFTING_SPEED
	drv_n = drv_n + 1
	if drv_n = DRV_SHIFT_CON then drv_shift_done
	return

drv_set_servo_high:
	out_servo_right = DRV_R_SERVO_HIGH
	out_servo_left = DRV_L_SERVO_HIGH
	goto drv_start_shift_back

drv_shift_done:
	drv_prev_shift = inp_shift
	drv_n = 0
	return

'---------------set drive outputs------------
'sets drive outputs then determines if one or two joysticks
'calibrates axis_1 and axis_2

drv_allow_drive:
'-------------axis 1 calibration--------------
	if inp_joy_1 < (joy_1_rest - DRV_BUFFER) or inp_joy_1 > (joy_1_rest + DRV_BUFFER) then drv_axis_1_calib_det
	inp_joy_1 = DRV_JOY_CENTER
	goto drv_axis_2_calib

drv_axis_1_calib_det:
	if (inp_joy_1 < joy_1_rest) then drv_axis_1_calib_lower
	inp_joy_1 = ((2000 + DRV_JOY_CENTER + ((inp_joy_1 - (joy_1_rest + DRV_BUFFER)) * DRV_JOY_MAX_RANGE / (joy_1_max - (joy_1_rest + DRV_BUFFER)))) min 2127 max 2255) - 2000
	goto drv_axis_2_calib

drv_axis_1_calib_lower:
	inp_joy_1 = ((2000 + DRV_JOY_CENTER - (((joy_1_rest - DRV_BUFFER) - inp_joy_1) * DRV_JOY_MIN_RANGE / ((joy_1_rest - DRV_BUFFER) - joy_1_min))) min 2000 max 2127) - 2000

'intentionally falls through

'---------------axis 2 calibration--------------
drv_axis_2_calib:
	if inp_joy_2 < (joy_2_rest - DRV_BUFFER) or inp_joy_2 > (joy_2_rest + DRV_BUFFER) then drv_axis_2_calib_det
	inp_joy_2 = DRV_JOY_CENTER
	goto drv_fine_cont

drv_axis_2_calib_det:
	if (inp_joy_2 > joy_2_rest) then drv_axis_2_calib_above
	inp_joy_2 = ((2000 + DRV_JOY_CENTER - (((joy_2_rest - DRV_BUFFER) - inp_joy_2) * DRV_JOY_MIN_RANGE / ((joy_2_rest - DRV_BUFFER) - joy_2_min))) min 2000 max 2127) - 2000
	goto drv_fine_cont

drv_axis_2_calib_above:
	inp_joy_2 = ((2000 + DRV_JOY_CENTER + ((inp_joy_2 - (joy_2_rest + DRV_BUFFER)) * DRV_JOY_MAX_RANGE / (joy_2_max - (joy_2_rest + DRV_BUFFER)))) min 2127 max 2255) - 2000

'intentionally falls through
'after calibrates goes to determine if finecontrol switch is on
'if yes then sets motors to 80%, if not then goes set drive output

drv_fine_cont:
	if (inp_drv_fc = DRV_FINE_ON) then drv_fine_cont_set
	goto drv_drive_output_set

drv_fine_cont_set:
	inp_joy_1 = ((2000 + ((DRV_DIV_MUL_CENTER_FC + (DRV_FINE_CON_MUL * inp_joy_1)) / DRV_FINE_CON_DIV)) min 2000 max 2255) - 2000
	inp_joy_2 = ((2000 + ((DRV_DIV_MUL_CENTER_FC + (DRV_FINE_CON_MUL * inp_joy_2)) / DRV_FINE_CON_DIV)) min 2000 max 2255) - 2000
'intentionally falls through

'---------set drive outputs--------
drv_drive_output_set:
	if (inp_joy_sw = DRV_ONE_JOYSTICK)	then drv_one_joy_set
	out_drive_right = inp_joy_1
	out_drive_left = inp_joy_2

	'these two lines of code check if the joysticks are both at extremes and if they are it sets the variables so the 
	'caster will dpeloy during its sub routine
	if inp_joy_1 > DRV_JOY_MAX_EXTREME_BUFFER and inp_joy_2 < DRV_JOY_MIN_EXTREME_BUFFER then drv_set_caster
	if inp_joy_2 > DRV_JOY_MAX_EXTREME_BUFFER and inp_joy_1 < DRV_JOY_MIN_EXTREME_BUFFER then drv_set_caster
	goto drv_turbo_det

'sets tic tac to board
drv_one_joy_set:
	out_drive_left = ((2000 + inp_joy_1 - inp_joy_2 + DRV_MOTOR_STOP) min 2000 max 2255) - 2000
	out_drive_right = ((2000 + inp_joy_1 + inp_joy_2 - DRV_MOTOR_STOP) min 2000 max 2255) - 2000

	'this next line checks if the x values of the joystick is within a buffer of the extreme and if the y value is within 
	'the buffer of the center and if both of these are, it sets the variables so the caster will deploy during its sub 
	'routine

	if ((inp_joy_2 > DRV_JOY_MAX_EXTREME_BUFFER) or (inp_joy_2 < DRV_JOY_MIN_EXTREME_BUFFER)) and (inp_joy_1 < (joy_1_rest + DRV_BUFFER)) and (inp_joy_1 > (joy_1_rest - DRV_BUFFER)) then drv_set_caster 
	goto drv_turbo_det

drv_set_caster:
	inp_cast = C_CAST_SWITCH_DOWN

'intentionally falls through

'----------set turbo output----------
'determines if turbo switch is on, if yes then goes to reverse motors
drv_turbo_det:
'debug "b4->l: ", dec out_drive_left, " r: ", dec out_drive_right
	if inp_turbo = DRV_TURBO_ON then drv_drive_r_reverse
	out_drive_left = ((2000 + ((DRV_DIV_MUL_CENTER_SCALE + (DRV_SCAL_MUL * out_drive_left)) / DRV_SCAL_DIV)) min 2000 max 2255) - 2000
	out_drive_right = ((2000 + ((DRV_DIV_MUL_CENTER_SCALE + (DRV_SCAL_MUL * out_drive_right)) / DRV_SCAL_DIV)) min 2000 max 2255) - 2000
'intentionally falls through

'---------reverse right motor------------
'reverses right motor, so does direction actually wanted to go in
drv_drive_r_reverse:
	out_drive_right = ((2000 + (2 * DRV_MOTOR_STOP) - out_drive_right) min 2000 max 2255) - 2000
'debug "after->l: ", dec out_drive_left, " r: ", dec out_drive_right,cr
	return

'====================== GRABBER OPERATIONS ===================================
'=============================================================================
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						'This part stops the motor
	return									'This returns to the Main program.

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

'This program makes the Reader cry.  The good kind of crying.
'===================== CASTER OPERATIONS =====================================
'=============================================================================
caster_main:
	if (inp_cast = C_CAST_SWITCH_UP) then caster_up	'This part checks if the switch is up or down
	if (inp_caster_pot < C_CAST_DOWN) then caster_move_down		'This part checks if sensor down senses anything
	out_cast = MOTOR_STOP						'This part stops the motor
	return									'This returns to the Main program. 
 
caster_up:
	if (inp_caster_pot > C_CAST_UP) then caster_move_up		'This part checks if sensor up senses anything
	out_cast = MOTOR_STOP
	return

caster_move_up:
	out_cast = C_CAST_REV		'This part runs the motor reverse
	return

caster_move_down:
	out_cast = C_CAST_FWD		'This part runs the motor reverse
	return

'This program makes the Reader cry.  The good kind of crying (Times 2).
'=============== BALL COLLECTION & DELIVERY OPERATIONS =======================
'=============================================================================
'This is the starting of the ball handling system.
'There are two three stage switches, one for the lower assembly and one for the higher assembly
'Up is forward, neutral is self-explanatory and down is backwards

ball_handling_main:
	gosub bc_lower_assembly			'Go to the subroutine bc_lower_assembly
	gosub bc_upper_assembly			'Go to the subroutine bc_higher_assembly
'	debug " C: ", dec bc_counterl, " H: ", dec bc_counterh
	return					'return to the statemaster

bc_lower_assembly:
	if ((bc_previous_state & C_BC_LOWER) = (inp_bc_sw & C_BC_LOWER)) OR ((bc_previous_state & C_BC_LOWER) = C_BC_NEUTRAL) then bc_lower_cont_dir
	if (bc_counterl > 0) then bc_counterl_dec				'If the counter is greater than zero then go to the counter decrementer
'intentionally falls through


bc_lower_cont_dir:
      bc_counterl = C_BC_COUNTERLOOP
      bc_previous_state = (bc_previous_state & C_BC_HIGHER) | (inp_bc_sw & C_BC_LOWER)
	if (inp_bc_sw & C_BC_LOWER_FWD) = C_BC_LOWER_FWD then bc_motorlower_fwd	'If the position of the switch is the same as last time then check the check the motor statues
	if (inp_bc_sw & C_BC_LOWER_REV) = C_BC_LOWER_REV then bc_motorlower_bwd
	goto bc_motorlower_neutral

bc_counterl_dec:
	bc_counterl = bc_counterl - 1							'Decrements the counter by one
'intentionally falls through

bc_motorlower_neutral:
	out_bc_motorlower = MOTOR_STOP							'Set the motor to stop
	return										'Return to bc_lower_assembly

bc_motorlower_fwd:
	out_bc_motorlower = C_BC_ONFWDL							'Set the motor to forward
	return										'Return to bc_lower_assembly

bc_motorlower_bwd:
	out_bc_motorlower = C_BC_ONBWD							'Set the motor to backwards
	return										'Return to bc_lower_assembly

bc_upper_assembly:
	if ((bc_previous_state & C_BC_HIGHER) = (inp_bc_sw & C_BC_HIGHER)) OR ((bc_previous_state & C_BC_HIGHER) = C_BC_NEUTRAL) then bc_higher_cont_dir
	if (bc_counterh > 0) then bc_counterh_dec				'If the counter is greater than zero then go to the counter decrementer
'intentionally falls through

bc_higher_cont_dir:
      bc_previous_state = (bc_previous_state & C_BC_LOWER) | (inp_bc_sw & C_BC_HIGHER)
      bc_counterh = C_BC_COUNTERLOOP
'debug "sw ", bin4 inp_bc_sw
	if (inp_bc_sw & C_BC_HIGHER_FWD) = C_BC_HIGHER_FWD then bc_motorhigher_fwd	'If the position of the switch is the same as last time then check the check the motor statues
	if (inp_bc_sw & C_BC_HIGHER_REV) = C_BC_HIGHER_REV then bc_motorhigher_bwd
	goto bc_motorhigher_neutral

bc_counterh_dec:
	bc_counterh = bc_counterh - 1							'Decrements the counter by one
'intentionally falls through

bc_motorhigher_neutral:
	out_bc_motorhigher = MOTOR_STOP							'Set the motor to stop
	return										'Return to bc_lower_assembly

bc_motorhigher_fwd:
	out_bc_motorhigher = C_BC_ONFWDH							'Set the motor to forward
	return										'Return to bc_lower_assembly

bc_motorhigher_bwd:
	out_bc_motorhigher = C_BC_ONBWD							'Set the motor to backwards
	return	
'============== AUTO-CALIBRATION ======================
'For further reference Joy 1 is either the y-axis on the one joystick and y-axis if it is two joysticks
'

calibrate_main:
	joy_1_min	= inp_joy_1						'Reset the motors to joy input for calibrating
	joy_1_max	= inp_joy_1
	joy_2_min	= inp_joy_2
	joy_2_max	= inp_joy_2
	joy_1_rest	= inp_joy_1
	joy_2_rest	= inp_joy_2
'intentional drop through

cal_check_values:
'      debug "1min: ", dec joy_1_min, " 1r: ", dec joy_1_rest, " 1max: ", dec joy_1_max, " 2min: ", dec joy_2_min, " 2r: ", dec joy_2_rest, " 2max: ", dec joy_2_max, cr

toggle LED_RESET
toggle LED_BASIC_RUN
Serout USERCPU, OUTBAUD, [255,255,MOTOR_STOP,relayA,MOTOR_STOP,0,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 (inp_joy_1 > joy_1_max) then set_1_max
	if (inp_joy_1 > joy_1_min) then calibrate_2nd_axis

	joy_1_min = inp_joy_1
	goto calibrate_2nd_axis
	
set_1_max:
	joy_1_max = inp_joy_1
'intentional drop through

calibrate_2nd_axis:
	if (inp_joy_2 > joy_2_max) then set_2_max
	if (inp_joy_2 > joy_2_min) then calibrate_finish

	joy_2_min = inp_joy_2
	goto calibrate_finish

set_2_max:
	joy_2_max = inp_joy_2
'intentional drop through

calibrate_finish:
	Serin COMA\COMB, INBAUD, [oi_swA,oi_swB,rc_swA,rc_swB,inp_joy_1,PB_mode,inp_caster_pot,inp_joy_2]
	if (inp_calibrate = OFF) then cal_check_values
	if (inp_joy_sw = DRV_TWO_JOYSTICK) then cal_write_joy_2
'	debug " CalibW  :   " , cr
	WRITE  0, joy_1_min
	WRITE  1, joy_1_max
	WRITE  2, joy_1_rest
	WRITE  3, joy_2_min
	WRITE  4, joy_2_max
	WRITE  5, joy_2_rest
	return

cal_check_joy_read:
'	debug " CalibR1  :   " , cr
	if (inp_joy_sw = DRV_TWO_JOYSTICK) then cal_read_joy_2
	READ	0, joy_1_min
	READ  1, joy_1_max
	READ  2, joy_1_rest
	READ  3, joy_2_min
	READ  4, joy_2_max
	READ  5, joy_2_rest
	return

cal_write_joy_2:
	WRITE  0, joy_1_min
	WRITE  1, joy_1_max
	WRITE  2, joy_1_rest
	WRITE  6, joy_2_min
	WRITE  7, joy_2_max
	WRITE  8, joy_2_rest
	return

cal_read_joy_2:
	READ	0, joy_1_min
	READ  1, joy_1_max
	READ  2, joy_1_rest
	READ  6, joy_2_min
	READ  7, joy_2_max
	READ  8, joy_2_rest
	return