' PROGRAM:    2001 FIRST										v7.8
' Written by: Team 112 SW
' Date:       4/5/01
'
'
'         N       N   RRRRRRR       OOOOO       SSSSS                  ff
'         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:
'	.01 		- Variables and if statements for switches. No code but State Manager
'				Need to add led code to output status to leds
'	.02 		- Added led code and fixed all of the variables. Placed air pump
'				code so compressor turns off when the valves are full
'	.03 		- Mini reviewed by Chris. took out calibration stuff because we
'				aren't writing that code yet.
'
'	1.0  		- Unreleased Beta version (tested a lot of stuff that ultimately didn't work)
'
'	2.0  		- Basically :) the same code as 1.0 , but with a small bug fixed that when implemented
'				with UNCROSSED drive motor wires works perfectly :)!!!!!
'     3.0a 		- Added servo switching
'	3.001a 	- Added more 'commentary'
'	3.0b 		- Added logarithmic adjustment of drive outputs
'	
'
'	6.0		- Completely working thanks to Dan and Erik spending a Friday working
'			  on it!!! Yea! Correct inputs from sensors, and correct gear shifting 
'			  routine. Sweeper arm has a large momentum buffer, and needs to be constantly
'			  maintained
'	6.5		- Added the drive logarithm to the program for FAR BETTER control
'	6.6		- Major Code Clean-up by Niraj (fixing comments, labeling subs (mmm...), 
'			  adding comments...)
'	6.7		- Fixed issues from v6.5 after reviewing
'	7.0		- The ultimate code with no issues
'
'	7.1		- Added a small braking routine to shifting code
'	7.1b		- Added code to make the compressor run extra loops to counteract
'			  the air cylinder leaking.
'	7.2		- Fully Working Code =D
'	7.3		- Mapped the balance led to indicate the state of the compressor
'	7.6		- Added drive fine control when reset button is pressed 
'	7.7		- Removed drive logarithm (normal operation log)
'	7.8		- Re-Calibrated joystick
'
'	Software Students:
'		The State Master-
'			Erik Baar
'
'		Excellent Drive Coders-
'			Niraj Sanghvi
'			Dan Leach
'
'		Balance (Slave) Team Coders-
'			Pepe Lazarevski
'			Sean Cleary
'			Carl Philips
'
'	Pseudo Adult Leaders:
'		Chris Carpenter
'		William Sanchez
'		Rahul Trived
'		3 other Motorola Engineers
'
'
'
'  ===========================================================================
'  ======= AND NOW, THE PROGRAM YOU'VE ALL BEEN WAITING FOR!!!! ==============
'  ===========================================================================
'
' 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 --------------------------
'******Commented out variables are for a two joystick system

io_joy_x	VAR byte	'Port 1, X-axis on Joystick
io_joy_y	VAR byte	'Port 1, Y-axis 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 ----------------------------

io_sweeper_pos VAR byte	'The variable that sets the sweeper position

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

io_drive_left	VAR byte	'Left drive motor output value
io_drive_right	VAR byte	'Right drive motor output value
io_servo		VAR byte	'Output value for gear shifting servo
io_sweeper_rotate	VAR byte	'Output value for the sweeper motor

'------------------ Gear Shifting Variables ----------------------------------
drv_prev_shift	VAR	bit	'Variable that stores previous state of gear
drv_shift_counter VAR	byte	'Counter for stopping the robot while shifting

'------------------ Compressor Variables -------------------------------------
compressor_counter VAR	byte	'Counter for running the compressor for longer
prev_psi		VAR	byte	'The previous psi_indicator value

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

'oi_swA
io_turbo	VAR oi_swA.bit1	'port1, pin 7
io_ramp	VAR oi_swA.bit2	'port1, pin 10
io_sweeper	VAR oi_swA.bit3	'port1, pin 14
io_rear_grab VAR oi_swA.bit6	'port2, pin 15
'oi_swB
io_start	VAR oi_swB.bit1	'port2, pin 2
io_shift	VAR oi_swB.bit0	'port2, pin 7
io_front_grab VAR oi_swB.bit2	'port2, pin 4

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

