Transcript Slide 1

Introduction to Team 294
Programming Concepts
Ken Sterk
7/29/2007
7/18/2015
1
Goals and Objective
 To highlight the major software related physical
components and programming elements used in a
FRC robot
 To establish a basic understanding of the base FRC
code
 To understand the primary functions and algorithms
used by team 294
 To develop familiarity with the software compilation
process and the downloading process
7/18/2015
2
Software Elements
 How do human commands actually get to
robot?




Push joystick/Click button
???
Wheels move, Arm scores.
We win!!!!
7/18/2015
3
Software Elements cont.
 Well not quite. Lets expand upon “???”
 There are several major items between
Pushing the joystick and getting the robot to
move
 The OI (operators interface)
 The FRC robot controller
 The Motor controllers and relays and sensors
 Victors
 Spikes
 Encoders, potentiometers, etc
7/18/2015
4
Software Elements cont.
 The Software interacts with all of these
elements. Thus you can look at the robot
sort of like this..
Motors/pneumatics
Spike
Victor
Sensor
Operators
Interface
Radio
Robot
Controller
The Robot
7/18/2015
5
Software Elements cont.
 So now human commands get to the robot
through the following path
 Push joystick/Click button
 Signals sent from OI to robot controller via radio
 Robot controller
 ??? (oh noes not again…)
 Victors and spikes get signals and send power to
motors and pneumatics
 Wheels move… Arm scores…
 We win!!!!
7/18/2015
6
Robot Controller
 So what does the robot controller do?
 Essentially it’s the brain of the entire robot.
 The robot controller is simply a small
microprocessor.
 Using the inputs from it OI and readings from the
sensors the robot controller will determine the
commands to send the Victors and spikes based
upon the algorithms and logic we have written.
 It has multiple ports for communication and power
7/18/2015
7
Robot Controller
PWM outputs
Backup Battery
Main Power
Analog inputs
Programming Port
Digital I/O
Tether port
Radio Port
Reset + Program buttons
7/18/2015
8
Operators Interface
 That is the OI is an
integrated piece of
hardware that interprets
signals received from
analog/digital inputs such
as Joysticks, buttons, toggle
switches.
 It has the ability to send
data and receive data
 Receive data such as our
HUD (heads up display) or
error data.
 Sends joystick data, switch
values, etc.
7/18/2015
9
Operators Interface cont.
Competition
Port
Tether
Port
Radio
Port
Dashboard
viewer port
Port 3
Port 1
Port 2
Port 4
Reset
7/18/2015
LED Display
toggle
Team #
dipswitch
10
Victor 884 + Spike
The Victor 884 is a speed controller used
by the FRC robot controller board to
command the motors to run


It gets a PWM command from the robot
controller
Through its internal circuitry it generates
a current to provide the motors based
upon the input PWM value
 The Spike is essentially a small switch
that allows current to either pass in one
direction or another.


It can be used to drive small motors in
forward, reverse, or stop (brake).
Or more commonly it is used to drive
the solenoids we use in out pneumatic
system to open or close valves.
7/18/2015
11
Sensors
 There are several sensor types that have been used by team 294 in the
past few years.
 Briefly we have used:

Potentiometers



Encoders



Used to measure angular rotation, or angular rotational velocity.
Either magnetic or optical in construction, outputs a simple set of pulses per
revolution.
Rate Gyro



Simply a variable resistor that outputs a different voltage based upon what angular
position it is at.
Used to measure our angular position based upon a reference position.
Measures the angular rotational rate.
Useful for determining overall robot rotational rate
Camera

7/18/2015
Obviously it’s a camera. It outputs relative angles of the object that its looking at
based upon its calibrated color its looking for
12
Sensors cont.
 Potentiometers
CMU camera
 Encoder
7/18/2015
13
Getting Started with Code
 You need a computer….duh
 You will need a serial port or a USB to serial converter
 Download or install all the necessary programs
 MicroChip MPLAB:

This is the default FIRST C programming environment. This is where all of
our code will be written. We can open projects and modify the code here.
There are several different types of compilers and environments.
 C-18

This is the compiler to translate the written code into a language that is
recognizable to the embedded microchip in the robot controller. The compiler
is specifically tailored to compile the code to a specific set of microchips.
 IFI-Loader

This is the default software used to program the actual robot. It uses the
serial connection between your PC and the robot controller.
 The FRC Default source code

7/18/2015
The “source” is essentially a fancy word for all the logic that we are going to
edit and the put on the robot
14
Getting Started with Code
 C Code Hierarchy




.c -Source Files
.h -Header Files
.lib -Library Files
.lkr -Linker Scripts
 We will edit the .h
and .c files
 Compile and
download to the RC
7/18/2015
15
Working with Code
 We are primarily concerned with modifying only the
.c and .h files in the code.
 More specifically we are going to be modifying the
user_routines.c and user_routines.h file.
 Here we have code that enables us to read inputs from the
OI
 We can sample the inputs from the sensors
 We can the utilize this information and run our custom
functions (PID) to make the robot do what we want to do.
 To understand how this all works together we will
take a look at how all the code is put together
7/18/2015
16
Working with Code
 What is this .c and .h file?
 The .c file is considered the source file.
 This file is where the actual math is done. It is where
you will put all your functions.
 Eg. Motor1= (255-JoystickInput)*scale;
 The .h file is called the header file
 The header file is where we enumerate (define) all the
variables that we are going to be needing for the
functions called in the .c file
 It should have the same name as the .c file
7/18/2015
17
Working with code: Main.c
 Everything starts within the
top function called main.c
 This function simply calls all
the necessary functions
needed to run the robot.
 It first will initialize all the
variables that you have
defined
 It will the process all the
inputs
 It will run autonomous code
if needed
 And finally it will execute all
of your written algorithms
and write the data to the
output of the controller.
7/18/2015
/*******************************************************************************
* FUNCTION NAME: main
* PURPOSE:
Main program function.
* CALLED FROM:
ifi_startup.c
* ARGUMENTS:
none
* RETURNS:
void
* DO NOT DELETE THIS FUNCTION
*******************************************************************************/
void main (void)
{
#ifdef UNCHANGEABLE_DEFINITION_AREA
IFI_Initialization ();
/* DO NOT CHANGE! */
#endif
User_Initialization();
/* You edit this in user_routines.c */
statusflag.NEW_SPI_DATA = 0;
/* DO NOT CHANGE! */
while (1)
/* This loop will repeat indefinitely. */
{
#ifdef _SIMULATOR
statusflag.NEW_SPI_DATA = 1;
#endif
if (statusflag.NEW_SPI_DATA)
{
because */
/* 26.2ms loop area */
/* I'm slow! I only execute every 26.2ms
/* that's how fast the Master uP gives me
data. */
Process_Data_From_Master_uP();
if (autonomous_mode)
{
User_Autonomous_Code();
/* You edit this in user_routines.c */
/* DO NOT CHANGE! */
/* You edit this in user_routines_fast.c
*/
}
}
Process_Data_From_Local_IO();
/* You edit this in user_routines_fast.c */
/* I'm fast! I execute during every
loop.*/
} /* while (1) */
} /* END of Main */
18
Working with Default Code
/*******************************************************************************
* FUNCTION NAME: main
* PURPOSE:
Main program function.
* CALLED FROM:
ifi_startup.c
* ARGUMENTS:
none
* RETURNS:
void
* DO NOT DELETE THIS FUNCTION
*******************************************************************************/
void main (void)
{
#ifdef UNCHANGEABLE_DEFINITION_AREA
IFI_Initialization ();
/* DO NOT CHANGE! */
#endif
User_Initialization();
/* You edit this in user_routines.c */
statusflag.NEW_SPI_DATA = 0;
/* DO NOT CHANGE! */
while (1)
/* This loop will repeat indefinitely. */
{
#ifdef _SIMULATOR
statusflag.NEW_SPI_DATA = 1;
#endif
if (statusflag.NEW_SPI_DATA)
{
Process_Data_From_Master_uP();
if (autonomous_mode)
{
User_Autonomous_Code();
}
}
Process_Data_From_Local_IO();
}
7/18/2015
/*
/*
/*
/*
26.2ms loop area */
I'm slow! I only execute every 26.2ms because */
that's how fast the Master uP gives me data. */
You edit this in user_routines.c */
/* DO NOT CHANGE! */
/* You edit this in user_routines_fast.c */
/* You edit this in user_routines_fast.c */
/* I'm fast! I execute during every loop.*/
} /* while (1) */
/* END of Main */
19
User_Initialization
 This is where we
