'MODIFIED BY YANG AND YU-LOUNG '****************************************************************************** '* INPUT/OUTPUT MAPPING '* Spikes: '* #1 - compressor '* #2 - arm up/down '* #3 - gripper hook '* #4 - light '* '* Victors: '* #1 - drive motor - left '* #2 - drive motor - right '* '* Robot Digital Inputs: '* #1 - 115 psi pressure switch '* '* Controller Switches: '* port 4, pin 2 - compressor switch '* port 4, pin 9 - arm up/down switch '* port 4, pin 15 - gripper switch '****************************************************************************** ' PROGRAM: Full-Size RC 2003 Default (2 Joystick Drive) ' Written by: Innovation First, Inc. ' Date: 2002 Dec 18 ' ' Define BS2-SX Project Files ' ' {$PBASIC 2.5} ' {$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 '-------------------------- ART-added Variables ------------------------ auton_case VAR nib auton_count VAR byte phase1 VAR byte phase2 VAR byte phase3 VAR byte phase4 VAR byte phase5 VAR byte '---------- 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 0 c_p4_y CON 0 c_p1_x CON 0 c_p2_x CON 0 c_p3_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 0 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 0 c_delta_t CON 0 c_PB_mode CON 1 c_packet_num CON 1 c_res01 CON 0 '-------- ART-Added Constants -------------- '------------------------------------------- c_on CON 1 c_off CON 0 c_motor_max CON 250 c_motor_fwd2 CON 210 c_motor_fwd1 CON 170 c_motor_min CON 5 c_motor_stop CON 127 c_light_on CON 1 '@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ TO CHANGE AUTONOMOUS VARIABLES!!!!!!!!!! 'Number of seconds * 38 c_fwd1_short CON 17 c_fwd1_time CON 50 c_turn_time CON 38 c_fwd2_short CON 17 c_fwd2_time CON 50 c_fwd3_short CON 17 c_fwd3_time CON 50 '@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ '---------- 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 '--------------- ART INITIALIZATION auton_count = 0 phase1 = c_fwd1_short phase1_1 = c_fwd1_short + c_fwd1_short phase1_2 = c_fwd1_time phase2 = c_fwd1_time + c_turn_time phase3 = c_fwd1_time + c_turn_time + c_fwd2_short phase3_1 = c_fwd1_time + c_turn_time + c_fwd2_short + c_fwd2_short phase3_2 = c_fwd1_time + c_turn_time + c_fwd2_time phase4 = c_fwd1_time + c_turn_time + c_fwd2_time + c_turn_time phase5 = c_fwd1_time + c_turn_time + c_fwd2_time + c_turn_time + c_fwd3_short phase5_1 = c_fwd1_time + c_turn_time + c_fwd2_time + c_turn_time + c_fwd3_short + c_fwd3_short phase5_2 = c_fwd1_time + c_turn_time + c_fwd2_time + c_turn_time + c_fwd3_time relay1_fwd = 0 relay1_rev = 0 relay2_fwd = 0 relay2_rev = 0 relay3_fwd = 0 relay3_rev = 0 '------------------------------------------------------------------- '-------------- Autonomous Mode Case ------------------------------- auton_case = 2 '============================================================================================================= '========== 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 DO LOOP ===================================================================================== '============================================================================================================= Do '---------- 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,rc_swA,PB_mode,packet_num, p2_y,p1_y] ' art user added subprogram ' *****Basically I increased the number of phases (and the constants) with motor_fwd intervals until max at 250. It seems ' like the most logical way of doing it but it'll be interesting to see if you have other ways of doing it, unless you want ' to do another one like c_fwd1_short2 instead of c_fwd1_short + c_fwd1_short for the phases. Both cases' straightaways ' have been done, but you wanted to do the turns.***** ' *****NOTE TO YANG AND MYSELF (if I forget): If you want to add even more intervals, just add more c_motor_fwdX (x=number), ' fix and shorten the duration of c_fwd1_short to 10 (or whatever you want it), ' and make phase1_3, phase1_4, etc. by adding additional c_fwdX_short (don't blame me if this note doesn't make sense--it's ' like midnight and I just finished bio)***** if auton_mode then select (auton_case) case 1 'two ninety degree turns to the left select (auton_count) case 0 to phase1 'first turn, drive forward p1_y = c_motor_fwd1 p2_y = c_motor_fwd1 case 0 to phase1_1 'first turn, drive forward p1_y = c_motor_fwd2 p2_y = c_motor_fwd2 case 0 to phase1_2 'first turn, drive forward p1_y = c_motor_max p2_y = c_motor_max case phase1 + 1 to phase2 'turn left 90 degrees p1_y = c_motor_max p2_y = c_motor_min case phase2 + 1 to phase3 'straight for a while p1_y = c_motor_fwd1 p2_y = c_motor_fwd1 case phase2 + 1 to phase3_1 'straight for a while p1_y = c_motor_fwd2 p2_y = c_motor_fwd2 case phase2 + 1 to phase3_2 'straight for a while p1_y = c_motor_max p2_y = c_motor_max case phase3 + 1 to phase4 'second left 90 degree turn p1_y = c_motor_max p2_y = c_motor_min case phase4 + 1 to phase5 'straight into the boxes and bump them down p1_y = c_motor_fwd1 p2_y = c_motor_fwd1 case phase4 + 1 to phase5_1 'straight into the boxes and bump them down p1_y = c_motor_fwd2 p2_y = c_motor_fwd2 case phase4 + 1 to phase5_2 'straight into the boxes and bump them down p1_y = c_motor_max p2_y = c_motor_max case else 'at the end of the loop, stop the motors p1_y = c_motor_stop p2_y = c_motor_stop endselect case 2 'two ninety degree turns to the right select (auton_count) case 0 to phase1 'first turn, drive forward p1_y = c_motor_fwd1 p2_y = c_motor_fwd1 case 0 to phase1_1 'first turn, drive forward p1_y = c_motor_fwd2 p2_y = c_motor_fwd2 case 0 to phase1_2 'first turn, drive forward p1_y = c_motor_max p2_y = c_motor_max case phase1 + 1 to phase2 'turn right 90 degrees turn p1_y = c_motor_min p2_y = c_motor_max case phase2 + 1 to phase3 'straight for a while p1_y = c_motor_fwd1 p2_y = c_motor_fwd1 case phase2 + 1 to phase3_1 'straight for a while p1_y = c_motor_fwd2 p2_y = c_motor_fwd2 case phase2 + 1 to phase3_2 'straight for a while p1_y = c_motor_max p2_y = c_motor_max case phase3 + 1 to phase4 'second right 90 degree turn p1_y = c_motor_min p2_y = c_motor_max case phase4 + 1 to phase5 'straight into the boxes and bump them down p1_y = c_motor_fwd1 p2_y = c_motor_fwd1 case phase4 + 1 to phase5_1 'straight into the boxes and bump them down p1_y = c_motor_fwd2 p2_y = c_motor_fwd2 case phase4 + 1 to phase5_2 'straight into the boxes and bump them down p1_y = c_motor_max p2_y = c_motor_max case else 'at the end of the loop, stop the motors p1_y = c_motor_stop p2_y = c_motor_stop endselect endselect auton_count = auton_count + 1 endif relay1_fwd = p3_sw_trig & rc_sw1 'switch for compressor port 3 pin 2 relay1_rev = 0 'this is the 115psi switch on the Robot Controller relay2_fwd = p3_sw_top 'switch for compressor port 3 pin 7 relay2_rev = ~p3_sw_top 'switch for compressor port 3 pin 10 relay3_fwd = p3_sw_aux1 'switch for compressor port 3 pin relay3_rev = ~p3_sw_aux1 'switch relay4_fwd = 1 'turns the rotating light ON relay4_rev = 0 '----------- 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 '---------- 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 = 0 then 'User Mode is Off select (p1_y) 'Check position of Port 1 Joystick case 0 to 56 'Joystick is in full reverse position Out8 = 0 ' Turn PWM1 green LED - OFF Out9 = 1 ' Turn PWM1 red LED - ON case 125 to 129 'Joystick is in neutral position Out8 = 1 ' Turn PWM1 Green LED - ON Out9 = 1 ' Turn PWM1 red LED - ON case 216 to 255 'Joystick is in full forward position Out8 = 1 ' Turn PWM1 Green LED - ON Out9 = 0 ' Turn PWM1 red LED - OFF case else 'In either forward or reverse position Out8 = 0 ' Turn PWM1 green LED - OFF Out9 = 0 ' Turn PWM1 red LED - OFF endselect select (p2_y) 'Check position of Port 2 Joystick case 0 to 56 'Joystick is in full reverse position Out10 = 0 ' Turn PWM2 green LED - OFF Out11 = 1 ' Turn PWM2 red LED - ON case 125 to 129 'Joystick is in neutral position Out10 = 1 ' Turn PWM2 Green LED - ON Out11 = 1 ' Turn PWM2 red LED - ON case 216 to 255 'Joystick is in full forward position Out10 = 1 ' Turn PWM2 Green LED - ON Out11 = 0 ' Turn PWM2 red LED - OFF case else 'In either forward or reverse position Out10 = 0 ' Turn PWM2 green LED - OFF Out11 = 0 ' Turn PWM2 red LED - OFF endselect '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 else '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 endif '---------- PWM outputs Limited by Limit Switches --------------------------- ' if rc_sw5 = 1 then p3_y = p3_y MAX 127 ' if rc_sw6 = 1 then p3_y = p3_y MIN 127 ' if rc_sw7 = 1 then p4_y = p4_y MAX 127 ' if rc_sw8 = 1 then p4_y = p4_y MIN 127 ' if rc_sw9 = 1 then p1_wheel = p1_wheel MAX 127 ' if rc_sw10 = 1 then p1_wheel = p1_wheel MIN 127 ' if rc_sw11 = 1 then p2_wheel = p2_wheel MAX 127 ' if rc_sw12 = 1 then p2_wheel = p2_wheel MIN 127 ' if rc_sw13 = 1 then p3_wheel = p3_wheel MAX 127 ' if rc_sw14 = 1 then p3_wheel = p3_wheel MIN 127 ' if rc_sw15 = 1 then p4_wheel = p4_wheel MAX 127 ' if rc_sw16 = 1 then p4_wheel = p4_wheel MIN 127 '============================================================================================================= '========== 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,0,127,127,127,127,127,127, 127,127,127,127] Loop 'Never Exits