io_ramp_up	VAR rc_swA.bit1
io_psi_indicator VAR rc_swA.bit2

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

io_mars_light_fwd	VAR RelayA.bit0
io_grab_front_out	VAR RelayA.bit2
io_grab_rear_out	VAR RelayA.bit4
io_ramp_moveup	VAR RelayA.bit6
io_ramp_movedown	VAR RelayA.bit7

io_compressor	VAR RelayB.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

'========== DEFINE CONSTANTS =================================================
'=============================================================================

'---------- Arm Constants ----------------------------------------------------

GOAL_EXTEND			CON	1
GOAL_CLOSE			CON	0

RAMP_UP			CON	1
RAMP_DOWN			CON	0

SWEEPER_FORWARD_SLOW	CON	200
SWEEPER_REVERSE_SLOW	CON	80

SWEEPER_CONFLICT_AREA	CON	193

SWEEPER_MAX			CON	180
SWEEPER_ZERO		CON	35

COMPRESSOR_EXTRA_LOOPS	CON	20

'========== DRIVE CONSTANTS ==================================================
'=============================================================================

'---------- Drive Constants --------------------------------------------------
'These constants are for the joystick WITH a black thing

'DRV_JOY_X_CENTER		CON	135		'X-axis joystick center
'DRV_JOY_Y_CENTER		CON	143		'Y-axis joystick center
'DRV_JOY_X_MAX_RANGE	CON	77		'X-axis maximum value
'DRV_JOY_X_MIN_RANGE	CON	60		'X-axis minimum value
'DRV_JOY_Y_MAX_RANGE	CON	61		'Y-axis maximum value
'DRV_JOY_Y_MIN_RANGE	CON	66		'Y-axis minimum value
'DRV_JOY_LEFT_BUFFER	CON	125 		'Joystick center left buffer
'DRV_JOY_RIGHT_BUFFER	CON 	145 		'Joystick center right buffer
'DRV_JOY_UP_BUFFER	CON 	153 		'Joystick center up buffer
'DRV_JOY_DOWN_BUFFER	CON 	133 		'Joystick center down buffer

DRV_JOY_X_CENTER		CON	125		'X-axis joystick center
DRV_JOY_Y_CENTER		CON	131		'Y-axis joystick center
DRV_JOY_X_MAX_RANGE	CON	83		'X-axis maximum value
DRV_JOY_X_MIN_RANGE	CON	73		'X-axis minimum value
DRV_JOY_Y_MAX_RANGE	CON	71		'Y-axis maximum value
DRV_JOY_Y_MIN_RANGE	CON	75		'Y-axis minimum value
DRV_JOY_LEFT_BUFFER	CON	115 		'Joystick center left buffer
DRV_JOY_RIGHT_BUFFER	CON 	135 		'Joystick center right buffer
DRV_JOY_UP_BUFFER		CON 	141 		'Joystick center up buffer
DRV_JOY_DOWN_BUFFER	CON 	121 		'Joystick center down buffer





DRV_MAX_POSS_VALUES	CON	128		'The maximum possible values for the joystick
DRV_STOP_TIME		CON	10		'Number of loops to stop robot while shifting
DRV_SHIFT_LOOP		CON	20		'Number of loops to wait for servos to shift
DRV_SHIFT_SPEED		CON	142		'Speed to send to drive motors while shifting
DRV_LOG_DIV			CON	127		'The divisor used in the logarithm for
							'exponential drive increase
DRV_FC_SCALE		CON	2		'The scale factor for more sensitive drive
'---------- Drive Motor Adjust Constants -------------------------------------

'Constants for adjusting for motors to 80% power
'MUL indicates a multiplier, DIV indicates a divider

