' PROGRAM: Full-Size RC 2003 Default (2 Joystick Drive)
' Written by: Innovation First, Inc.
' Date: 2002 Dec 18

' Modified by:Daniel Wesley Mahan
' Date: 2003 Jan 8

' Define BS2-SX Project Files
'

' { 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 ----------------------------------------------------------
'-------------------------------------------------------------------------------------------------------------
AirOn Var bit
AirTimer var byte

LatchOn Var bit
LatchTimer var byte

ExtRetSpeed var byte
Phase var nib




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

extravar var byte

'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




'---------- Daniel Wesley Mahan Declarations for Variables ------------------------------------------------------

i VAR word 'indicator used for counting loop cycles






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

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



Phase = 0

ExtRetSpeed = 127

'=============================================================================================================
'========== MAIN DO LOOP =====================================================================================
'=============================================================================================================


'---------- Daniel Wesley Mahan Edit Code --------------------------------------------------------------------

i = 1 ' Make initial value of i equal to one



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,p2_y,p1_y,p4_y,p3_y,extravar,extravar,extravar,extravar]



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

If auton_mode = 1 Then auto_1 'if in auto mode run automation routine



If rc_sw7 = 0 Then goarm
If rc_sw7 = 1 Then stoparm

stoparm:
If p3_y < 127 Then quitarm
If p3_y > 126 Then goarm
quitarm:
p3_y = 127
goarm:



'---------- 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 User_Mode_On 'Skip this if User Mode is on

if p1_y<=56 then Full_Reverse 'User Mode is off, checking status of p1_y

if p1_y<=129 AND p1_y>=125 then Neutral

if p1_y<=255 AND p1_y>=216 then Full_Forward





'In either forward or reverse position
OUT8 = 0 ' Turn PWM1 green LED - OFF
OUT9 = 0 ' Turn PWM1 red LED - OFF
goto Done_With_Port_One

Full_Reverse: 'Joystick is in full reverse position
OUT8 = 0 ' Turn PWM1 green LED - OFF
OUT9 = 1 ' Turn PWM1 red LED - ON
goto Done_With_Port_One

Neutral: 'Joystick is in neutral position
OUT8 = 1 ' Turn PWM1 Green LED - ON
OUT9 = 1 ' Turn PWM1 red LED - ON
goto Done_With_Port_One

Full_Forward: 'Joystick is in full forward position
OUT8 = 1 ' Turn PWM1 Green LED - ON
OUT9 = 0 ' Turn PWM1 red LED - OFF
goto Done_With_Port_One

Done_With_Port_One:

if p2_y<=56 then Full_Reverse_Two 'User Mode is off, checking status of p2_y

if p2_y<=129 AND p2_y>=125 then Neutral_Two

if p2_y<=255 AND p2_y>=216 then Full_Forward_Two

'In either forward or reverse position
OUT10 = 0 ' Turn PWM2 green LED - OFF
OUT11 = 0 ' Turn PWM2 red LED - OFF
goto Done_With_Port_Two

Full_Reverse_Two: 'Joystick is in full reverse position
OUT10 = 0 ' Turn PWM2 green LED - OFF
OUT11 = 1 ' Turn PWM2 red LED - ON
goto Done_With_Port_Two

Neutral_Two: 'Joystick is in neutral position
OUT10 = 1 ' Turn PWM2 Green LED - ON
OUT11 = 1 ' Turn PWM2 red LED - ON
goto Done_With_Port_Two

Full_Forward_Two: 'Joystick is in full forward position
OUT10 = 1 ' Turn PWM2 Green LED - ON
OUT11 = 0 ' Turn PWM2 red LED - OFF
goto Done_With_Port_Two


Done_With_Port_Two:

'This drives the "Relay 1" and "Relay 2" "Robot Feedback" lights on the OI
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 Done_With_This_Section


User_Mode_On: 'User Mode is ON
OUT8 = p1_y.bit0 ' Set ouput bits to display p1_y variable
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

Done_With_This_Section:

'------------------------------ BEGIN USER ADDED CODE ------------------------


'special junk

if NOT(p1_sw_top = 1) then fourt
p1_y = 255 - (p1_x)
p2_y = p1_x
fourt:

