Chief Delphi

Chief Delphi (http://www.chiefdelphi.com/forums/index.php)
-   Programming (http://www.chiefdelphi.com/forums/forumdisplay.php?f=51)
-   -   2002 FRC Controller... (http://www.chiefdelphi.com/forums/showthread.php?t=68458)

gallo26 14-07-2008 23:53
2002 FRC Controller...
 
Hey I'm working on repairing all my team's old robots back to working condition for robot demonstrations and driver practice. The problem i am facing is these controllers now use BASIC programming. Now i understand most of it but im still stuck on one part of it. the PWMs...

This robot is a 3 wheeled robot usin rocking gearshift. When in low gear, all 3 wheels and powered. When in High gear the back 2 wheels are not powered, and the front one drives the robot. Steering uses one motor and a potentiometer. When programmed correctly, the steering will react and mimic the joystick like an RC car would (servo). We also have a Mini Krunch which has a servo and motor onboard that runs off the port 3 X and Y axis

I am working on linking the first 3 PWMs to Port 1 Y axis. thats easy. and the Steering motor to PWM4. But i dont see where to link that, or how to add the potentiometer into this. And then after that... how do i tell it to not power the rear motors while in high gear?

I read all the documents about Legacy and all that, but i just dont understand it...

Thank you to anyone who can help me with this!!!

Mark McLeod 15-07-2008 10:05
Re: 2002 FRC Controller...
 
Draw yourself a flowchart of what you what to happen.
The code will depend a little on what version of pBasic you’re using. In 2002 there was an if-then statement but no if-then-else. 2003 introduced the if-then-else, so nowadays it’s easier to code up.

Turning off the motors when in high gear would look something like:
Code:
pwm1 = p1_y
pwm2 = p1_y
pwm3 = p1_y
If gear < High then  skipit
    pwm2 = 127
    pwm3 = 127
skipit:
Make sure the victors are in coast though, otherwise, you'll be trying to drag rather than roll the rear wheels around.

For steering I imagine you’d want to force your potentiometer reading to match your joystick steering axis (making sure the pot is mechanically at 127 when the wheel is straight forward. Here is a simple way, but you'd have to play with the values for pwm4 so it didn't turn too quickly, overshoot, and start thrashing back n forth.
P.S. It's also nice to have some mechanical drag on the steering, so it holds it's position a little and isn't just waggling loosely.
Code:
pwm4 = 127
If pot < p1_x then pwm4 = 100
If pot > p1_x then pwm4 = 155
A more sophisticated control would use the difference between where you want to be (p1_x) and where it currently is (pot) to slow down the steering motor as p1_x and pot get close. For example, pwm4 = (p1_x - pot) * 5 / 10, where you'd play with the value 5/10 until you slowed the response so it wouldn't overshoot and thrash. Just be aware of how pBasic deals with negative numbers.

gallo26 15-07-2008 10:23
Re: 2002 FRC Controller...
 
Well im not too sure i need to worry about the victors, The wheels are indirectly powered. They are inflated tires and giant rollers come down and push on them to power them. When it switches to high gear, it ricks with 3 pistons, removing force from the rear wheels and pushing a larger roller on the front wheel.

Ok thanks! that pretty much addresses all my problems. I'll add that and then go out and test it Thursday when im at the school.

One more thing..... do i need to edit or change the Serout line?

again Thanks so much!

Mark McLeod 15-07-2008 10:26
Re: 2002 FRC Controller...
 
You shouldn't have to change Serout. The pwm's should already be there.

P.S. I saw your victor comment. Wherever you have your code to move the rollers up and down you can probably drop in your victor control as well.

gallo26 15-07-2008 10:52
Re: 2002 FRC Controller...
 
Yeah ok, that works. Can i limit the PWMs by saying something like...

Code:
'---------- PWM outputs Limited ------------------------------------------------------------------------------
'-------------------------------------------------------------------------------------------------------------

if relay2_fwd = 1 then next1:
        PWM2 = 127
next1:

if relay2_fwd = 1 then next2:
        PWM3 = 127

next2:

if relay2_fwd = 0 then next3:
        PWM2 = p1_y
next3:

if relay2_fwd = 0 then next4:
        PWM3 = p1_y
next 4:
I saw that in the default code for PWM limitations with limit switches. so i thought if i said whenever the High gear part of that relay is active.... that specific PWM is off

Mark McLeod 15-07-2008 10:58
Re: 2002 FRC Controller...
 
Yep, that'll work.
That's the older pBasic style, so it's demonstrating one way to work around not having an if-then-else statement. There are several ways to do it. This is a shorter way to do the same thing:
Code:
PWM2 = p1_y
if relay2_fwd = 1 then next1:
            PWM2 = 127
next1:

gallo26 15-07-2008 11:01
Re: 2002 FRC Controller...
 
o ok. lol yeah that looks a lot easier!

Now one thing i was just mentioned, we want to add Emergency Stop buttons.

In pBASIC how to i say When i press this button p2_sw_aux1, the robot is disabled until i do a powercycle?

Mark McLeod 15-07-2008 11:16
Re: 2002 FRC Controller...
 
You want to create a software "latch" that remembers the emergency stop button was pushed.

Up at the top declare a variable that will be your latch.
Code:
emergency_stop VAR byte ' Stop until power cycle
Make sure you initialize it BEFORE your MainLoop:
Code:
emergency_stop = 0
Inside your main loop check p2_sw_aux1 to see if you should stop.
Code:
If p2_sw_aux1 = 1 then emergency_stop = 1
emergency_stop will only get set to 1. It never resets to 0 anywhere unless you power cycle.

Then immediately before your Serout, test emergency_stop and turn everything off.
Code:
If emergency_stop = 0 then nevermind:
    PWM1 = 0
    PWM2 = 0
 'and so on (also decide how you want your relays set)
nevermind:
You could also put an alternate Serout command inside the If with 127's hardcoded in place of the PWM variables.

Another way to do it would be to prevent the regular Serout from being called at all by wrapping it with your emergency_stop check and the Master controller will shut everything down for you. This'll force the "Basic Run Err" light to come on in about a tenth of a second.
Code:
If emergency_stop = 1 then nevermind:
    Serout(...
nevermind:

gallo26 15-07-2008 11:26
Re: 2002 FRC Controller...
 
wow thanks!!!! as you can tell i'm not really the programming type, but i decided to work on it before the next meeting so we can run tests with it.

You have been such a big help thanks!!! :)

gallo26 15-07-2008 19:32
Re: 2002 FRC Controller...
 
Sorry to bother you again, but i finally finished what i needed to do with the code thanks to your help. but now im getting an error. When i do a Syntax Check i get an
Quote:
Error 129-Expected ':' or end of-line
But it only does that for everyline i have that states
Code:
PWM1 = p1_y
or anything else with a PWM. what could be causing this?

Thanks again for helping me!

Mark McLeod 15-07-2008 20:38
Re: 2002 FRC Controller...
 
The PWM1,2,3,4 that you're using need to be declare just like we declared "emergency_stop" in an earlier post.

Looking at the default code I'm guessing you started with, I spoke too quickly about not needing to change the Serout command. You don't have to change it, but you might want to so that it's clearer.
---------------
In the default code you're using, the "PWM" variables don't really exist.
PWMx is used only in comments and have not been declared. (comment lines start with a ').

These two lines are at the bottom:
Code:
'  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,relayB,p3_y,p4_y,p1_x,p2_x,p3_x,p4_x,p1_wheel,p2_wheel,p3_wheel,p4_wheel]
The first line is only a comment and the PWMs are just marking the fixed positions in the Serout call where you put the values that you want to send out to the motors. Position is everything in the Serout call. For example, see that (PWM1) is the third argument in the comment line. That means in the real line of code below it the value of p1_y, as the third argument, will be sent out to drive PWM1.

The default code actually reuses existing variables to hold the pwm values it wants to send. For instance, p3_y is used here is these lines straight from the default:
Code:
if rc_sw5 = 0 then next1:
    p3_y = p3_y MAX 127
next1:
p3_y is then used as the seventh argument in the Serout call, so it's holding the value for (PWM3).

-----------
If you do go with declaring the PWMs you started with, then I'd recommend just copying the working line of code and commenting out the original line ('). Then just replace p1_y, etc with the four PWM variables you're using.

gallo26 15-07-2008 21:21
Re: 2002 FRC Controller...
 
Ok, so all the errors caused by the PWM commands i commented out. i changed the serout as well. here is the full code. Im not worrying about the pot for steering until i can get the rest working. As you will see i was not able to get the rear wheels to stop while in high gear and im not sure what variables are needed or not. maybe you can take out the comments and see if you get the same errors i get. This was taken from the Default code and i'm not sure what i do or dont need to functionality

Thanks Mark for your help, i know this is probably annoying you by now

Code:
' PROGRAM:    CK4 Full Size Code
' 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




'---------- Misc. --------------------------------------------------------------------------------------------
'-------------------------------------------------------------------------------------------------------------
packet_num        VAR byte
'delta_t        VAR byte
PB_mode                VAR byte
emergency_stop        VAR byte        'Stop Robot until Power Cycle




'=============================================================================================================
'========== 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,p2_x,p1_x,p4_x,p3_x,PB_mode,packet_num,p2_y,p1_y,p4_y,p3_y,p2_wheel,p1_wheel,p4_wheel,p3_wheel]




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




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



'~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
'====================================== END Driving Code =====================================================
'~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~



'---------- 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 = rc_sw1 = 1                      '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----------------------------------------------------------
'-------------------------------------------------------------------------------------------------------------

'if relay2_fwd = 1 then next1:
'        PWM2 = 127
'next1:
'
'if relay2_fwd = 1 then next2:
'        PWM3 = 127
'
'next2:




'---------- Rear wheels move while in Low gear----------------------------------------------------------------
'-------------------------------------------------------------------------------------------------------------

'if relay2_fwd = 0 then next3:
'        PWM2 = p1_y
'next3:
'
'if relay2_fwd = 0 then next4:
'        PWM3 = p1_y
'next 4:



'~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
'====================================== END Driving Code Limitations =========================================
'~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~


'~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
'====================================== EMERGENCY STOP COMMAND CHECK =========================================
'~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~


'If p2_sw_aux1 = 1 then emergency_stop = 1






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

Joe Ross 15-07-2008 22:42
Re: 2002 FRC Controller...
 
Did you rearrange the serout command? everything needs to stay in the same order as the original.

Mark McLeod 16-07-2008 09:48
Re: 2002 FRC Controller...
 
The short answer is you need to keep the original Serout command in exactly the order it was,

Another issue is some of the changes you've made to the initialization constants, e.g.,
Code:
c_p2_x CON 0
have to be reflected in a change to the Serin command. A different take on that whole position thing.
They have to be in the order shown in the Serin comment line, but you remove the variables in the middle that you've turned off, such as, p2_x.

------------
The long answer...
What Joe said.
You rearranged the Serout arguments, but the other side of the call treats every position in this exact order:
Code:
' Serout USERCPU, OUTBAUD, [255,255,(PWM1),relayA,(PWM2),relayB,(PWM3),(PWM4),(PWM5),(PWM6),(PWM7),(PWM8),(PWM9),(PWM10),(PWM11),(PWM12),(PWM13),(PWM14),(PWM15),(PWM16)]
The backside of Serout doesn't see the name "relayA" and realize you moved it around. It only receives the value of relayA, e.g., 100, and just assumes the fourth argument is what you want relayA to be. So you send:
Serout USERCPU, OUTBAUD, [255,255,p1_y,relayA,p2_y,relayB]
and what the RC receives might be:
[255,255,127,102,127,100]
There's no way for the computer to know if you've swapped any of the values around.

You're getting the same thing in reverse with the Serin call. You've told the computer that p2_x isn't wanted, so you have an argument in the Serin call that the computer isn't expecting. The RC sends you values in order for
[oi_swA,oi_swB,rc_swA,rc_swB,p1_x,...]
but you're stuffing them into
[oi_swA,oi_swB,rc_swA,rc_swB,p2_x,p1_x,...]
Nobody complains, but you won't get the values you want where you want them.

The only way Serout can be changed is you can leave some of those PWM arguments off the very end if you don't happen to be using them. Serout will recognize that it only has 10 arguments rather than the full 20.

------------
Another issue you'll have is the robot controller only has space for 26 variables and right now 26 are declared. You're not using all of them, so you can comment out some to free space for some new PWMx variables. Or you can reuse the variables already declared (p1_wheel), just like the default code does with p1_y, etc.

Here's an example to turn off your rear wheels while in high gear. It reuses the variables p2_y and p3_y to hold what you want for the pwm outputs, but you can replace these with other variables.
Code:
'---------- Rear wheels dont move while in High gear (relay2_fwd=0) ------
p2_y = p1_y
p3_y = p1_y
if relay2_fwd = 1 then next1:
  p2_y = 127
  p3_y = 127
next1:
 

Serout USERCPU, OUTBAUD, [255,255,p1_y,relayA,p2_y,relayB,p3_y,p4_y,p1_x,p2_x,p3_x,p4_x,p1_wheel,p2_wheel,p3_wheel,p4_wheel]
A clearer version is:
Code:
'Comment out the variables you aren't using (and change the Serin arguments to match, including the c_p1_wheel , etc. initializations)
'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
 
' Add your new variables
PWM1    VAR Byte
PWM2    VAR Byte
PWM3    VAR Byte
PWM4    VAR Byte
PWM5    VAR Byte
PWM6    VAR Byte
PWM7    VAR Byte
 
'---------- Rear wheels dont move while in High gear (relay2_fwd=0) ------
PWM2 = p1_y
PWM3 = p1_y
if relay2_fwd = 1 then next1:
  PWM2 = 127
  PWM3 = 127
next1:
 

Serout USERCPU, OUTBAUD, [255,255,PWM1,relayA,PWM2,relayB,PWM3,PWM4,PWM5,PWM6,PWM7]
 
P.S.
I un-commented all your code and it compiles with a couple of minor errors. I don't have an RC handy to test it though, but I can get one if necessary and you have trouble.
1) Comment out the variables you aren't using (p1_wheel, p2_x, etc.) that way an error will let you know if you forgot and left it in a Serin/Serout call.
2) Add new PWMx variables
3) Fix the Serin call
4) Change the Serout call to use PWM1, etc.
5) next 4: needs to be next4:
6) The check for emergency_stop should be "= 0" rather than 1
7) The emergency_stop example I originally gave you had a syntax error. It should have been:
Code:
IF p2_sw_aux1 = 0 THEN  next5:
    emergency_stop = 1
