' PROGRAM:    FirstDef.bsx      Example Program
' Written by: Innovation First, Inc. and
'             Parallax, Inc.
' Date:       12/28/99
' Modified:   01/24/00
'
' Define BS2-SX Project Files
' {$STAMP BS2SX,PGM1.bsx,PGM2.bsx,PGM3.bsx,PGM4.bsx}
'
' Note! By defining all of the consituent programs as above, merely opening
'       this file will invoke the stamp editor to open the constituent files
'       too.
' Note! This example code assumes that you have a joystick connected to P1 of
'       the operator interface, the operator interface, radio link, and the
'       Robot controller are up and running, and you have a servo plugged in
'       to PWM1
'


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

PB_mode	VAR byte
'packet_num	VAR byte
'delta_t	VAR byte
Temp		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) -----------------------
'  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
'  start and stop 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
InitDone		var Temp.bit0

'---------- Bit flags used to manage multiple programs -----------------------
'  Each bit of this register is set when the 


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


'========== Scratch pad RAM usage  ===========================================

'0	used for Power on status in PGM 0
'1	used for Speed RAM logic in PGM 2


'========== PGM 0  =====================================================

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

p1_x = 127		'Port 1, X-axis on Joystick
p2_x = 127		'Port 2, X-axis on Joystick
p3_x = 127		'Port 3, X-axis on Joystick
p4_x = 127		'Port 4, X-axis on Joystick

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

p1_wheel = 127	'Port 1, Wheel on Joystick
p2_wheel = 127	'Port 2, Wheel on Joystick
p3_wheel = 127	'Port 3, Wheel on Joystick
p4_wheel = 127	'Port 4, Wheel on Joystick

'p1_aux = 127	'Port 1, Aux Analog
'p2_aux = 127	'Port 2, Aux Analog
'p3_aux = 127	'Port 3, Aux Analog
'p4_aux = 127	'Port 4, Aux Analog


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

'This portion of the program should be exectuted only once after power up, and not thereafter.
'So, we will set a bitflag when it is done, and before executing, test the bitflag to see if
' it should be executed.

		get 0,Temp
		if InitDone = 1 then Mainloop
		debug "Doing Init code...",cr
		Output COMA
		low COMA
		low COMB
		put 1,127				' Init Ramp Variable to 127
Wait_init:	if IN3 = 1 then Wait_init:
		Shiftout COMB,COMA,1, [dataInitA,dataInitB,dataInitC,dataInitD,dataInitVolt]
		Input COMA
		high COMB
		InitDone = 1
		put 0,Temp

'========== 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 "Define Constants for Init" section.
'  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.
'
'  Example of Serin variable order (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,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 ===============================================
'  Add you custom code here.
'  Delete any of the following sections below (except for Output Data in the last program).


'---------- Buttons to Relays ------------------------------------------------

'  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
						'Relay 1 wont go Forward if rc_sw1 is ON

relay1_rev = p1_sw_top &~ rc_sw2 	'Port 1 Thumb = Relay 1 Reverse
						'Relay 1 wont go Reverse if rc_sw2 is ON

relay2_fwd = p2_sw_trig &~ rc_sw3 	'Port 2 Trigger = Relay 2 Forward
						'Relay 2 wont go Forward if rc_sw3 is ON

relay2_rev = p2_sw_top &~ rc_sw4 	'Port 2 Thumb = Relay 2 Reverse
						'Relay 2 wont go Reverse if 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 always Forward
relay8_rev = 0				'Relay 8 always Forward


'---------- Feedback LEDs for PWM1, PWM2 -------------------------------------

Out8  =   p1_y/216		'LED is ON when Victor883 full forward (default CAL)
Out9  = ~(p1_y/56  max 1)	'LED is ON when Victor883 full reverse (default CAL)
Out10 =   p2_y/216		'LED is ON when Victor883 full forward (default CAL)
Out11 = ~(p2_y/56  max 1)	'LED is ON when Victor883 full reverse (default CAL)


'---------- Feedback LEDs for Relay1, Relay2 ---------------------------------

Out13 = relay1_fwd		'LED is ON when Relay 1 is Forward
Out12 = relay1_rev		'LED is ON when Relay 1 is Reverse
Out15 = relay2_fwd		'LED is ON when Relay 2 is Forward
Out14 = relay2_rev		'LED is ON when Relay 2 is Reverse


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


	Debug "PGM0: Port:1 Y:", dec3 p1_y
	Run 1		' Go to next program (PGM 1)

' Note! The Output Data section must be moved to the end of the program (in the last program file).