' PROGRAM:    Full-Size RC User 2003  (1 Joystick Drive)
' Written by: Innovation First, Inc.
' Date:       2002 Nov 26
'
' Define BS2-SX Project Files
'
' {$STAMP BS2SX}


'=============================================================================================================
'========== DECLARE VARIABLES ================================================================================
'=============================================================================================================
'  Below is a list of declared input and output variables.  Comment or un-comment
'  the variables as needed.  Declare any additional variables required in
'  your main program loop.  Note that you may only use 26 total variables.


'---------- Operator Interface (OI) - Analog Inputs ----------------------------------------------------------
'-------------------------------------------------------------------------------------------------------------
p1_x		VAR byte	'Port 1, X-axis on Joystick
'p2_x		VAR byte	'Port 2, X-axis on Joystick
p3_x		VAR byte	'Port 3, X-axis on Joystick
'p4_x		VAR byte	'Port 4, X-axis on Joystick

p1_y		VAR byte	'Port 1, Y-axis on Joystick
'p2_y		VAR byte	'Port 2, Y-axis on Joystick
p3_y		VAR byte	'Port 3, Y-axis on Joystick
'p4_y		VAR byte	'Port 4, Y-axis on Joystick

'p1_wheel	VAR byte	'Port 1, Wheel on Joystick
'p2_wheel	VAR byte	'Port 2, Wheel on Joystick
'p3_wheel	VAR byte	'Port 3, Wheel on Joystick
'p4_wheel	VAR byte	'Port 4, Wheel on Joystick

'p1_aux		VAR byte	'Port 1, Aux on Joystick
'p2_aux		VAR byte	'Port 2, Aux on Joystick
'p3_aux		VAR byte	'Port 3, Aux on Joystick
'p4_aux		VAR byte	'Port 4, Aux on Joystick



'---------- 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 (RC) - Analog Inputs ------------------------------------------------------------
'-------------------------------------------------------------------------------------------------------------
'sensor1	VAR byte	'RC Analog Input 1, connector pin 2
'sensor2	VAR byte	'RC Analog Input 2, connector pin 16
'sensor3	VAR byte	'RC Analog Input 3, connector pin 5
'sensor4	VAR byte	'RC Analog Input 4, connector pin 19
'sensor5	VAR byte	'RC Analog Input 5, connector pin 8
'sensor6	VAR byte	'RC Analog Input 6, connector pin 22
'sensor7	VAR byte	'RC Analog Input 7, connector pin 11
'bat_volt	VAR byte	'RC Analog Input 8, hardwired to the Battery
				'Vin = ((4.7/14.7)* Battery voltage)-0.4
				'Binary Battery Voltage = (Vin/5.0 V)*255


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


'---------- Misc. --------------------------------------------------------------------------------------------
'-------------------------------------------------------------------------------------------------------------
packet_num	VAR byte
'delta_t	VAR byte
PB_mode		VAR byte
'drive_R		Var byte	'Drive Right
'drive_L		Var byte	'Drive Left

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

p1_sw_trig	VAR oi_swA.bit0	'Joystick Trigger Button, same as Port4 pin5 
p1_sw_top	VAR oi_swA.bit1	'Joystick Top Button,     same as Port4 pin8
p1_sw_aux1	VAR oi_swA.bit2	'Aux input,               same as Port4 pin9
p1_sw_aux2	VAR oi_swA.bit3	'Aux input,               same as Port4 pin15

p3_sw_trig	VAR oi_swA.bit4	'Joystick Trigger Button, same as Port2 pin5 
p3_sw_top	VAR oi_swA.bit5	'Joystick Top Button,     same as Port2 pin8
p3_sw_aux1	VAR oi_swA.bit6	'Aux input,               same as Port2 pin9
p3_sw_aux2	VAR oi_swA.bit7	'Aux input,               same as Port2 pin15

'p2_sw_trig	VAR oi_swB.bit0	'Joystick Trigger Button
'p2_sw_top	VAR oi_swB.bit1	'Joystick Top Button
'p2_sw_aux1	VAR oi_swB.bit2	'Aux input
'p2_sw_aux2	VAR oi_swB.bit3	'Aux input

