Re: 2002 FRC Controller...
Ok well here is the code i tried. i cleaned it up a lot so other's can look and see what i have done for other robots.
Hopefully you can spot the problem...
Code:
' PROGRAM: CK4 Full Size Code
'{$PORT COM4}
' Written by: B Gallo
' Date: 2008 Jul 14
'
' 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 Primary Driver Steering
'p2_x VAR byte 'Port 2, X-axis on Joystick n/a
p3_x VAR byte 'Port 3, X-axis on Joystick Secondary Driver Moni Krunch Steering
'p4_x VAR byte 'Port 4, X-axis on Joystick n/a
p1_y VAR byte 'Port 1, Y-axis on Joystick Primary Driver Forward/Reverse
'p2_y VAR byte 'Port 2, Y-axis on Joystick n/a
p3_y VAR byte 'Port 3, Y-axis on Joystick Secondary Driver Mini Krunch Forward/Reverse
'p4_y VAR byte 'Port 4, Y-axis on Joystick n/a
'p1_wheel VAR byte 'Port 1, Wheel on Joystick n/a
'p2_wheel VAR byte 'Port 2, Wheel on Joystick n/a
'p3_wheel VAR byte 'Port 3, Wheel on Joystick n/a
'p4_wheel VAR byte 'Port 4, Wheel on Joystick n/a
'p1_aux VAR byte 'Port 1, Aux on Joystick n/a
'p2_aux VAR byte 'Port 2, Aux on Joystick n/a
'p3_aux VAR byte 'Port 3, Aux on Joystick n/a
'p4_aux VAR byte 'Port 4, Aux on Joystick n/a
'---------- 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 Steering Potentiometer
'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
'---------- Robot Controller PWM Outputs ---------------------------------------------------------------------
'-------------------------------------------------------------------------------------------------------------
PWM1 VAR byte
PWM2 VAR byte
PWM3 VAR byte
PWM4 VAR byte
PWM5 VAR byte
PWM6 VAR byte
PWM7 VAR byte
'---------- Misc. --------------------------------------------------------------------------------------------
'-------------------------------------------------------------------------------------------------------------
packet_num VAR byte
'delta_t VAR byte
PB_mode VAR byte
emergency_stop VAR byte 'Stop Robot until Power Cycle
pressure_switch VAR byte 'turn on Relay LED when robot is Pressurized
'=============================================================================================================
'========== 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 n/a
p1_sw_top VAR oi_swA.bit1 'Joystick Top Button, same as Port4 pin8 n/a
p1_sw_aux1 VAR oi_swA.bit2 'Aux input, same as Port4 pin9 n/a
p1_sw_aux2 VAR oi_swA.bit3 'Aux input, same as Port4 pin15 n/a
p3_sw_trig VAR oi_swA.bit4 'Joystick Trigger Button, same as Port2 pin5 n/a
p3_sw_top VAR oi_swA.bit5 'Joystick Top Button, same as Port2 pin8 n/a
p3_sw_aux1 VAR oi_swA.bit6 'Aux input, same as Port2 pin9 n/a
p3_sw_aux2 VAR oi_swA.bit7 'Aux input, same as Port2 pin15 n/a
p2_sw_trig VAR oi_swB.bit0 'Joystick Trigger Button Switch to High Gear
p2_sw_top VAR oi_swB.bit1 'Joystick Top Button Switch to Low Gear
p2_sw_aux1 VAR oi_swB.bit2 'Aux input n/a
p2_sw_aux2 VAR oi_swB.bit3 'Aux input Emergency Stop
p4_sw_trig VAR oi_swB.bit4 'Joystick Trigger Button Release Mini Krunch
p4_sw_top VAR oi_swB.bit5 'Joystick Top Button Open Left Claw
p4_sw_aux1 VAR oi_swB.bit6 'Aux input Open Right Claw
p4_sw_aux2 VAR oi_swB.bit7 'Aux input Emergency Stop
'---------- Aliases for each RC switch input -----------------------------------------------------------------
'-------------------------------------------------------------------------------------------------------------
' Below are aliases for the digital inputs located on the Robot Controller.
rc_sw1 VAR rc_swA.bit0 'Pressure Switch
rc_sw2 VAR rc_swA.bit1 'n/a
rc_sw3 VAR rc_swA.bit2 'n/a
rc_sw4 VAR rc_swA.bit3 'n/a
rc_sw5 VAR rc_swA.bit4 'n/a
rc_sw6 VAR rc_swA.bit5 'n/a
rc_sw7 VAR rc_swA.bit6 'n/a
rc_sw8 VAR rc_swA.bit7 'n/a
rc_sw9 VAR rc_swB.bit0 'n/a
rc_sw10 VAR rc_swB.bit1 'n/a
rc_sw11 VAR rc_swB.bit2 'n/a
rc_sw12 VAR rc_swB.bit3 'n/a
rc_sw13 VAR rc_swB.bit4 'n/a
rc_sw14 VAR rc_swB.bit5 'n/a
rc_sw15 VAR rc_swB.bit6 'n/a
rc_sw16 VAR rc_swB.bit7 'n/a
'---------- Aliases for each RC Relay outputs ----------------------------------------------------------------
'-------------------------------------------------------------------------------------------------------------
' Below are aliases for the relay outputs located on the Robot Controller.
relay1_fwd VAR RelayA.bit0 'Compressor
relay1_rev VAR RelayA.bit1 'n/a
relay2_fwd VAR RelayA.bit2 'High Gear
relay2_rev VAR RelayA.bit3 'Low Gear
relay3_fwd VAR RelayA.bit4 'Open Left Claw
relay3_rev VAR RelayA.bit5 'Close Left Claw
relay4_fwd VAR RelayA.bit6 'Open Right Claw
relay4_rev VAR RelayA.bit7 'Close Right Claw
relay5_fwd VAR RelayB.bit0 'Team Color Light
relay5_rev VAR RelayB.bit1 'n/a
relay6_fwd VAR RelayB.bit2 'n/a
relay6_rev VAR RelayB.bit3 'n/a
relay7_fwd VAR RelayB.bit4 'n/a
relay7_rev VAR RelayB.bit5 'n/a
relay8_fwd VAR RelayB.bit6 'n/a
relay8_rev VAR RelayB.bit7 'n/a
'---------- 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 0
c_p3_y CON 1
c_p4_y CON 0
c_p1_x CON 1
c_p2_x CON 0
c_p3_x CON 1
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 1
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 1
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 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
'=============================================================================================================
'========== 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
'~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
'====================================== EMERGENCY STOP COMMAND INITIALIZE ====================================
'~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
emergency_stop = 0
'=============================================================================================================
'========== 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,p1_x,p3_x,PB_mode,packet_num,p1_y,p3_y]
'---------- 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 = 1 &~ rc_sw1 'Relay 1 Forward, unless rc_sw1 is ON Compressor ON
relay1_rev = 0 ' Compressor OFF
relay2_fwd = p2_sw_trig 'Port 2 Trigger = Relay 2 Forward, High Speed
relay2_rev = p2_sw_top 'Port 2 Thumb = Relay 2 Reverse, Low Speed
relay3_fwd = 1 'Relay 3 Forward Left Claw Closed
relay3_rev = p4_sw_top 'Port Thumb = Relay 3 Reverse Left Claw Open
relay4_fwd = 1 'Port 4 Trigger = Relay 4 Forward Right Claw Closed
relay4_rev = p4_sw_aux1 'Port 4 Thumb = Relay 4 Reverse Right Claw Open
relay5_fwd = 1 'Relay 5 Forward = Team Color Light Always on
relay5_rev = 0 '
'relay6_fwd = '
'relay6_rev = '
'relay7_fwd = '
'relay7_rev = '
'relay8_fwd = '
'relay8_rev = '
'---------- 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 skip_this_code
if p1_y > 129 then p1_y_not_127
if p1_y < 125 then p1_y_not_127
Out8 = 1
Out9 = 1
goto exit_p1_y_test
p1_y_not_127:
Out8 = p1_y/216 'LED is ON when Port 1 Y is full forward
Out9 = ~(p1_y/56 max 1) 'LED is ON when Port 1 Y is full reverse
exit_p1_y_test:
if p1_x > 129 then p1_x_not_127
if p1_x < 125 then p1_x_not_127
Out10 = 1
Out11 = 1
goto exit_p1_x_test
p1_x_not_127:
Out10 = p1_x/216 'LED is ON when Port 1 X is full forward
Out11 = ~(p1_x/56 max 1) 'LED is ON when Port 1 X is full reverse
exit_p1_x_test:
skip_this_code:
'~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
'====================================== Driving Code =========================================================
'~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
'---------- Primary Drive ------------------------------------------------------------------------------------
'-------------------------------------------------------------------------------------------------------------
' This code Tells the robot to put forth power to motors for forward and backward movements
' Joystick forward = Robot forward
' Joystick backward = Robot backwards
PWM1 = p1_y
PWM2 = p1_y
PWM3 = p1_y
'---------- Primary Steer ------------------------------------------------------------------------------------
'-------------------------------------------------------------------------------------------------------------
' This code Tells the robot to steer left or right depending on the potentiometer values
' Joystick forward = Robot forward
' Joystick backward = Robot backwards
PWM4 = p1_x
'---------- Secondary Drive (Minikrunch) ---------------------------------------------------------------------
'-------------------------------------------------------------------------------------------------------------
' This code Tells the robot to put forth power to motors for forward and backward movements on Mini Krunch
' Joystick forward = Robot forward
' Joystick backward = Robot backwards
PWM5 = p3_y
'---------- Secondary Drive (Minikrunch) ---------------------------------------------------------------------
'-------------------------------------------------------------------------------------------------------------
' This code Tells the robot to put forth power to motors for forward and backward movements
' Joystick forward = Robot forward
' Joystick backward = Robot backwards
PWM5 = p3_y
'---------- Secondary Steer (Minikrunch) ---------------------------------------------------------------------
'-------------------------------------------------------------------------------------------------------------
' This code Tells the robot to steer left or right on Mini Krunch
' Joystick forward = Robot forward
' Joystick backward = Robot backwards
PWM6 = p3_x
'~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
'====================================== END Driving Code =====================================================
'~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
'---------- Pressure Switch Value ----------------------------------------------------------------------------
'-------------------------------------------------------------------------------------------------------------
If relay1_fwd = 0 then next1:
pressure_switch = 1
next1:
If relay1_fwd = 1 then next2:
pressure_switch = 0
next2:
'---------- Relay Feedback Lights ----------------------------------------------------------------------------
'-------------------------------------------------------------------------------------------------------------
' This drives the "Relay 1" and "Relay 2" "Robot Feedback" lights on the Operator
' Interface.
if user_display_mode = 1 then user_mode_is_ON
Out13 = relay1_fwd 'LED is ON when Relay 1 is CW Compressor ON
Out12 = pressure_switch 'LED is ON when Pressure Switch = 1 Up to Pressure
Out15 = relay2_fwd 'LED is ON when Relay 2 is CW High Gear
Out14 = relay2_rev 'LED is ON when Relay 2 is CCW Low Gear
goto display_done
user_mode_is_ON: 'Send Port1 Y to 7-segment display when PB_mode.bit5 = 1
out8 = p1_y.bit0
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
display_done:
'~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
'====================================== Driving Code Limitations =============================================
'~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
'---------- Rear wheels dont move while in High gear (relay2_fwd=0) ------------------------------------------
'-------------------------------------------------------------------------------------------------------------
PWM2 = p1_y
PWM3 = p1_y
if relay2_fwd = 1 then next3:
PWM2 = 127
PWM3 = 127
next3:
'---------- Rear wheels move while in Low gear----------------------------------------------------------------
'-------------------------------------------------------------------------------------------------------------
if relay2_rev = 1 then next4:
PWM2 = p1_y
PWM3 = p1_y
next4:
'~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
'====================================== END Driving Code Limitations =========================================
'~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
'~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
'====================================== Mini Krunch Release ==================================================
'~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
If p4_sw_trig = 1 then next5:
PWM7 = 0
next5:
If p4_sw_trig = 0 then next6:
PWM7 = 254
next6:
'~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
'====================================== EMERGENCY STOP COMMAND CHECK =========================================
'~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
If p2_sw_aux1 = 0 then next7:
emergency_stop = 1
next7:
'~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
'====================================== EMERGENCY STOP COMMAND ===============================================
'~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
If emergency_stop = 1 then nevermind:
PWM1 = 127 'Front Drive Motor
PWM2 = 127 'Rear Left Drive Motor
PWM3 = 127 'Rear Right Drive Motor
PWM4 = 127 'Steering Motor
PWM5 = 127 'Mini Krunch Drive Motor
PWM6 = 127 'Mini Krunch Steering Motor
PWM7 = 127 'Mini Krunch Release Servo
relay1_fwd = 0 'Compressor
relay2_fwd = 0 'High Gear
relay2_rev = 1 'Low Gear
relay3_fwd = 1 'Open Left Claw
relay3_rev = 0 'Close Left Claw
relay4_fwd = 1 'Open Right Claw
relay4_rev = 0 'Close Right Claw
relay5_fwd = 0 'Team Color Light
nevermind:
'=============================================================================================================
'========== 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),(PWM2),(PWM3),(PWM4),(PWM5),(PWM6),(PWM7),relayA,relayB]
Serout USERCPU, OUTBAUD, [255,255,p1_y,p1_y,p1_y,p1_x,p3_y,p3_x,127,relayA,relayB]
Goto MainLoop:
Stop
' --------------------------------------------------------------------
' / Copyright (c) 2005 USFIRST Robotics Team-79 (Team Krunch) \
'| |
'| The Captain Krunch program is free software; you can redistribute it |
'| and/or modify it under the terms of the GNU General Public License as |
'| published by the Free Software Foundation; version 2 of the License. |
'| |
'| This program is distributed in the hope that it will be useful, |
'| but WITHOUT ANY WARRANTY; without even the implied warranty of |
'| MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU |
'| General Public License for more details. |
'| |
'| You should have received a copy of the GNU General Public License |
'| along with this program; if not, write to the Free Software |
'| Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. |
'| |
'| The following additional restrictions apply to all files associated |
'| with the software unless explicitly disclaimed in individual files. |
'| |
'| 1. The origin of this software must not be misrepresented; you must |
'| not claim that you wrote the original software. |
'| 2. Altered source versions must be plainly marked as such, and must |
'| not be misrepresented as being the original software. |
'| 3. If you redistribute modified sources, please include in the file |
'| ChangeLog history the information documenting your changes. |
'| 4. If you use any of this software in a Robot, an acknowledgment in |
'| the Robot documentation would be appreciated but is not required. |
'| |
'| Please send bug reports and comments by E-mail to: |
'| |
'| bgallo8@earthlink.net or Captain@krunch79.com |
'| |
'| For bug reports, please include the version number of KRUNCH code, |
'| the make-options used to compile it, and as much additional |
'| information as possible. |
'| |
'| This software contains contributions from other authors. Where |
' \ possible, those authors are acknowledged in the source code. /
' --------------------------------------------------------------------
|