DRV_L_FWD_MUL		CON	4	
DRV_L_FWD_DIV		CON	5
DRV_L_BWD_MUL		CON	4
DRV_L_BWD_DIV		CON	5
DRV_R_FWD_MUL		CON	4	
DRV_R_FWD_DIV		CON	5
DRV_R_BWD_MUL		CON	4
DRV_R_BWD_DIV		CON	5
DRV_SERVO_HIGH 		CON 	70	'Value corresponding to high gear
DRV_SERVO_LOW		CON 	0	'Value corresponding to low gear

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


'========== DECLARE VARIABLES ================================================
'=============================================================================

'========= 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_io_joy_x		CON	1
c_p2_y		CON	0
c_io_joy_x_2	CON	0
c_p4_y		CON	0

c_io_joy_y		CON	1
c_io_joy_y_2	CON	0
c_p2_x		CON	0
c_p4_x		CON	0

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_oi_swA		CON	1
c_oi_swB		CON	1

c_sensor2		CON	0
c_io_sweeper_pos	CON	1
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	0
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 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	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_io_joy_x_2 <<1 + c_p4_x <<1 + c_io_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_io_sweeper_pos <<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

Wait_init:
	if IN3 = 1 then Wait_init:

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


'  ========== INITIALIZE VARIABLES ===========================================
'  ===========================================================================
io_drive_right = MOTOR_STOP
io_drive_left = MOTOR_STOP
io_sweeper_rotate = MOTOR_STOP
io_servo = DRV_SERVO_LOW
io_mars_light_fwd = ON
io_grab_front_out = ON
io_grab_rear_out = ON
io_ramp_moveup = ON
io_ramp_movedown = OFF
io_compressor = ON
io_shift = ON
drv_shift_counter = DRV_SHIFT_LOOP
drv_prev_shift = ON
prev_psi = OFF
compressor_counter = 0