initialize all of the
variables to their
starting values
 Initialize PWM values
to send to victors as
neutral (127). More
on this later
 Initialize the digital
inputs and outputs
 Initialize the input and
output ports for use in
the code
7/18/2015
/*********************************************************************
**********
* FUNCTION NAME: User_Initialization
* PURPOSE:
This routine is called first (and only once) in the
Main function.
*
You may modify and add to this function.
* CALLED FROM:
main.c
* ARGUMENTS:
none
* RETURNS:
void
**********************************************************************
*********/
void User_Initialization (void)
{
Set_Number_of_Analog_Channels(SIXTEEN_ANALOG);
/* DO NOT CHANGE!
*/
/* FIRST: Set up the I/O pins you
digital_io_01 = digital_io_02 =
INPUT;
digital_io_05 = digital_io_06 =
INPUT;
digital_io_09 = digital_io_10 =
INPUT;
digital_io_18 = INPUT; /* Used
want to use as digital INPUTS. */
digital_io_03 = digital_io_04 =
digital_io_07 = digital_io_08 =
digital_io_11 = digital_io_12 =
for pneumatic pressure switch. */
/* SECOND: Set up the I/O pins you want to use as digital OUTPUTS. */
digital_io_17 = OUTPUT;
/* Example - Not used in Default Code. */
/* THIRD: Initialize the values on the digital outputs. */
rc_dig_out17 = 0;
/* FOURTH: Set your initial PWM values. Neutral is 127. */
pwm01 = pwm02 = pwm03 = pwm04 = pwm05 = pwm06 = pwm07 = pwm08 = 127;
pwm09 = pwm10 = pwm11 = pwm12 = pwm13 = pwm14 = pwm15 = pwm16 = 127;
20
Working with Default Code
/*******************************************************************************
* FUNCTION NAME: main
* PURPOSE:
Main program function.
* CALLED FROM:
ifi_startup.c
* ARGUMENTS:
none
* RETURNS:
void
* DO NOT DELETE THIS FUNCTION
*******************************************************************************/
void main (void)
{
#ifdef UNCHANGEABLE_DEFINITION_AREA
IFI_Initialization ();
/* DO NOT CHANGE! */
#endif
User_Initialization();
/* You edit this in user_routines.c */
statusflag.NEW_SPI_DATA = 0;
/* DO NOT CHANGE! */
while (1)
/* This loop will repeat indefinitely. */
{
#ifdef _SIMULATOR
statusflag.NEW_SPI_DATA = 1;
#endif
if (statusflag.NEW_SPI_DATA)
{
Process_Data_From_Master_uP();
if (autonomous_mode)
{
User_Autonomous_Code();
}
}
Process_Data_From_Local_IO();
}
7/18/2015
/*
/*
/*
/*
26.2ms loop area */
I'm slow! I only execute every 26.2ms because */
that's how fast the Master uP gives me data. */
You edit this in user_routines.c */
/* DO NOT CHANGE! */
/* You edit this in user_routines_fast.c */
/* You edit this in user_routines_fast.c */
/* I'm fast! I execute during every loop.*/
} /* while (1) */
/* END of Main */
21
Process_Data_From_masterUP
 The name sounds complicated but in reality it calls a
set of functions used to read in the info from the
camera.
 It also is used to process any other data from any
other sensors that need to be read
 But most importantly it calls the default_routine.
 And finally it will take all the data written by the
default routine and put the data on the output ports
of the robot controller.
7/18/2015
22
Default_routine
 The default routine is the basic code that is
used to control the robot.
 This is the location where team 294 does the
majority of the changes to the code.
 The default code is preloaded onto the Robot
Controller which provides a way to simply
build a robot and drive it around without
worrying too much about programming
7/18/2015
23
Default_routine
 Since the code is so long
only snippets of the code
will be shown.
 Essentially it does two
things
 Reads the OI values from
the joysticks and assigns
the correct PWM to each
wheel
 Maps button presses on
the joystick to the relay
outputs on the robot
controller
7/18/2015
/*******************************************************************************
* FUNCTION NAME: Default_Routine
* PURPOSE:
Performs the default mappings of inputs to outputs for the
*
Robot Controller.
* CALLED FROM:
this file, Process_Data_From_Master_uP routine
* ARGUMENTS:
none
* RETURNS:
void
*******************************************************************************/
void Default_Routine(void)
{
/*---------- Analog Inputs (Joysticks) to PWM Outputs----------------------*-------------------------------------------------------------------------*
This maps the joystick axes to specific PWM outputs.
*/
pwm01 = p1_y;
pwm02 = p2_y;
pwm03 = p3_y;
pwm04 = p4_y;
pwm05 = p1_x;
pwm06 = p2_x;
pwm07 = p3_x;
pwm08 = p4_x;
pwm09 = p1_wheel;
pwm10 = p2_wheel;
pwm11 = p3_wheel;
pwm12 = p4_wheel;
/*---------- Buttons to Relays---------------------------------------------*-------------------------------------------------------------------------* This default code 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 C symbol for AND
*/
relay1_fwd = p1_sw_trig & rc_dig_in01; /* FWD only if switch1 is not closed.
relay1_rev = p1_sw_top & rc_dig_in02; /* REV only if switch2 is not closed.
relay2_fwd = p2_sw_trig & rc_dig_in03; /* FWD only if switch3 is not closed.
relay2_rev = p2_sw_top & rc_dig_in04; /* REV only if switch4 is not closed.
relay3_fwd = p3_sw_trig;
relay3_rev = p3_sw_top;
relay4_fwd = p4_sw_trig;
relay4_rev = p4_sw_top;
relay8_fwd = !rc_dig_in18; /* Power pump only if pressure switch is off. */
relay8_rev = 0;
*/
*/
*/
*/
24
Working with Default Code
/*******************************************************************************
* FUNCTION NAME: main
* PURPOSE:
Main program function.
* CALLED FROM:
ifi_startup.c
* ARGUMENTS:
none
* RETURNS:
void
* DO NOT DELETE THIS FUNCTION
*******************************************************************************/
void main (void)
{
#ifdef UNCHANGEABLE_DEFINITION_AREA
IFI_Initialization ();
/* DO NOT CHANGE! */
#endif
User_Initialization();
/* You edit this in user_routines.c */
statusflag.NEW_SPI_DATA = 0;
/* DO NOT CHANGE! */
while (1)
/* This loop will repeat indefinitely. */
{
#ifdef _SIMULATOR
statusflag.NEW_SPI_DATA = 1;
#endif
if (statusflag.NEW_SPI_DATA)
{
Process_Data_From_Master_uP();
if (autonomous_mode)
{
User_Autonomous_Code();
}
}
Process_Data_From_Local_IO();
}
7/18/2015
/*
/*
/*
/*
26.2ms loop area */
I'm slow! I only execute every 26.2ms because */
that's how fast the Master uP gives me data. */
You edit this in user_routines.c */
/* DO NOT CHANGE! */
/* You edit this in user_routines_fast.c */
/* You edit this in user_routines_fast.c */
/* I'm fast! I execute during every loop.*/
} /* while (1) */
/* END of Main */
25
Autonomous code
 This is the special code
position where we will put
out autonomous
functions.
 Only used when
autonomous time is valid.
 The default autonomous
code does nothing
 This runs in the
user_routines_fast file
/*******************************************************************************
* FUNCTION NAME: User_Autonomous_Code
* PURPOSE:
Execute user's code during autonomous robot operation.
* You should modify this routine by adding code which you wish to run in
* autonomous mode. It will be executed every program loop, and not
* wait for or use any data from the Operator Interface.
* CALLED FROM:
main.c file, main() routine when in Autonomous mode
* ARGUMENTS:
none
* RETURNS:
void
*******************************************************************************/
void User_Autonomous_Code(void)
{
/* Initialize all PWMs and Relays when entering Autonomous mode, or else it
will be stuck with the last values mapped from the joysticks. Remember,
even when Disabled it is reading inputs from the Operator Interface.
*/
pwm01 = pwm02 = pwm03 = pwm04 = pwm05 = pwm06 = pwm07 = pwm08 = 127;
pwm09 = pwm10 = pwm11 = pwm12 = pwm13 = pwm14 = pwm15 = pwm16 = 127;
relay1_fwd = relay1_rev = relay2_fwd = relay2_rev = 0;
relay3_fwd = relay3_rev = relay4_fwd = relay4_rev = 0;
relay5_fwd = relay5_rev = relay6_fwd = relay6_rev = 0;
relay7_fwd = relay7_rev = relay8_fwd = relay8_rev = 0;
while (autonomous_mode)
/* DO NOT CHANGE! */
{
if (statusflag.NEW_SPI_DATA)
/* 26.2ms loop area */
{
Getdata(&rxdata);
/* DO NOT DELETE, or you will be stuck here forever!
*/
/* Add your own autonomous code here. */
Generate_Pwms(pwm13,pwm14,pwm15,pwm16);
Putdata(&txdata);
/* DO NOT DELETE, or you will get no PWM outputs! */
}
}
}
7/18/2015
26
Working with Default Code
/*******************************************************************************
* FUNCTION NAME: main
* PURPOSE:
Main program function.
* CALLED FROM:
ifi_startup.c
* ARGUMENTS:
none
* RETURNS:
void
* DO NOT DELETE THIS FUNCTION
*******************************************************************************/
void main (void)
{
#ifdef UNCHANGEABLE_DEFINITION_AREA
IFI_Initialization ();
/* DO NOT CHANGE! */
#endif
User_Initialization();
/* You edit this in user_routines.c */
statusflag.NEW_SPI_DATA = 0;
/* DO NOT CHANGE! */
while (1)
/* This loop will repeat indefinitely. */
{
#ifdef _SIMULATOR
statusflag.NEW_SPI_DATA = 1;
#endif
if (statusflag.NEW_SPI_DATA)
{
Process_Data_From_Master_uP();
if (autonomous_mode)
{
User_Autonomous_Code();
}
}
Process_Data_From_Local_IO();
}
7/18/2015
/*
/*
/*
/*
26.2ms loop area */
I'm slow! I only execute every 26.2ms because */
that's how fast the Master uP gives me data. */
You edit this in user_routines.c */
/* DO NOT CHANGE! */
/* You edit this in user_routines_fast.c */
/* You edit this in user_routines_fast.c */
/* I'm fast! I execute during every loop.*/
} /* while (1) */
/* END of Main */
27
Process_Data_From_Local_IO
 Another really long name that does not do anything
too complicated
 All it does is process data every program loop
 So if you have something that needs to be executed
every single program loop this is where you will put
it
 The only thing team 294 has running every loop is
the autonomous code
 We have also experimented in the past with using
interrupts for reading in inputs from other sensors
7/18/2015
28
Changes to the Default Code
 Team 294 does make some changes to the default
code
 These changes enable us to do much more
complicated things.
 We have code to drive the arm linkages by using a PID
algorithm.
 Operate the gripper to effectively grab game pieces and
score points
 Autonomous code to score during the autonomous periods
 In addition we have cleaned up some of the initializations to
make the code more legible and intuitive
7/18/2015
29
294 Code Changes
 All these changes exist with in the files we
just covered before.
 The files we have changed are
 User_routines
 We wrote our own default_routine. Now called
control_routine
 User_routines_fast
 We have added our own autonomous code
7/18/2015
30
Control_routine
 This is the code used for the
2007 robot
 For the control routine to work
we needed to first add
initialization to the arm motors.
Hence we have added this line
to the User_initialization
function
7/18/2015
/* Add any other initialization code here. */
//initialize pid
pid_rotate_init();
pid_drive_init();
pid_shoulder_init();
pid_elbow_init();
31
Control_routine cont.
 Now onto the bulk of what we have done
 What we are attempting to do is take in commands
from the OI and make the robot do what we want to
do
 First: Drive all 6 motors based upon the commands sent by
the joystick
 Second: Move the arm to a predefined position that we
want to go.
 E.g. Pick up off ground, Score, descore. Etc
 Operate the pneumatics to open and close the gripper to
pick up the game piece
 So lets take a look at that your teammates wrote
7/18/2015
32
Control_routines cont.
 First we define all the internal
variables to the function.
 Eg. The grabber state, the arm
command, etc
 Next read in the actual arm
positions from the sensors
(potentiometers)
 Then set the arm pwms to 127
 A pwm of 127 is considered to
be zero motion
 The pwm ranges from 0-255
 0 being full reverse and 255 full
forward.
#include
#include
#include
#include
#include
#include
<stdio.h>
"control_routine.h"
"lightpid.h"
"ifi_aliases.h"
"ifi_default.h"
"ifi_utilities.h"
unsigned char autonomous_mode_choice;
switch
//converted value from rotary
void Controls_Routine()
{
static int elbow_arm_cmd_tmp = 0;
static unsigned char grabber_counter = 0;
static unsigned char init_state = 0;
static unsigned char grabber_state; //0 open, 1 closed
static int shoulder_arm_pos;
static int elbow_arm_pos;
static int shoulder_arm_cmd;
static int elbow_arm_cmd;
static unsigned char autonomous_mode_overlord = 0; //makes it so
only runs once
int left_tmp, right_tmp;
//Get arm positions
shoulder_arm_pos = Get_Analog_Value(SHOULDER_POT);
elbow_arm_pos = Get_Analog_Value(ELBOW_POT);
pwm01 = pwm02 = pwm03 = pwm04 = pwm05 = pwm06 = pwm07 = 127; //Zero
PWMS
7/18/2015
33
Control_routines cont.
 Next we have modified the drive
code to drive all 6 motors we
have on the robot.
 If you notice that the names are
different than calling it pwm01,
pwm02


The reason for this is to make it
simple to read the code.
The actual mapping of the
“LEFT_DRIVE_CIMS” is located
in the header file!
//
//
//----------------------------Drive Code--------------------//
LEFT_DRIVE_CIMS = DRIVER_LEFT; //non speed reduction
RIGHT_DRIVE_CIMS = DRIVER_RIGHT; //non speed reduction
if(DRIVER_LEFT_TOP == 1)
{
LEFT_DRIVE_CIMS = 127 + ((int)DRIVER_LEFT 127)/(int)DRIVE_SPEED_REDUCTION_FACTOR;
RIGHT_DRIVE_CIMS = 127 - ((int)DRIVER_RIGHT 127)/(int)DRIVE_SPEED_REDUCTION_FACTOR;
}
else
{
LEFT_DRIVE_CIMS = DRIVER_LEFT;
RIGHT_DRIVE_CIMS = 255 - DRIVER_RIGHT;
}
 We have also included the
ability to let the operated reduce
the top speed of the robot for a
fine control of the driving
7/18/2015
34
Control_routines cont.
 Next is the arm commands
 In 2007 we had two ways to control the arm
 First we could use a “fine tune” using the codrivers joystick
 Or we could use the preset positions for pickup,
score low, score high etc.
 These used the toggle switches on the on the control
board
7/18/2015
35
Control_routine cont.
 Fine control. Takes the
input from the joystick and
issues a new command to
the arm (either shoulder or
elbow) in small increments
 This calls the PID function
to be reviewed later
 Again notice the variables
names to be intuitive and
easy to understand
 Also comments are really
useful to separate out the
code
7/18/2015
//----------------------------Shoulder Fine Tune-----------//
if(SHOULDER_FINE_TUNE < 51 && SHOULDER_FINE_TUNE > 47)
{
SHOULDER_FP = SHOULDER_BB = 127;
}
else if(SHOULDER_FINE_TUNE > 245)
{
shoulder_arm_cmd += 5;
}
if(SHOULDER_FINE_TUNE < 160 && SHOULDER_FINE_TUNE > 130)
{
shoulder_arm_cmd -= 5;
}
//---------------------------Elbow Fine Tune------//
if(ELBOW_FINE_TUNE > 134 || ELBOW_FINE_TUNE < 110)
{
//we have moved the joystick
if(elbow_arm_cmd_tmp == 0)
{
elbow_arm_cmd_tmp = elbow_arm_pos;
}
elbow_arm_cmd = elbow_arm_cmd_tmp + ((int)ELBOW_FINE_TUNE 127); // /(int)ELBOW_FINE_TUNE_REDUCTION;
}
else
{
elbow_arm_cmd_tmp = 0;
ELBOW_FP = 127;
}
//------------------------End ARM Fine Tune--------------//
36
Control_routine cont.




Using preset positions.
Only one shown for brevity
Sets the command to a predefined position
Positions set via calibration before the match
//-----------------------------PICKUP--------------------//
else if(RIGHT_PRESETS > 193)
{
if(LEFT_PRESETS > 242) //ground
{
shoulder_arm_cmd = GROUND_SHOULDER_ARM_EXE;
elbow_arm_cmd = GROUND_ELBOW_ARM_EXE;
}
else if(LEFT_PRESETS > 216) //wall
{
shoulder_arm_cmd = WALL_SHOULDER_ARM;
elbow_arm_cmd = WALL_SHOULDER_ARM;
}
}
7/18/2015
37
Control_routine cont.
 This is the code to
effectively run the gripper.
It will open and close based
upon the drivers
commands
//----------------------------GRABBER CONTROL-----------------------//
if(DRIVER_LEFT_TRIGGER == 1) //open grabber
{
if(grabber_counter >= 5)
{
grabber_state = 1;
grabber_counter = 0;
}
grabber_counter++;
}
else if(DRIVER_RIGHT_TRIGGER == 1) //close grabber
{
grabber_state = 0;
}
if(GRABBER_OVERRIDE
== 1)
{
grabber_state = 1 - grabber_state;
}
if(grabber_state == 1)
{
GRABBER = 1;
GRABBER_REV = 0;
}
else
{
GRABBER = 0;
GRABBER_REV = 1;
}
}
7/18/2015
38
control_routine
 Finally the arm command has been set, the gripper
is either set to open and close
 But now we need to send the arm commands to the
motors.
 We do this through a function called PID
 What PID is a very effective method to write the PWM
commands to the motors.
 It uses the error between the arms current location and
commanded position to determine how fast it should go
 There is a ton of theory here that we can go over another time
 The PWM are then written by the PID function and
sent to the arm motors
 That then finishes off the control_routine!
7/18/2015
39
Autonomous code
 In the user_routine_fast file simply add in the
code you want to execute under the
autonomous
 In 2007, we never got autonomous code fully
tested due to a variety of other problems
 In 2006 we had a fairly effective autonomous
code that could select between several
options to score
 In 2005 we had a very simple set of code to
score a couple points consistently
7/18/2015
40
Wrapping it up…
 So we have covered:
 Some of the basic elements that are used as
interfaces between our code and the actual robot
 The tools necessary to build and compile code
onto the robot
 A quick (hopefully) overview of what the default
code looks like and how it runs
 Finally, the changes made to the default code by
team 294
7/18/2015
41