' PROGRAM: 2000 Default Code with One Joystick Control RevNC ' Written by: Innovation First, Inc. ' Date: 03 Feb 00 ' ' 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 PWM1 VAR byte 'Motor 1 PWM2 VAR byte 'Motor 2 PWM3 VAR byte 'Motor 3 PWM4 VAR byte 'Motor 4 PWM5 VAR byte 'Both ball dumper motors 'dumper VAR byte 'PWM7 VAR byte PWM6 VAR byte '---------- 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 speedx VAR byte speedy 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 ' 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 '========= 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 '========== 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 = 0 '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 'dumper = 0 '========== 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 ======================================================== '============================================================================= pwm3 = 128 'pwm4 = 128 speedx = 127 speedy = 127 pwm5 = 127 relay1_rev = 0 PUT 1, 127 PUT 2, 127 put 3, 127 PUT 5, 0 Mainloop: Serin COMA\COMB, INBAUD, [oi_swA,oi_swB,rc_swA,rc_swB,p2_x,p1_x,p4_y,p3_x,PB_mode,p2_y,p1_y,p4_y,p3_y,p1_wheel,p1_wheel,p1_wheel,p1_wheel] relay2_fwd = 1 gosub FILTER: gosub CUTSPEED: gosub DEADSTICK: gosub BALLDUMPER: gosub GOALGRABBER: gosub BALLHANDLER: 'gosub SERVO: GET 1,speedx GET 2,speedy if p3_sw_trig = 1 then flip: PWM1 = (((2000 + speedy - speedx + 127) Min 2000 Max 2254) - 2000) '<---ADDED PWM2 = (((2000 + speedy + speedx - 127) Min 2000 Max 2254) - 2000) '<---ADDED goto flipend: flip: PWM1 = abs(255 - (((2000 + speedy - speedx + 127) Min 2000 Max 2254) - 2000)) '<---ADDED PWM2 = abs(255 - (((2000 + speedy + speedx - 127) Min 2000 Max 2254) - 2000)) '<---ADDED flipend: debug HOME 'debug "p3_sw_top ", DEC p3_sw_top," rc_sw9: ", DEC rc_sw9, " RC_sw6: ", DEC rc_sw6, " rc_sw7 ", DEC rc_sw7, " pwm5 ", DEC pwm5, " pwm6 ", DEC pwm6, " rc_sw8 ", DEC rc_sw8 debug " PWM1: ", DEC PWM1, " PWM2: ", DEC PWM2, " PWM3: ", DEC PWM3 'debug " PWM5: ", DEC PWM5, " PWM6: ", DEC PWM6, CR Serout USERCPU, OUTBAUD, [255,255,pwm1,relayA,pwm2,relayB,pwm3,pwm3,PWM5,PWM6,p3_y,p4_y,p1_x,p2_x,p3_x,p4_y,p1_wheel,p1_wheel,p1_wheel,p1_wheel,127,127,127,127] goto Mainloop: x_step CON 8 y_step CON 25 FILTER: GET 1,speedx IF p2_x > speedx then raisex: IF p2_x < speedx then lowerx: GOTO endx: lowerx: speedx = (speedx MIN x_step) - x_step GOTO endx: raisex: speedx = (speedx MAX (255 - x_step)) + x_step endx: speedx = p2_x PUT 1, speedx GET 2, speedy IF p2_y > speedy then raisey: IF p2_y < speedy then lowery: GOTO endy: lowery: speedy = (speedy MIN y_step) - y_step GOTO endy: raisey: speedy = (speedy MAX (255 - y_step)) + y_step endy: speedy=p2_y PUT 2,speedy RETURN DEADSTICK: GET 2, speedy if abs(p2_y-127)>10 then ed2y: speedy=127 PUT 2,speedy ed2y: GET 1,speedx if abs(p2_x-127)>10 then ed2x: speedx=127 PUT 1, speedx ed2x: RETURN CUTSPEED: GET 1,speedx if speedx>127 then cutx1: if speedx<127 then cutx2: goto endcutx: cutx1: speedx=((speedx-127)/3)+127 goto endcutx: cutx2: speedx=127-((127-speedx)/3) endcutx: PUT 1, speedx RETURN step1 CON 1 ' <-- speed at which ball wheels reverse 'speedx = 127 BALLHANDLER: GET 3, speedx speedx = 127 if p3_sw_aux2 = 0 then ballhon: speedx = 250 ballhon: if pwm3 > speedx then down: if pwm3 < speedx then up: goto temp3: up: pwm3 = (pwm3 + 1) MAX 254 goto temp3: down: pwm3 = (pwm3 - 1) MIN 127 temp3: PUT 3, speedx RETURN BALLDUMPER: pwm5 = 127 pwm6 = 127 if p3_sw_top = 1 then ballup: if p3_sw_aux1 = 1 then balldown: goto enddumper: ballup: pwm5 = 200 + (54 * rc_sw8) pwm6 = 1 + (54 * rc_sw8) if rc_sw6 = 0 OR rc_sw9 = 0 then a1: pwm5 = 127 pwm6 = 127 a1: goto enddumper: balldown: pwm5 = 55 - (54 * rc_sw8) pwm6 = 254 - (54 * rc_sw8) if rc_sw7 = 0 then a3: pwm5 = 127 pwm6 = 127 a3: goto enddumper: enddumper: if rc_sw8 = 1 then labelledo: goto labelledo2: labelledo: out8 = 1 goto enddump2: labelledo2: out8 = 0 enddump2: RETURN GOALGRABBER: relay1_fwd = 0 relay1_rev = 0 if p2_sw_trig = 1 then goal1: if p2_sw_trig = 0 then goal2: goto goale: goal1: relay1_fwd = 1 goto goale: goal2: relay1_rev = 1 goale: if rc_sw1 = 0 then a4: relay1_fwd = 0 a4: if rc_sw2 = 0 then a5: relay1_rev = 0 a5: RETURN