Full-Size_RC_DEF_2003_PB2.bsx

' PROGRAM:    Full-Size RC Default 2003 (2 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

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

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,p2_x,p1_x,p4_x,p3_x,PB_mode,packet_num,p2_y,p1_y,p4_y,p3_y,p2_wheel,p1_wheel,p4_wheel,p3_wheel]


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



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


'---------- Buttons to Relays---------------------------------------------------------------------------------
'-------------------------------------------------------------------------------------------------------------
'   This maps the joystick buttons to specific relay outputs.  Relays 1 and 2
'   use limit switches to stop the movement in one direction.
'   The &  used below is the PBASIC symbol for AND
'   The &~ used below is the PBASIC symbol for AND NOT

	relay1_fwd = p1_sw_trig &~ rc_sw1 	'Port 1 Trigger = Relay 1 Forward, unless rc_sw1 is ON
	relay1_rev = p1_sw_top  &~ rc_sw2 	'Port 1 Thumb   = Relay 1 Reverse, unless rc_sw2 is ON
	relay2_fwd = p2_sw_trig &~ rc_sw3 	'Port 2 Trigger = Relay 2 Forward, unless rc_sw3 is ON
	relay2_rev = p2_sw_top  &~ rc_sw4 	'Port 2 Thumb   = Relay 2 Reverse, unless rc_sw4 is ON

	relay3_fwd = p3_sw_trig			'Port 3 Trigger = Relay 3 Forward
	relay3_rev = p3_sw_top 			'Port 3 Thumb   = Relay 3 Reverse
	relay4_fwd = p4_sw_trig			'Port 4 Trigger = Relay 4 Forward
	relay4_rev = p4_sw_top 			'Port 4 Thumb   = Relay 4 Reverse

	relay5_fwd = p1_sw_aux1			'Port 1 Aux1    = Relay 5 Forward
	relay5_rev = p1_sw_aux2			'Port 1 Aux2    = Relay 5 Forward
	relay6_fwd = p3_sw_aux1			'Port 3 Aux1    = Relay 6 Forward
	relay6_rev = p3_sw_aux2			'Port 3 Aux2    = Relay 6 Forward
	relay7_fwd = p4_sw_aux1			'Port 4 Aux1    = Relay 7 Forward
	relay7_rev = p4_sw_aux2			'Port 4 Aux2    = Relay 7 Reverse

	relay8_fwd = 1				'Relay 8        = Forward (turns the rotating light ON)
	relay8_rev = 0				'Relay 8        = Forward


'---------- PWM Feedback lights-------------------------------------------------------------------------------
'-------------------------------------------------------------------------------------------------------------
'   This drives the "PWM1" and "PWM2" "Robot Feedback" lights on the Operator 
'   Interface.  The lights are green for joystick forward and red for joystick
'   reverse.  Both red and green are on when the joystick is centered.  Use the
'   trim tabs on the joystick to adjust the center.


	if user_display_mode = 1 then skip_this_code
		if p1_y > 129 then p1_y_not_127
		if p1_y < 125 then p1_y_not_127
			Out8 = 1
			Out9 = 1
			goto exit_p1_y_test
		p1_y_not_127:
			Out8  = p1_y/216                     'LED is ON when Port 1 Y is full forward
			Out9  = ~(p1_y/56  max 1)            'LED is ON when Port 1 Y is full reverse
		exit_p1_y_test:
	
		if p2_y > 129 then p2_y_not_127
		if p2_y < 125 then p2_y_not_127
			Out10 = 1
			Out11 = 1
			goto exit_p2_y_test
		p2_y_not_127:
			Out10 = p2_y/216                     'LED is ON when Port 2 Y is full forward
			Out11 = ~(p2_y/56  max 1)            'LED is ON when Port 2 Y is full reverse
		exit_p2_y_test:
	skip_this_code:



'---------- Relay Feedback Lights ----------------------------------------------------------------------------
'-------------------------------------------------------------------------------------------------------------
'   This drives the "Relay 1" and "Relay 2" "Robot Feedback" lights on the Operator 
'   Interface.  

	if user_display_mode = 1 then user_mode_is_ON
		Out13 = relay1_fwd                      'LED is ON when Relay 1 is CW  
		Out12 = relay1_rev                      'LED is ON when Relay 1 is CCW
		Out15 = relay2_fwd                      'LED is ON when Relay 2 is CW  
		Out14 = relay2_rev                      'LED is ON when Relay 2 is CCW
	goto display_done
	
	user_mode_is_ON:                                'Send Port1 Y to 7-segment display when PB_mode.bit5 = 1
		out8    = p1_y.bit0
		out9    = p1_y.bit1
		out10   = p1_y.bit2
		out11   = p1_y.bit3
		out12   = p1_y.bit4
		out13   = p1_y.bit5
		out14   = p1_y.bit6
		out15   = p1_y.bit7
	display_done:

'---------- PWM outputs Limited by Limit Switches  -----------------------------------------------------------
'-------------------------------------------------------------------------------------------------------------

if rc_sw5 = 0 then next1:
	p3_y = p3_y MAX 127
next1:

if rc_sw6 = 0 then next2:
	p3_y = p3_y MIN 127
next2:

if rc_sw7 = 0 then next3:
	p4_y = p4_y MAX 127
next3:

if rc_sw8 = 0 then next4:
	p4_y = p4_y MIN 127
next4:

if rc_sw9 = 0 then next5:
	p1_wheel = p1_wheel MAX 127
next5:

if rc_sw10 = 0 then next6:
	p1_wheel = p1_wheel MIN 127
next6:

if rc_sw11 = 0 then next7:
	p2_wheel = p2_wheel MAX 127
next7:

if rc_sw12 = 0 then next8:
	p2_wheel = p2_wheel MIN 127
next8:

if rc_sw13 = 0 then next9:
	p3_wheel = p3_wheel MAX 127
next9:

if rc_sw14 = 0 then next10:
	p3_wheel = p3_wheel MIN 127
next10:

if rc_sw15 = 0 then next11:
	p4_wheel = p4_wheel MAX 127
next11:

if rc_sw16 = 0 then next12:
	p4_wheel = p4_wheel MIN 127
next12:


'=============================================================================================================
'========== 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,p1_y,relayA,p2_y,relayB,p3_y,p4_y,p1_x,p2_x,p3_x,p4_x,p1_wheel,p2_wheel,p3_wheel,p4_wheel]

Goto MainLoop:


Stop