'------wait for calibrate button to be pressed
	Serout USERCPU, OUTBAUD, [255,255,io_drive_right,relayA,io_drive_left,relayB,io_sweeper_rotate,io_servo,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.
begin_wait:
	Serin COMA\COMB, INBAUD, [oi_swA,oi_swB,rc_swA,rc_swB,io_joy_x,PB_mode,io_sweeper_pos,io_joy_y]
	gosub psi_MainLoop
	toggle 8
	toggle 7
	if (io_start = ON) then State_Manager
	io
Serout USERCPU, OUTBAUD, [255,255,io_drive_right,relayA,io_drive_left,relayB,io_sweeper_rotate,io_servo,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 begin_wait
'=============================================================================

'******the state manager infinite loop begins here!!!
State_Manager:

'---------- 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,io_joy_x,PB_mode,io_sweeper_pos,io_joy_y]

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

	gosub psi_MainLoop
	gosub arm_MainLoop
	gosub drv_MainLoop

State_End:
'========== 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,io_drive_right,relayA,io_drive_left,relayB,io_sweeper_rotate,io_servo,io_servo,MOTOR_STOP,MOTOR_STOP,MOTOR_STOP,MOTOR_STOP,MOTOR_STOP,MOTOR_STOP,MOTOR_STOP,MOTOR_STOP,MOTOR_STOP,MOTOR_STOP,MOTOR_STOP]
	Goto State_Manager

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

'Calls sub-routines and checks for the state of the gear shift

'========== THE PSI LOOP =====================================================
'=============================================================================
psi_MainLoop:

	if (io_psi_indicator = ON) then psi_no_change
	if (io_psi_indicator = prev_psi) then psi_no_change
	if (compressor_counter = 0) then psi_init
	
	io_compressor = ON
	compressor_counter = compressor_counter - 1
	if (compressor_counter = 0) then psi_no_change
	goto psi_bypass

psi_init:
	compressor_counter = COMPRESSOR_EXTRA_LOOPS
	goto psi_bypass

psi_no_change:
	io_compressor = io_psi_indicator
	prev_psi = io_psi_indicator

psi_bypass:
	if (io_compressor = OFF) then bal_led_on
	out10 = OFF
	return

bal_led_on:
	out10 = ON
	return

'========== THE ARM LOOP =====================================================
'=============================================================================

arm_MainLoop:


'code for the front goal grabber arm
	io_grab_front_out = io_front_grab			'set the solenoid output to the closed value


'code for the rear goal grabber arm
	io_grab_rear_out = io_rear_grab			'set the solenoid output to the closed value



'code for the bridge ramp arm
	if (io_sweeper_pos > SWEEPER_CONFLICT_AREA) then ramp_bypass

	if (io_ramp = ON) then turn_ramp_off

	io_ramp_moveup = ON
	io_ramp_movedown = OFF
	goto ramp_bypass

turn_ramp_off:
	io_ramp_moveup = OFF
	io_ramp_movedown = ON

ramp_bypass:

'code for the sweeper arm

	if (io_ramp_up = OFF) then arm_bypass
	if ((io_sweeper_pos <= SWEEPER_ZERO) AND (io_sweeper = OFF)) then stop_sweeper
	if ((io_sweeper_pos >= SWEEPER_MAX) AND (io_sweeper = ON)) then stop_sweeper
	if (io_sweeper = OFF) then move_sweeper_slowly_reverse

	io_sweeper_rotate = SWEEPER_FORWARD_SLOW
	return

move_sweeper_slowly_reverse:
	io_sweeper_rotate = SWEEPER_REVERSE_SLOW
	return

stop_sweeper:

	io_sweeper_rotate = MOTOR_STOP

arm_bypass:
	return

'========== THE DRIVE LOOP ===================================================
'=============================================================================
'  The drive routines follow and are commented so any normal alien can 
'  understand them

drv_MainLoop:

	out9 = ON		'turn normal led on
	out8 = OFF		'turn start led off
	
	if (drv_shift_counter <> DRV_SHIFT_LOOP) then drv_shifting
	if (drv_prev_shift <> io_shift) then drv_shift

'  Calls scale routine, then sets output values, reverses the x-axis inputs, and checks if turbo was pressed

	gosub drv_scale

	if (io_start = ON) then drv_fine_control

'	gosub drv_logarithm
	
bypass_drv_logarithm:

	'The frame of reference is from the FRONT of the robot
	
	io_drive_left = (((2000 + io_joy_y - io_joy_x + MOTOR_STOP) min 2000 max 2255) - 2000)
	io_drive_right = (((2000 + io_joy_y + io_joy_x - MOTOR_STOP) min 2000 max 2255) - 2000)

	gosub drv_rev
	
'	if (io_turbo = ON) then State_End
	if (io_shift = OFF) then State_End
	
	gosub drv_reduce

	goto State_End

'=)===========================================================================
'=)======== SUBROUTINES =)====================================================
'=)===========================================================================

drv_fine_control:
	if (io_joy_y > MOTOR_STOP) then drv_fc_y_fwd
	if (io_joy_y = MOTOR_STOP) then drv_fc_y_bypass
	io_joy_y = ((2000 + MOTOR_STOP - ((MOTOR_STOP - io_joy_y)/DRV_FC_SCALE)) min 2000 max 2255) - 2000
	goto drv_fc_y_bypass

drv_fc_y_fwd:
	io_joy_y = ((2000 + MOTOR_STOP + ((io_joy_y - MOTOR_STOP)/DRV_FC_SCALE)) min 2000 max 2255) - 2000

drv_fc_y_bypass:

	if (io_joy_x > MOTOR_STOP) then drv_fc_x_fwd
	if (io_joy_x = MOTOR_STOP) then drv_fc_x_bypass
	io_joy_x = ((2000 + MOTOR_STOP - ((MOTOR_STOP - io_joy_x)/DRV_FC_SCALE)) min 2000 max 2255) - 2000
	goto drv_fc_x_bypass

drv_fc_x_fwd:
	io_joy_x = ((2000 + MOTOR_STOP + ((io_joy_x - MOTOR_STOP)/DRV_FC_SCALE)) min 2000 max 2255) - 2000

drv_fc_x_bypass:
	goto bypass_drv_logarithm

'  ======== THE GEAR SHIFTING 'SUB' ACTUALLY LABELS ==========================
'  ===========================================================================
'  This routine uses the multiplication of non-zero DRV_SERVO_HIGH and then
'  increments a counter thus entering a loop a number of times equal to
'  DRV_SHIFT_LOOP.  During this loop, the servos are switched and the drive
'  is engaged at a speed equal to DRV_SHIFT_SPEED after DRV_STOP_TIME loops

drv_shift:
	drv_prev_shift = io_shift
	drv_shift_counter = 0
	
'  Counts the shift counter

drv_shifting:
	if (drv_shift_counter > DRV_STOP_TIME) then drv_shift_slowly
	io_drive_left = MOTOR_STOP
	io_drive_right = MOTOR_STOP
	goto drv_shift_speed_bypass

drv_shift_slowly:
	io_servo = DRV_SERVO_HIGH - (drv_prev_shift * DRV_SERVO_HIGH)
	io_drive_left = DRV_SHIFT_SPEED
	io_drive_right = DRV_SHIFT_SPEED

drv_shift_speed_bypass:
	gosub drv_rev
	drv_shift_counter = drv_shift_counter + 1
	goto State_End


'  ======== THE DRV_SCALE SUB ================================================
'  ===========================================================================
'  The following routines scale the X & Y values of the joystick to a range of 
'  128 based on where the joystick currently is. They are necessary because 
'  the joystick is imperfect.

drv_scale:
	if (io_joy_x > DRV_JOY_RIGHT_BUFFER) then drv_adjust_x_fwd
	if (io_joy_x < DRV_JOY_LEFT_BUFFER) then drv_adjust_x_bwd
	io_joy_x = MOTOR_STOP
	goto drv_bypass_adjust_x
 
drv_adjust_x_fwd:
	io_joy_x = io_joy_x - DRV_JOY_RIGHT_BUFFER
	io_joy_x = ((2000 + (DRV_MAX_POSS_VALUES * io_joy_x / DRV_JOY_X_MAX_RANGE)) min 2000 max 2127) - 2000
	io_joy_x = io_joy_x + MOTOR_STOP
	goto drv_bypass_adjust_x

drv_adjust_x_bwd:
	io_joy_x = DRV_JOY_LEFT_BUFFER - io_joy_x
	io_joy_x = ((2000 + (DRV_MAX_POSS_VALUES * io_joy_x / DRV_JOY_X_MIN_RANGE)) min 2000 max 2127) - 2000
	io_joy_x = MOTOR_STOP - io_joy_x

drv_bypass_adjust_x:
	if (io_joy_y > DRV_JOY_UP_BUFFER) then drv_adjust_y_fwd
	if (io_joy_y < DRV_JOY_DOWN_BUFFER) then drv_adjust_y_bwd
	io_joy_y = MOTOR_STOP
	goto drv_bypass_adjust_y
 
drv_adjust_y_fwd:
	io_joy_y = io_joy_y - DRV_JOY_UP_BUFFER
	io_joy_y = ((2000 + (DRV_MAX_POSS_VALUES * io_joy_y / DRV_JOY_Y_MAX_RANGE)) min 2000 max 2127) - 2000
	io_joy_y = io_joy_y + MOTOR_STOP
	goto drv_bypass_adjust_y

drv_adjust_y_bwd:
	io_joy_y = DRV_JOY_DOWN_BUFFER - io_joy_y
	io_joy_y = ((2000 + (DRV_MAX_POSS_VALUES * io_joy_y / DRV_JOY_Y_MIN_RANGE)) min 2000 max 2127) - 2000
	io_joy_y = MOTOR_STOP - io_joy_y

drv_bypass_adjust_y:
	return


'  ======== THE DRV_LOGARITHM SUB ===IT SLICES AND DICES!!!==ALL IN ONE!!!====
'  ===========================================================================
'  The following routine adjusts the drive motors to increase speed faster as 
'  they get farther from 127. It is beyond all human comprehension.

drv_logarithm:
	if (io_joy_x > MOTOR_STOP) then drv_x_fwd
	if (io_joy_x = MOTOR_STOP) then drv_x_bypass
	io_joy_x = MOTOR_STOP - ((((MOTOR_STOP - io_joy_x) * (MOTOR_STOP - io_joy_x)) / DRV_LOG_DIV) min 0 max 127)
	goto drv_x_bypass

drv_x_fwd:
	io_joy_x = MOTOR_STOP + (2000 + (((io_joy_x - MOTOR_STOP) * (io_joy_x - MOTOR_STOP)) / DRV_LOG_DIV) min 2000 max 2127) - 2000

drv_x_bypass:

	if (io_joy_y > MOTOR_STOP) then drv_y_fwd
	if (io_joy_y = MOTOR_STOP) then drv_y_bypass
	io_joy_y = MOTOR_STOP - ((((MOTOR_STOP - io_joy_y) * (MOTOR_STOP - io_joy_y)) / DRV_LOG_DIV) min 0 max 2127)
	goto drv_y_bypass

drv_y_fwd:
	io_joy_y = MOTOR_STOP + (2000 + (((io_joy_y - MOTOR_STOP) * (io_joy_y - MOTOR_STOP)) / DRV_LOG_DIV) min 2000 max 2127) - 2000

drv_y_bypass:
	return

'  ======== THE DRV_REV SUB ==================================================
'  ===========================================================================
'  This sub-routine inverts the values of the right motor because . . . ,
'  Well, just because it's the only way it'll work right :)

drv_rev:
	if (io_drive_right > MOTOR_STOP) then drv_rev_fwd
	if (io_drive_right = MOTOR_STOP) then drv_rev_bypass
	io_drive_right = ((2000 + MOTOR_STOP + (MOTOR_STOP - io_drive_right)) min 2000 max 2255) - 2000
	goto drv_rev_bypass

drv_rev_fwd:
	io_drive_right = ((2000 + MOTOR_STOP - (io_drive_right - MOTOR_STOP)) min 2000 max 2255) - 2000

drv_rev_bypass:
	return

'  ======== THE DRV_REDUCE SUB ===============================================
'  ===========================================================================
'  Reduces the output to the motors to 80% of what the input would have output 
'  to conserve the motors.

drv_reduce:
'  adjust for 80% power on drive motors
	if (io_drive_left > MOTOR_STOP) then drv_d_l_adj_fwd
	if (io_drive_left = MOTOR_STOP) then drv_d_l_no_adj
	io_drive_left = MOTOR_STOP - (((MOTOR_STOP - io_drive_left) * DRV_L_BWD_MUL) / DRV_L_BWD_DIV)
	goto drv_bypass_d_l_adj

drv_d_l_adj_fwd:
	io_drive_left = (((io_drive_left - MOTOR_STOP) * DRV_L_FWD_MUL) / DRV_L_FWD_DIV) + MOTOR_STOP
	goto drv_bypass_d_l_adj

drv_d_l_no_adj:
	io_drive_left = MOTOR_STOP

drv_bypass_d_l_adj:
	if (io_drive_right > MOTOR_STOP) then drv_d_r_adj_fwd
	if (io_drive_right = MOTOR_STOP) then drv_d_r_no_adj
	io_drive_right = MOTOR_STOP - (((MOTOR_STOP - io_drive_right) * DRV_R_BWD_MUL) / DRV_R_BWD_DIV)
	goto drv_bypass_d_r_adj

drv_d_r_adj_fwd:
	io_drive_right = (((io_drive_right - MOTOR_STOP) * DRV_R_FWD_MUL) / DRV_R_FWD_DIV) + MOTOR_STOP
	goto drv_bypass_d_r_adj

drv_d_r_no_adj:
	io_drive_right = MOTOR_STOP

drv_bypass_d_r_adj:
	return