' PROGRAM: 2000 Default Code RevNC.bsx ' Written by: Innovation First, Inc. ' Date: 12/28/99 ' ' 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 left_control var p1_y 'left joystick motor control right_control var p2_y 'right joystick motor control '---------- 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 'holds relay output variables relayB VAR byte 'holds relay output variables '---------- Misc. ------------------------------------------------------------ PB_mode VAR byte 'packet_num VAR byte 'delta_t 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 reverse_control var p2_sw_top 'reverse the motor speeds caster_control var p1_sw_top 'move caster up or down lg_control var p1_sw_trig 'left grabber joystick control rg_control var p2_sw_trig 'right grabber joystick control lg_sw_grab var p4_sw_trig 'left grabber grab switch lg_sw_release var p4_sw_top 'left grabber release switch rg_sw_grab var p4_sw_aux1 'right grabber grab switch rg_sw_release var p4_sw_aux2 'left grabber release switch lg_sw_control var p1_sw_aux1 'left grabber manual wind control rg_sw_control var p1_sw_aux2 'right grabber manual wind control '---------- 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 hp_switch VAR rc_sw1 'high pressure switch lp_switch VAR rc_sw2 'low pressure switch ls_forward VAR rc_sw3 'front left sensor on servo rs_forward VAR rc_sw4 'front right sensor on servo ls_grabber VAR rc_sw5 'left grabber sensor rs_grabber VAR rc_sw6 'right grabber sensor lg_unwind VAR rc_sw7 'left grabber motor unwind limit switch lg_wind VAR rc_sw8 'left grabber motor wind limit switch rg_unwind VAR rc_sw9 'right grabber motor unwind limit switch rg_wind VAR rc_sw10 'right grabber motor wind limit switch rg_in VAR rc_sw11 'right grabber pneumatics in limit switch rg_out VAR rc_sw12 'right grabber pneumatics out limit switch lg_in VAR rc_sw13 'left grabber pneumatics in limit switch lg_out VAR rc_sw14 'left grabber pneumatics out limit switch '---------- 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 leftsolenoid_fwd VAR relay1_fwd leftsolenoid_rev VAR relay1_rev rightsolenoid_fwd VAR relay2_fwd rightsolenoid_rev VAR relay2_rev castersolenoid_fwd VAR relay3_fwd castersolenoid_rev VAR relay3_rev lightrelay_fwd VAR relay8_fwd '---------- 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 '-------------------USER DEFINED CONSTANTS------------------------------------ ON CON 1 OFF CON 0 DEAD_ZONE CON 10 'movement range not counted from joystick SAFE_ZONE CON 1000 'added for pos val - min/max operations MAX_ACC CON 10 'maximum allowed motor change MAX_ACC_VAL CON SAFE_ZONE + MAX_ACC 'con for min motor change MIN_ACC_VAL CON SAFE_ZONE - MAX_ACC 'con for max motor change MOTOR_OFF CON 127 'constant to hold variable COMPRESSOR_ACC CON 5 'maximum allowed compressor change MOTOR_MAX CON 254 'maximum value allowed for speed MOTOR_MIN CON 0 'minimum value allowed for speed UP CON 0 'value for caster solenoid - up DOWN CON 1 'value for caster solenoid - down CASTER_CON CON 30 'variation from 127 for caster speed GRABBER_CON CON 30 'variation from 127 for window motor speed WIND_SPEED CON MOTOR_OFF - GRABBER_CON 'wind speed - grabber UNWIND_SPEED CON MOTOR_OFF + GRABBER_CON 'unwind speed - grabber WOUND CON 0 'value for grabber being wound UNWOUND CON 1 'value for grabber being unwound IN CON 0 'value for grabber cylinder being in OUT CON 1 'value for grabber cylinder bing out '-------------------USER DEFINED VARIABLES------------------------------------ speed_l var byte 'left motor speed speed_r var byte 'right motor speed compressor_speed var byte 'speed for compressor speed controller lg_motor var byte 'variable for left grabber motor speed rg_motor var byte 'variable for right grabber motor speed tempjoystick var byte 'temporary storage space 'state variables trigger1_last var bit 'track if solenoid button is held down trigger2_last var bit 'track if solenoid button is held down caster_control_last var bit 'track if caster button is held down caster_state var bit 'tracks desired state of the caster lg_cylinder var bit 'state variable - left cylinder position rg_cylinder var bit 'state variable - right cylinder postion 'grabber varialbes value var nib 'variable for value lookup table (grabber) index var nib 'variable for index lookup table (grabber) lg_grab_desired var bit 'tracks if desired - grab left grabber lg_release_desired var bit 'tracks if desired - release left grabber rg_grab_desired var bit 'tracks if desired - grab right grabber rg_release_desired var bit 'tracks if desired - release right grabber lg_wind_desired var bit 'tracks desired state of left window motor rg_wind_desired var bit 'tracks desired state of right window motor '-------------------USER VARIABLE INITIALIZATIONS----------------------------- speed_l = MOTOR_OFF speed_r = MOTOR_OFF compressor_speed = MOTOR_OFF lg_motor = MOTOR_OFF rg_motor = MOTOR_OFF trigger1_last = OFF trigger2_last = OFF caster_control_last = OFF caster_state = UP 'sensor variables lg_grab_desired = OFF lg_release_desired = OFF rg_grab_desired = OFF rg_release_desired = OFF lg_wind_desired = WOUND rg_wind_desired = WOUND lg_cylinder = IN rg_cylinder = IN lightrelay_fwd = ON tempjoystick = 0 value = 0 index = 0 leftsolenoid_fwd = lg_cylinder leftsolenoid_rev = ~lg_cylinder rightsolenoid_fwd = rg_cylinder rightsolenoid_rev = ~rg_cylinder castersolenoid_fwd = caster_state castersolenoid_rev = OFF '========= 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 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 '========== 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 '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 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 '========== 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,PB_mode,p2_y,p1_y] '---------- Blink BASIC RUN LED ---------------------------------------------- Toggle 7 'Basic Run LED on the RC is toggled ON/OFF every loop. '---------- 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 '========== PERFORM OPERATIONS =============================================== ' Add your custom code here. ' Delete any of the following sections below (except for Output Data) as desired. Gosub Controls Gosub Drive Gosub LeftGrabber Gosub RightGrabber Gosub Pneumatics Gosub Compressor Gosub GrabberMotor '========== 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,speed_l,relayA,speed_r,relayB,compressor_speed,lg_motor,rg_motor,127,127,127,127,127,127,127,127,127,127,127] Goto MainLoop Controls: Gosub caster_control_routine Gosub lg_control_routine Gosub rg_control_routine Gosub lg_grabber_routine Gosub rg_grabber_routine goto Control_return caster_control_routine: if caster_control = ON then caster_alternate caster_control_last = OFF return caster_alternate: if caster_control_last = ON then caster_alternate_return caster_control_last = ON caster_state = ~caster_state caster_alternate_return: return lg_control_routine: if lg_control = ON then lg_trigger_check trigger1_last = OFF if lg_sw_grab = ON then lg_control_grab if lg_sw_release = ON then lg_control_release return lg_trigger_check: if trigger1_last = ON then Control_return trigger1_last = ON lg_cylinder = ~lg_cylinder return lg_control_grab: lg_grab_desired = ON return lg_control_release: lg_release_desired = ON return rg_control_routine: if rg_control = ON then rg_trigger_check trigger2_last = OFF if rg_sw_grab = ON then rg_control_grab if rg_sw_release = ON then rg_control_release return rg_trigger_check: if trigger2_last = ON then Control_return trigger2_last = ON rg_cylinder = ~rg_cylinder return rg_control_grab: rg_grab_desired = ON return rg_control_release: rg_release_desired = ON return lg_grabber_routine: if lg_grab_desired = ON then lg_grabber_routine_return if lg_release_desired = ON then lg_grabber_routine_return lg_wind_desired = lg_sw_control lg_grabber_routine_return: return rg_grabber_routine: if rg_grab_desired = ON then rg_grabber_routine_return if rg_release_desired = ON then rg_grabber_routine_return rg_wind_desired = rg_sw_control rg_grabber_routine_return: return Control_return: return Drive: inverse: if reverse_control = OFF then left_motor tempjoystick = left_control left_control = ~right_control right_control = ~tempjoystick left_motor: if (abs(left_control - 127) > DEAD_ZONE) then left_calc left_control = MOTOR_OFF left_calc: speed_l = speed_l + (((left_control - speed_l + SAFE_ZONE) min MIN_ACC_VAL max MAX_ACC_VAL) - SAFE_ZONE) right_motor: if (abs(right_control - 127) > DEAD_ZONE) then right_calc right_control = MOTOR_OFF right_calc: speed_r = speed_r + (((right_control - speed_r + SAFE_ZONE) min MIN_ACC_VAL max MAX_ACC_VAL) - SAFE_ZONE) Drive_return: return LeftGrabber: if lg_grab_desired = ON then LeftGrabber_grab if lg_release_desired = ON then LeftGrabber_release goto LeftGrabber_return LeftGrabber_grab: if lg_in = ON then LeftGrabber_grab_out if lg_out = ON then LeftGrabber_grab_in goto LeftGrabber_return LeftGrabber_grab_out: lg_cylinder = OUT goto LeftGrabber_return LeftGrabber_grab_in: lg_cylinder = IN lg_grab_desired = OFF goto LeftGrabber_return LeftGrabber_release: if lg_in = ON then LeftGrabber_release_wind if lg_unwind = ON then LeftGrabber_release_in if lg_out = ON then LeftGrabber_release_unwind lg_cylinder = OUT goto LeftGrabber_return LeftGrabber_release_unwind: lg_wind_desired = UNWOUND goto LeftGrabber_return LeftGrabber_release_in: lg_cylinder = IN goto LeftGrabber_return LeftGrabber_release_wind: lg_cylinder = IN lg_wind_desired = WOUND lg_release_desired = OFF LeftGrabber_return: return RightGrabber: if rg_grab_desired = ON then RightGrabber_grab if rg_release_desired = ON then RightGrabber_release goto RightGrabber_return RightGrabber_grab: if rg_in = ON then RightGrabber_grab_out if rg_out = ON then RightGrabber_grab_in goto RightGrabber_return RightGrabber_grab_out: rg_cylinder = OUT goto RightGrabber_return RightGrabber_grab_in: rg_cylinder = IN rg_grab_desired = OFF goto RightGrabber_return RightGrabber_release: if rg_in = ON then RightGrabber_release_wind if rg_unwind = ON then RightGrabber_release_in if rg_out = ON then RightGrabber_release_unwind rg_cylinder = OUT goto RightGrabber_return RightGrabber_release_unwind: rg_wind_desired = UNWOUND goto RightGrabber_return RightGrabber_release_in: rg_cylinder = IN goto RightGrabber_return RightGrabber_release_wind: rg_cylinder = IN rg_wind_desired = WOUND rg_release_desired = OFF RightGrabber_return: return Compressor: if lp_switch = ON then Compressor_on if hp_switch = OFF then Compressor_off if compressor_speed > 127 then Compressor_on goto Compressor_return Compressor_on: compressor_speed = (compressor_speed + COMPRESSOR_ACC) max MOTOR_MAX goto Compressor_return Compressor_off: compressor_speed = MOTOR_OFF goto Compressor_return Compressor_return: return Pneumatics: Left_Cylinder: leftsolenoid_fwd = lg_cylinder leftsolenoid_rev = ~lg_cylinder Right_Cylinder: rightsolenoid_fwd = rg_cylinder rightsolenoid_rev = ~rg_cylinder Caster_Cylinder: castersolenoid_fwd = caster_state Pneumatics_return: return GrabberMotor: GrabberMotor_left: lg_motor = MOTOR_OFF value = lg_wind_desired + (2*lg_wind) + (4*lg_unwind) lookdown value, [0,1,2,3,4,5], index lookup index, [WIND_SPEED, UNWIND_SPEED, MOTOR_OFF, UNWIND_SPEED, WIND_SPEED, MOTOR_OFF], lg_motor GrabberMotor_right: rg_motor = MOTOR_OFF value = rg_wind_desired + (2*rg_wind) + (4*rg_unwind) lookdown value, [0,1,2,3,4,5], index lookup index, [WIND_SPEED, UNWIND_SPEED, MOTOR_OFF, UNWIND_SPEED, WIND_SPEED, MOTOR_OFF], rg_motor GrabberMotor_return: return Stop