'p4_sw_trig	VAR oi_swB.bit4	'Joystick Trigger Button
'p4_sw_top	VAR oi_swB.bit5	'Joystick Top Button
'p4_sw_aux1	VAR oi_swB.bit6	'Aux input
'p4_sw_aux2	VAR oi_swB.bit7	'Aux input


'---------- Aliases for each RC switch input -----------------------------------------------------------------
'-------------------------------------------------------------------------------------------------------------
'  Below are aliases for the digital inputs located on the Robot Controller.

rc_sw1		VAR rc_swA.bit0
rc_sw2		VAR rc_swA.bit1
rc_sw3		VAR rc_swA.bit2
rc_sw4		VAR rc_swA.bit3
rc_sw5		VAR rc_swA.bit4
rc_sw6		VAR rc_swA.bit5
rc_sw7		VAR rc_swA.bit6
rc_sw8		VAR rc_swA.bit7
rc_sw9		VAR rc_swB.bit0
rc_sw10		VAR rc_swB.bit1
rc_sw11		VAR rc_swB.bit2
rc_sw12		VAR rc_swB.bit3
rc_sw13		VAR rc_swB.bit4
rc_sw14		VAR rc_swB.bit5
rc_sw15		VAR rc_swB.bit6
rc_sw16		VAR rc_swB.bit7


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

relay1_fwd	VAR RelayA.bit0
relay1_rev	VAR RelayA.bit1
relay2_fwd	VAR RelayA.bit2
relay2_rev	VAR RelayA.bit3
relay3_fwd	VAR RelayA.bit4
relay3_rev	VAR RelayA.bit5
relay4_fwd	VAR RelayA.bit6
relay4_rev	VAR RelayA.bit7

relay5_fwd	VAR RelayB.bit0
relay5_rev	VAR RelayB.bit1
relay6_fwd	VAR RelayB.bit2
relay6_rev	VAR RelayB.bit3
relay7_fwd	VAR RelayB.bit4
relay7_rev	VAR RelayB.bit5
relay8_fwd	VAR RelayB.bit6
relay8_rev	VAR RelayB.bit7


'---------- Aliases for the Pbasic Mode Byte (PB_mode) -------------------------------------------------------
'-------------------------------------------------------------------------------------------------------------
'  Bit 7 of the PB_mode byte (aliased as comp_mode below) 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 indicated by a solid "Disabled" LED on the Operator Interface.
'  Comp_mode = 1 for Enabled, 0 for Disabled.
'
'  Bit 6 of the PB_mode byte (aliased as auton_mode below) indicates the status
'  of the Autonomous Mode, either Autonomous or Normal.  This indicates when
'  the robot must run on its own programming.  When in Autonomous Mode, all
'  OI analog inputs are set to 127 and all OI switch inputs are set to 0 (zero).
'  Auton_mode is indicated by a blinking "Disabled" LED on the Operator Interface.
'  Auton_mode = 1 for Autonomous, 0 for Normal.
'
'  Autonomous Mode can be turned ON by setting the RC to Team 0 (zero).
'
'  Bit 5 of the PB_mode byte (aliased as user_display_mode below) indicates when
'  the user selects the "User Mode" on the OI.  PB_mode.bit5 is set to 1 in "User Mode".
'  When the user selects channel, team number, or voltage, PB_mode.bit5 is set to 0
'  When in "User Mode", the eight Robot Feedback LED are turned OFF.
'  Note: "User Mode" is identified by the letter u in the left digit (for 4 digit OI's)
'  Note: "User Mode" is identified by decimal places on the right two digits (for 3 digit OI's)

comp_mode		VAR PB_mode.bit7
auton_mode		VAR PB_mode.bit6
user_display_mode	VAR PB_mode.bit5 


'=============================================================================================================
'========= 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 ------------------------------------------------
'-------------------------------------------------------------------------------------------------------------
c_p1_y				CON	1
c_p2_y				CON	1	 
c_p3_y				CON	1
c_p4_y				CON	1	 

c_p1_x				CON	1
c_p2_x				CON	1	 
c_p3_x				CON	1
c_p4_x				CON	1	 