if NOT(p1_sw_trig = 1) then fivet
p1_y = p1_y
p2_y = p1_y
fivet:

'-------------------------Air Code

relay7_fwd = 0 'compressor (relay 7) is always on by default
relay7_rev = 0

if Not(AirTimer <= 255) then nine 'always count up timer.
AirTimer = AirTimer + 1
nine:

if NOT(AirOn = 1) then seven 'if air is on, then drive the compressor (on relay 7)
relay7_fwd = 1
relay7_rev = 0
seven:

if Not(rc_sw1 = 1) then eight
AirOn = 1
AirTimer = 0
eight:

if Not((AirTimer = 127)) then ten 'if timer maxes out, then turn the air off.
AirOn = 0
ten:

'-------------------------End Air Code



'-------------------------Latch Code

p1_x = ExtRetSpeed

if not((p2_sw_top = 1) AND (Phase = 0) AND (rc_sw2 = 1 )) then onel
ExtRetSpeed = 230
Phase = 1
onel:

if not((rc_sw2 = 0) AND (Phase = 1)) then twol
ExtRetSpeed = 255
Phase = 2
twol:

if not((rc_sw2 = 1) AND (Phase = 2)) then threel
ExtRetSpeed = 60
Phase = 3
threel:

if not((rc_sw2 = 0) AND (Phase = 3)) then fourl
ExtRetSpeed = 0
Phase = 4
fourl:

if not((rc_sw2 = 1) AND (Phase = 4)) then fivel
ExtRetSpeed = 127
Phase = 0
fivel:

'-------------------------End Latch Code


relay1_fwd = p2_sw_trig
relay1_rev = 0
relay2_fwd = p4_sw_top
relay2_rev = p4_sw_trig


'-----------


'------------------------------ END USER ADDED CODE --------------------------







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


Goto end_1
auto_1:

FOR i = 1 TO 300 'first command, go forward
p1_y = 210 'sets value of tire 1 to forward
p2_y = 210 'sets value of tire 2 to forward
Serout USERCPU, OUTBAUD, [255,255,p1_y,relayA,p2_y,relayB,p3_y,p4_y,p1_x,p2_x,p3_x,p4_x,127,127,127,127,127,127,127,127]
NEXT

FOR i = 301 TO 475 'second command, turn right
p1_y = 200
p2_y = 85
Serout USERCPU, OUTBAUD, [255,255,p1_y,relayA,p2_y,relayB,p3_y,p4_y,p1_x,p2_x,p3_x,p4_x,127,127,127,127,127,127,127,127]
NEXT

FOR i = 476 TO 626 'third command, go forward
p1_y = 210
p2_y = 210
Serout USERCPU, OUTBAUD, [255,255,p1_y,relayA,p2_y,relayB,p3_y,p4_y,p1_x,p2_x,p3_x,p4_x,127,127,127,127,127,127,127,127]
NEXT

FOR i = 627 TO 821 'fourth command, turn right
p1_y = 200
p2_y = 85
Serout USERCPU, OUTBAUD, [255,255,p1_y,relayA,p2_y,relayB,p3_y,p4_y,p1_x,p2_x,p3_x,p4_x,127,127,127,127,127,127,127,127]
NEXT

FOR i = 822 TO 1702 'fifth command, go forward
p1_y = 210
p2_y = 210
Serout USERCPU, OUTBAUD, [255,255,p1_y,relayA,p2_y,relayB,p3_y,p4_y,p1_x,p2_x,p3_x,p4_x,127,127,127,127,127,127,127,127]
NEXT

FOR i = 1703 TO 3000 'sixth command, stop
p1_y = 127
p2_y = 127
Serout USERCPU, OUTBAUD, [255,255,p1_y,relayA,p2_y,relayB,p3_y,p4_y,p1_x,p2_x,p3_x,p4_x,127,127,127,127,127,127,127,127]
NEXT

end_1:

Serout USERCPU, OUTBAUD, [255,255,p1_y,relayA,p2_y,relayB,p3_y,p4_y,p1_x,p2_x,p3_x,p4_x,127,127,127,127,127,127,127,127]


goto MainLoop 'Never Exits