next5:

gallo26 16-07-2008 17:17
Re: 2002 FRC Controller...
 
Wow ok everytime i edit the code and un-comment something with the new fixes, it comes out clean. thank you soo much!!!! now i cant wait to test this tomorrow.

gallo26 17-07-2008 21:21
Re: 2002 FRC Controller...
 
Ok Mark,

I put in the code and it downloaded correctly. but after i did a power cycle on the robot, it said that there was a basic error. so im not sure what the problem was. the code compiled without errors. so im not sure what happened. would you have any ideas? thanks!

Mark McLeod 17-07-2008 21:40
Re: 2002 FRC Controller...
 
That's usually a problem with the Serin/Serout commands.

I just downloaded a version of your code into a controller and it's running okay. I don't have any motors hooked up though, so no promises on what your robot will do once you get past this error.

Post your code and let's have a quick look.

gallo26 17-07-2008 21:44
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. /
'  --------------------------------------------------------------------

Mark McLeod 17-07-2008 21:54
Re: 2002 FRC Controller...
 
How come you didn't update your Serout to use the PWM1, etc, variables?
I replaced that.

Serin is missing sensor1

Then it runs.

P.S.
the emergency_stop check is still the opposite of what it should be.
The way it is now your robot will do nothing until the emergency stop is pushed :yikes:

gallo26 17-07-2008 21:57
Re: 2002 FRC Controller...
 
Oh my you know thats it. i cant believe i didnt do that. ok, let me do that and i'll test it.


ok ok, so i changed that and im still getting a BASIC INIT ERROR. no clue what is causing that now. my serout now consists of

Code:
Serout USERCPU, OUTBAUD, [255,255,p1_y,p1_y,p1_y,p1_x,p3_y,p3_x,127,relayA,relayB,(PWM1),(PWM2),(PWM3),(PWM4),(PWM5),(PWM6),(PWM7),sensor1]

Mark McLeod 17-07-2008 22:03
Re: 2002 FRC Controller...
 
That's because of my mistype before.

sensor1 is missing from Serin (I'd originally typed Serout, caught and changed it, but not quickly enough.

gallo26 17-07-2008 22:09
Re: 2002 FRC Controller...
 
ohhh ok. i see now. let me test it


ahhhh it works!!!!! no error!!!!!

ok. so if it is still backwards.... which part? the switch part? or the emergency_stop part?

Mark McLeod 17-07-2008 22:16
Re: 2002 FRC Controller...
 
This part:
Code:
If emergency_stop = 1 then nevermind:
  PWM1 = 127 'Front Drive Motor
'and so on
emergency_stop is set to be "0" when you want to be driving and "1" when you want to be disabled.
the check "= 1" goes through your PWM1 = 127 code only while emergency_stop is "0" and you want to be driving.


It should be:
Code:
If emergency_stop = 0 then nevermind:

gallo26 17-07-2008 22:33
Re: 2002 FRC Controller...
 
Ohhhhhh ok ok. i got that. thanks!! now all i need to do is figure out why the PWM lights are flashing lol and then test with the actual robot

thank you soo much! i owe you!

gallo26 18-07-2008 21:03
Re: 2002 FRC Controller...
 
Ok. the last thing i promise!

I put in the code. all is well. The pwm lights on the OI randomly flash. and i havent changed that fromt he default. but that doesnt matter.

I have a small servo and vex motor that im using to test the PWM outputs. Only PWM3 works.

I know the code might be turning off PWM1 and 2 because of high speed. but when i press and hold p2_aux2... it still wont work. and im not sure why not.

None of the other PWMs work either and they are all written the same. PWM5 should run our mini robot onboard using the port 3 X and Y for controls...

So maybe its this controller im using to test or the code... Does it work for you?

Thanks!

Mark McLeod 18-07-2008 22:41
Re: 2002 FRC Controller...
 
You can add debug printouts of any of your variables so you can see what's going on.

Here's an example:
Code:
debug 2,0,0,"PWM1 = ", DEC3 PWM1
debug 2,0,1,"PWM2 = ", DEC3 PWM2
debug 2,0,2,"PWM3 = ", DEC3 PWM3
That third number after "debug" tells it what line to print on, so it keeps printing on the same line over and over. It makes it easier to watch.
Caution: If you print too much it'll slow the controller down and the Basic Run Err light might start blinking. It's okay if it blinks, I just don't want you to worry if you see that.

The LED default is setup a little strangely, so you can fix that when you have time. Your lights are all blinking because they are displaying the value of p1_y in binary for some odd reason.

The PWMs look okay for me, but we're not working with the same copy.

We should talk about how the relays (solenoid) work. There are some odd things going on there that might do wacky things, but get your PWMs working first.

P.S. 2000 posts. I should go to bed now...

gallo26 18-07-2008 22:46
Re: 2002 FRC Controller...
 
Hahaha ok ill do that then Thanks!!

Yeah and you should get some rest, helping me with really old code probly takes a toll.

And congrats.. 2000 posts


All times are GMT -5. The time now is 19:24.

Powered by vBulletin® Version 3.6.4
Copyright ©2000 - 2017, Jelsoft Enterprises Ltd.
Copyright © Chief Delphi