c_p1_wheel			CON	1
c_p2_wheel			CON	1	 
c_p3_wheel			CON	1
c_p4_wheel			CON	1	 

c_p1_aux			CON	0	 
c_p2_aux			CON	0	 
c_p3_aux			CON	0	 
c_p4_aux			CON	0	 

c_oi_swA			CON	1
c_oi_swB			CON	1

c_sensor1			CON	0
c_sensor2			CON	0
c_sensor3			CON	0
c_sensor4			CON	0
c_sensor5			CON	0
c_sensor6			CON	0
c_sensor7			CON	0
c_batt_volt			CON	0

c_rc_swA			CON	1
c_rc_swB			CON	1

c_delta_t			CON	0
c_PB_mode			CON	1
c_packet_num			CON	1
c_res01				CON	0

'---------- 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, the value = ((DESIRED FLASH VOLTAGE * 16.46) - 8.35)
'  Example, for a 6.5 Volt Flash trigger, set value = 99.

dataInitVolt			CON 	140


'=============================================================================================================
'========== 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		7	'define Basic Run LED on RC => out7

Output      	8	'define Robot Feedback LED => out8  => PWM1 Green
Output      	9	'define Robot Feedback LED => out9  => PWM1 Red
Output      	10	'define Robot Feedback LED => out10 => PWM2 Green
Output      	11	'define Robot Feedback LED => out11 => PWM2 Red
Output      	12	'define Robot Feedback LED => out12 => Relay1 Red
Output      	13	'define Robot Feedback LED => out13 => Relay1 Green
Output      	14	'define Robot Feedback LED => out14 => Relay2 Red
Output      	15	'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.

tempA		CON	c_p3_x <<1 + c_p4_x <<1 + c_p1_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_p1_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_p3_y <<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
Input		COMC

Wait_init:	if IN3 = 1 then Wait_init:
Shiftout	COMB,COMA,1, [dataInitA,dataInitB,dataInitC,dataInitD,dataInitVolt]
Input		COMA
high		COMB

Output		COMC
low		COMC

'=============================================================================================================
'========== MAIN LOOP ========================================================================================
'=============================================================================================================
Left VAR Byte
Right VAR Byte
Fork VAR byte
Temp VAR byte
LastLeft VAR byte ' the last value of the left motor.  To be used to fix that fucking error in auto. mode
LastRight VAR byte ' the last value of the right motor.  To be used to fix that fucking error in auto. mode
Countz VAR byte

CompressorCount VAR byte ' a variable to hold various bits that tell various things modes

TowerUp VAR byte

ChangeDirLeft VAR byte ' how fast the servos are changing their direction

Dir VAR byte
Dirz VAR byte

HuntMode VAR byte ' this tells the CPU what mode of hunting it is currently engaged in
changedirleft = 1
dir = 128
dirz = 5
Left = 127
Right = 127
Fork = 127

MainLoop:


'---------- 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,Left,p1_x,Left,p3_x,PB_mode,packet_num,Left,p1_y,Left,p3_y,Left,left,Left,left]


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



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







if auton_mode = 1 then skipDrive:
	'Right = (((2000 + p1_y + ((p1_x * 2 + 127)/3) - 127) Min 2000 Max 2254) - 2000)
	'Left = (((2000 + p1_y - ((p1_x * 2 + 127)/3) + 127) Min 2000 Max 2254) - 2000)
	Right = (((2000 + p1_y + (p1_x) - 127) Min 2000 Max 2254) - 2000)
	Left = (((2000 + p1_y - (p1_x) + 127) Min 2000 Max 2254) - 2000)

	

	if Left < 85 then lAB:
	if Left > 164 then lCD:
	if Left >= 85 then lBC1:
'              /
'             /
'          __/
'       __/       ' this is a graph of initial motor power (x-axis) versus adjusted motor power
'    __/          ' (y-axis).  The motors are adjusted to have less power when the input is 
'   /		  ' smaller, thus allowing more control at low speeds, and more speed at high
'  /		  ' speeds, without reducing overall speed.  The equations for the 3 lines that
' /		  ' make up the graph are, from left to right: (y = 10x/9), (y = 54x/74 + 34),
		  ' and (y = 10x/9 - 28), where y is adjusted power and x is initial power.


	goto lweAreDone:

	lBC1: ' part one of the middle, shallow line if statement
		if Left <= 164 then lBC:
	goto lWeAreDone:

	lAB: ' steep line
		Left = (115 * Left) / 85
	goto lWeAreDone:

	lBC: ' calculates the shallow, middle line
		Left = ((23 * Left) / 79) + 90
	goto lWeAreDone:

	lCD: ' calculates the steep, high line
		Left = ((116 * Left) / 90) - 73
	goto lWeAreDone:

lWeAreDone:
	if Right < 90 then rAB:
	if Right > 164 then rCD:
	if Right >= 90 then rBC1:

	goto rweAreDone:

	rBC1: ' part one of the middle, shallow line if statement
		if Right <= 164 then rBC:
	goto rWeAreDone:

	rAB: ' steep line
		Right = ((115 * Right) / 85) + 0
	goto rWeAreDone:

	rBC: ' calculates the shallow, middle line
		Right = ((23 * Right) / 79) + 90
	goto rWeAreDone:

	rCD: ' calculates the steep, high line
		Right = ((116 * Right) / 90) - 73
	goto rWeAreDone:
 	
rWeAreDone:
	
	

	relay1_fwd = 0
	relay1_rev = 0
	if p3_sw_trig = 0 then skipDrive1:
		Relay1_fwd = 1
		Relay1_rev = 0
		
	skipDrive1:
	if p3_sw_top = 0 then skipDrive2:
		Relay1_fwd=  0
		Relay1_rev = 1
	skipDrive2:

	fork = p3_y
	
	if countz > 10 then CheckTrigButton:
	goto SkipTowerToggle:
	CheckTrigButton:

	if p1_sw_trig = 1 then TowerToggle: ' if the trigger is pressed, then begin the toggling process
		goto SkipTowerToggle: ' if the button is not pressed, skip all this

		TowerToggle:
		if TowerUp = 1 then TowerDown: ' if the tower is already raised, goto towerdown
		goto TowerUpz: ' if that if statement is not fulfilled, go to tower up

			TowerUpz:
				countz = 0
				TowerUp = 1
				relay2_fwd = 0
				relay2_rev = 1
			goto SkipTowerToggle:
		
			TowerDown:
				
				countz = 0
				TowerUp = 0
				relay2_fwd = 1
				relay2_rev = 0
			goto SkipTowerToggle:



		goto SkipTowerToggle:
		
	SkipTowerToggle:


	
	
	if towerup = 0 then SkipCounter1:
		if Countz > 100 then SkipCountIncrement:
			Countz = Countz + 1
		SkipCountIncrement:

		if countz < 200 then SkipNeutralize:
			Relay2_fwd = 0
			relay2_rev = 0
		SkipNeutralize:
	SkipCounter1:

	if towerup = 1 then SkipCounter2:
		if Countz > 100 then SkipCountIncrement1:
			countz = countz + 1
		SkipCountIncrement1:

		if countz < 15 then SkipNeutralizez:
			relay2_fwd = 0
			relay2_rev = 0
		SkipNeutralizez:
	SkipCounter2:
	compressorcount = compressorcount + 1

	if huntmode > 10 then SkipHuntAdd:
		Huntmode = huntmode + 1
	SkipHuntAdd:

	if compressorcount = 160 then TurnCompressorOn:
	if compressorcount = 0 then turncompressoroff:

	

	
	if p1_sw_top = 0 then SkipCompressorFuncs:
	if huntmode < 10 then SkipCompressorFuncs:
		if relay3_fwd = 1 then TurnCompressorOff:
		if relay3_fwd = 0 then TurnCompressorOn:

		TurnCompressorOff:
			huntmode = 0
			relay3_fwd = 0
		goto SkipCompressorFuncs:
	
		TurnCompressorOn:
			huntmode = 0
			relay3_fwd = 1
		goto SkipCompressorFuncs:

	SkipCompressorFuncs:
skipDrive:







' 1 = swivelling
' 2 = locking


auton_modez VAR byte
debug cls

if auton_mode = 0 then skipAuton:
	debug "In Auto Mode",cr
	if huntmode > 1 then SkipSwivel:

		if changedirleft = 1 then SwingLeft:
		if ChangeDirLeft = 0 then SwingRight:
		goto SkipSwivel:

		SwingLeft:
			dir = dir + 6
			dirz = dirz + 6
		goto EndSwing:

		SwingRight:
			dir = dir - 6
			dirz = dirz - 6
		goto EndSwing:

		EndSwing:

		if dir > 245 then ChangeDirToRight:
		if dir < 130 then ChangeDirToLeft:
		goto EndSwitch:
		

		ChangeDirToRight:
			changedirleft = 0
			dir = 249
			dirz = 126
		goto Endswitch:

		ChangeDirToLeft:
			changedirleft = 1
			dir = 131
			dirz = 7		
		goto EndSwitch:

		EndSwitch:

		Left = 200
		Right = 170
	SkipSwivel:
	
	if rc_sw6 = 0 then SkipHunt: ' the Left Sensor gets activated
		debug "I'm in rc_sw6"

		dirz = (127 +127 + dir) / 3
		huntmode = 2
		
		if dir < 128 then TurnLeft:
		if dir > 127 then TurnRight:
		goto SkipHunt:

		TurnLeft:
			right = 200 - (127 - dir)
			left = 200
			lastleft = left
			lastright = right
			debug "in rc_sw6 - Turning Left",cr
		goto SkipHunt:

		TurnRight:
			left = 200 - (dir - 127)
			right = 200
			lastleft = left
			lastright = right
			debug "in rc_sw6 - turning right",cr
		goto SkipHunt:
	SkipHunt:

	if rc_sw1  = 0 then SkipRightHunt: ' the Right Sensor gets activated

	

		dir = (128 +128 + dirz) / 3
		huntmode = 2

		if dirz > 127 then TurnRightz:
		if dirz < 128 then TurnLeftz:
		goto SkipRightHunt:

		TurnLeftz:
			right = 200 - (127 - dirz)
			left = 200
			lastleft = left
			lastright = right
	
		goto SkipRightHunt:

		TurnRightz:
			right = 200
			left = 200 - (dirz - 127)
			lastleft = left
			lastright = right
	
		goto SkipRightHunt:

		
	SkipRightHunt:

	if huntmode < 2 then SkipAdd:
		huntmode = huntmode + 1
	SkipAdd:


	if Huntmode = 0 then AddFinal:
	goto SkipFinal:
	AddFinal:
		Huntmode = 1
	SkipFinal:

	if Huntmode < 40 then SkipReset: ' if huntmode is this old, then we lost the crate - go back to seeking mode
		Huntmode = 0
		dir = 128
		dirz = 5
		changedirleft = 1
	SkipReset:

	if rc_sw1 = 1 then BothPart2:
	goto SkipBoth:
	BothPart2:

	if rc_sw6 = 1 then BothAhead:
	goto skipBoth:

	BothAhead:
		left = 200
		right = 200
	
	goto SkipBoth:

	SkipBoth:

	if rc_sw1 = 0 then Correct1:
	goto NoCorrect:
	correct1:
	if rc_sw6 = 0 then correct2:
	goto nocorrect:
	correct2:
	if huntmode < 40 then correct3:
	goto nocorrect:
	correct3:
	if huntmode > 15 then correct:
	goto nocorrect:
	correct:
		left = lastleft
		right = lastright
	goto nocorrect:


	NoCorrect:
	left = 254 - left
	right = 254 - right
	
	
	

	temp = left
	left = right
	right = temp

	if p1_sw_trig = 0 then SkipEmergShutoff:
		left = 127
		right = 127
	
	SkipEmergShutOff:

skipAuton:

Left = 254 - Left
Right = 254 - Right

debug dec3 ? left
debug dec3 ? right

out8 = Towerup
out9 = 1- Towerup
out10 = relay3_fwd
out11 = 0
out12 = relay1_fwd
out13 = relay1_rev


'=============================================================================================================
'========== 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,Left,  relayA,Right, relayB,Left,  Right, p1_x,  115,   dir  ,  dirz,   127,115,    127,115,    115,    115,    115,    fork]

Goto MainLoop:


Stop