An Introduction to FIRST Robotics Programming in C

If you want to learn to program FIRST FRC robots in C, then this guide is for you. This guide assumes that you are already familiar with C or a similar language (like C++ or Java).

This document was written by Andrew Merrill of Team 1540 from the Catlin Gabel School, and is made available under the Creative Commons Attribution-Noncommercial-Share Alike license.

Contents:

Overview
Reference Documents
Software to Install
Programming Your Robot
Reading Inputs
Setting Outputs
Debugging Aids
Data Types
Customizing IFI's Code
Building Disable and Autonomous Switches
Suggested First Programming Projects
Further Resources

Overview

The robot is directly controlled by two on-board PIC microcontrollers, both of which are inside the FRC Robot Controller black box. One of those microcontrollers, called the User Processor, runs software that you will be writing. The other, called the Master Processor, runs low-level firmware provided by IFI. You write the robot software on a personal computer, compile it, and download it to the robot via a serial cable. Once your software has been downloaded to the robot, you no longer need your personal computer (though it can be helpful for debugging).

The robot can run in either Autonomous Mode, where it is completely on its own, or Remote Control Mode, where a human driver directs the robot. Recent FIRST competitions have involved a short autonomous period (10-20 seconds) followed by a longer remote controlled period (2 minutes).

During Remote Control Mode, the driver directs the robot using joysticks (and/or other devices) attached to the Operator Interface. The operator interface communicates with the Robot Controller via a wireless radio connection. The radio connection is invisible to the programmer and all of the communication details are handled by the provided firmware.

Your will be writing code that is run about every 26 milliseconds on the robot. The primary purpose of your code will be to check inputs (the driver's joysticks and on-board sensors) and decide what values to send to several outputs (such as motors and relays).

Reference Documents

You will want to carefully read the following documents from IFI Robotics:

These links are to the most recent version of each document available as of October, 2007.

Software to Install

You will need the following software programs:

Microchip MPLAB IDE version 7.2 (included in the FIRST Kit of Parts, or available from IFI for $50)
Microchip C18 Compiler version 2.4 (included with MPLAB)
IFI Loader version 1.1.0

used to download your program to the robot
do not use the older version included on the CD from FIRST or IFI - it is out of date
download IFI Loader version 1.1.0 from IFI

IFI Dashboard Viewer

optional, but a great debugging tool
download the Dashboard Viewer from IFI
read more about the Dashboard in the Debugging Aids section later in this document

The IFI Default Robot Program

this is the code that you will customize to run your robot
download the 2007 RC Default Code from IFI
check out the tips for customizing the IFI code later in this document

Note: You cannot just go and use your favorite C compiler or environment that you are already familiar with. You must use a cross-compiler that targets the PIC microcontroller.

Programming Your Robot

Before you get started, you will need to do some first time setup.

Install the MPLAB IDE (version 7.2) and C18 Compiler (version 2.4) from the CD.
Install IFI Loader version 1.1.0 after downloading it from IFI.
Create a new directory called C:\Robotics (or something similar) on your PC that will hold all of the robotics software you write.

Warning: The C18 Compiler cannot handle combined directory and file names longer than 64 characters.
Do not try to store your robotics software in My Documents or the Desktop or anywhere else with a long pathname.

Download the 2007 RC Default Code from IFI and unzip it into C:\Robotics to make a C:\Robotics\FrcCode_2007_8722 directory.
Optional: Follow the directions below for customizing IFI's code.
For each new project that you are going to work on, make a copy of the FrcCode_2007_8722 directory.

To write and edit your programs:

Run MPLAB (Start -> All Programs -> Microchip -> MPLAB IDE v7.20 -> MPLAB IDE)
From the Project menu, select Open….
Navigate to the C:\Robotics\FrcCode_2007_8722 folder (or the copy that you made) and select the FrcCode.mcp project file.
To compile your program, from the Project Menu select either Make or Build All.

Where to write your code:

If you are decide to directly edit IFI's functions:

Initialization code file user_routines.c function User_Initialization()
Remote Control code file user_routines.c function Process_Data_From_Master_uP()
Autonomous code file user_routines_fast.c function User_Autonomous_Code()

If you decide to follow my tips for customizing IFI's code:

Initialization code file custom_routines.c function Custom_Initialization()
Remote Control code file custom_routines.c function Custom_Remote_Control()
Autonomous code file custom_routines.c function Custom_Autonomous()

To download your program to your robot:

Remember to compile your program first! (Make or Build All)
Turn on your robot
On the Robot Controller, hold down the PROG button for 1-2 seconds until the Program State LED turns orange
Use a serial cable to connect the Program port on the Robot Controller to your PC

If your PC does not have a serial port, then you will need to use a USB to Serial converter
We have found that the USB to Serial converters are 3-6 times slower than a dedicated PC serial port

Run the IFI Loader program (Start -> All Programs -> IFI_Loader -> IFI_Loader)
Select the correct serial port from the PortSettings menu
Click on Browse and navigate to the FrcCode.hex file in the directory that you are working in
Click on Download to download your program to the robot

Your program will start to run immediately after the download is complete. So, make sure it waits for user input before starting the motors, or you may be in for an unpleasant surprise! You may want to build a disable switch.

Reading Inputs

There are several different sources of input data available to your robot programs. Some of your input will come from devices attached to the Operator Interface, and some will come from devices directly attached to the Robot Controller. Each input is accessed by referring to a predefined global variable, the names of which are indicated below.

Operator Interface Inputs

The Operator Interface has 4 joystick ports, each of which can be connected to a different joystick. Each joystick provides a total of 8 different input values:

Name	Port 1 Variable	Port 2 Variable	Port 3 Variable	Port 4 Variable	Range	Description
X Axis	p1_x	p2_x	p3_x	p4_x	0 to 255	Horizontal position of the joystick. 0 = full right; 127 = center; 255 = full left
Y Axis	p1_y	P2_y	p3_y	P4_y	0 to 255	Vertical position of the joystick. 0 = full back; 127 = center; 255 = full forward
Wheel	p1_wheel	p2_wheel	p3_wheel	p4_wheel	0 to 255	joystick dependent
Aux	p1_aux	p2_aux	p3_aux	p4_aux	0 to 255	joystick dependent
Trigger Switch	p1_sw_trig	p2_sw_trig	p3_sw_trig	p4_sw_trig	0 or 1	0 = not pressed; 1 = pressed
Thumb Switch	p1_sw_top	p2_sw_top	p3_sw_top	p4_sw_top	0 or 1	0 = not pressed; 1 = pressed
Aux Switch 1	p1_sw_aux1	p2_sw_aux1	p3_sw_aux1	p4_sw_aux1	0 or 1	0 = not pressed; 1 = pressed
Aux Switch 2	p1_sw_aux2	p2_sw_aux2	p3_sw_aux2	p4_sw_aux2	0 or 1	0 = not pressed; 1 = pressed

Keep in mind that most joysticks are not perfect. It is quite likely that your joystick will read something a little bit off from 127 when centered, and it may not be able to reach all the way to the 0 and 255 extremes. In your code, you will probably want to maintain a "dead zone" around 127, so that you treat any value within, say, 5 or 10 units of 127 is treated as if it were exactly 127.

Note that you are welcome to connect your own switches and controls to one or more of the Operator Interface joystick ports, so you can build your own custom control boards.

Robot Controller Inputs

The Robot Controller has two types of inputs: Analog Inputs and Digital Inputs.

· Analog Inputs

· The Robot Controller provides 16 analog inputs

· Each provides an integer in the range 0 to 1023 (10 bit resolution). Each analog input is measuring a voltage than can range from 0V to 5V, where 0V is read in software as a 0 and 5V is read in software as 1023.

· Analog inputs are used for sensors that generate a wide range of values, such as accelerometers, gyros, range finders, compasses, etc.

· The analog input variables are called rc_ana_in01, rc_ana_in02, …, rc_ana_in16.

· To read these variables you must call the Get_Analog_Value function and pass it the analog input variable that you want to read from. For example:
mysensor = Get_Analog_Value(rc_ana_in01);

· Digital Inputs

· The Robot Controller provides 18 digital IO pins, each of which can be used as a digital input.

· Each provides a 0 or 1 value. Note that the digital inputs default to 1, so you will often write code that takes action when a digital input goes to 0.

· Digital inputs are used for sensors that have exactly two states, such as open/closed, on/off, etc., like a limit switch or an electric eye.

· The digital input variables are called rc_dig_in01, rc_dig_in02, …, rc_dig_in18.

· Unlike the robot controller analog inputs, these variables can be read directly without calling a conversion function.

· Configuring Digital Inputs

· Each of the 18 digital IO pins on the robot controller can be configured for either input or output (but not both).

· Before using a robot controller digital pin for input, you must configure that pin as an input pin.

· This only needs to be done once, when your program is initialized.

· Each pin has a predefined global variable called digital_io_01, digital_io_02, …, digital_io_18 that must be assigned the value INPUT. For example:
digital_io_01 = INPUT;

Setting Outputs

There are three types of outputs your program can set to control your robot: PWMs, Digital Outputs, and Relay Outputs.

PWM Outputs

PWM (Pulse Width Modulation) signals are normally used to control motors.
The Robot Controller provides 16 PWM outputs.
Each can be set to a value in the range of 0 to 255, which indicate the speed and direction of the motor.

255 means full speed in one direction
127 means stopped
0 means full speed in the other direction.

The variable names for the PWM Outputs are pwm01, pwm02, …, pwm16.
Note that it is very likely that the motors on the right side of your robot will be mounted in the opposite orientation of the motors on the left. So, in order to drive your robot forward, you will want to set the PWM outputs connected to one side to values greater than 127 and the PWM outputs connected to the other side to values less than 127.
Also note that due to friction, inertia, etc., power values close to 127 will not actually move your robot, but will probably make the motors buzz as they try to turn but are unable to do so.

Digital Outputs

The Robot Controller provides 18 digital IO pins, each of which can be used as a digital output.
Each can be set to a 0 or 1 value.
The digital output variables are called rc_dig_out01, rc_dig_out02, …, rc_dig_out18.
Configuring Digital Inputs

Each of the 18 digital IO pins on the robot controller can be configured for either input or output (but not both).
Before using a robot controller digital pin for output, you must configure that pin as an output pin.
This only needs to be done once, when your program is initialized.
Each pin has a predefined global variable called digital_io_01, digital_io_02, …, digital_io_18 that must be assigned the value OUTPUT. For example:
digital_io_01 = OUTPUT;

Relay Outputs

The Robot Controller provides 8 relay outputs, each of which can be connected to a Spike Relay.
Each Spike Relay has two outputs, labeled M+ and M-, which can be controlled independently.
The relay variables are called relay1_fwd, relay1_rev, …, relay8_fwd, relay8_rev.
The _fwd variable sets the M+ relay output
The _rev variable sets the M- relay output
Each variable can be set to the values 0 and 1, where 0 means ground and 1 means 12V power.
Relays are very handy for controlling pneumatic systems on your robot.

Debugging Aids

Debugging a robot control program is a good deal trickier than debugging a program on a traditional computer. Among other problems, there are limited ways for you to generate debugging output as your program is running. The three primary options are Operator Interface LEDs, the Dashboard, and Serial Print statements.

Operator Interface LEDs

The Operator Interface box provides 11 LED lights labeled "ROBOT FEEDBACK" that you can set.
The variables that control the LEDs are:

Pwm1_green (also sets Operator Interface Joystick Port 1, pin 15)
Pwm1_red (also sets Operator Interface Joystick Port 1, pin 8)
Pwm2_green (also sets Operator Interface Joystick Port 1, pin 9)
Pwm2_red (also sets Operator Interface Joystick Port 1, pin 5)
Relay1_green (also sets Operator Interface Joystick Port 3, pin 15)
Relay1_red (also sets Operator Interface Joystick Port 3, pin 8)
Relay2_green (also sets Operator Interface Joystick Port 3, pin 9)
Relay1_red (also sets Operator Interface Joystick Port 3, pin 5)
Switch1_LED
Switch2_LED
Switch3_LED

Despite the misleading names, these LEDs have no connection to PWMs, Relays, or Switches.
However, the LEDs are labeled on the Operator Interface with labels that correspond to the variable names.
Setting an LED variable to 1 will light up the corresponding LED; setting it to 0 turns it off.
Note that most of these variables also set pins on the OI joystick ports, so you can attach custom LED displays or buzzers to them.

Dashboard

IFI provides a program called the Dashboard Viewer that runs on a Windows PC
The PC must be connected by a serial cable to the Dashboard port on the Operator Interface
The Dashboard can display:

The current values of all of the Operator Interface joystick inputs
The current values of all of the Robot Controller PWM outputs
The current status of the Operator Interface LEDs
Six bytes that you can set in your program, called user bytes

§ The user bytes are the best way to display a numeric variable while your program is running

§ The user byte variables are User_Byte1, User_Byte2, …, User_Byte6.

§ Since each is one byte, it can display a number between 0 and 255 only.

§ Since an int is two bytes, it takes two user bytes to display each int. For example:

§ int counter;
User_Byte1 = counter / 256;
User_Byte2 = counter % 256;

Download the Dashboard Viewer from IFI.

Serial Printing

Your program can send debugging output back over the same serial connection used to download your program to the robot.
When the IFI Loader finishes downloading your program, it opens a window on your PC to display the output.
This only works if you leave the serial cable connected to the PC and the Program port on the Robot Controller.
The following commands are available:

printf, which works like it normally does in C
PrintByte, which takes one unsigned char and displays it in hexadecimal
PrintWord, which takes one unsigned int and displays it in hexadecimal

Warnings:

Our team has found that the use of printf seems to cause difficult to diagnose bugs that go away when the printf statements are commented out.
We avoid the use of printf.
We have not experimented with PrintByte and PrintWord to see if they suffer from similar problems.
We use the Dashboard for all routine debugging.

Data Types

Here are some of the more commonly used data types provided by the C18 Compiler:

Name	Kind	Size	Minimum Value	Maximum Value
char	integer	8 bits	-128	127
unsigned char	integer	8 bits	0	255
int	integer	16 bits	-32,768	32,767
unsigned int	integer	16 bits	0	65,535
long	integer	32 bits	-2, 147, 483, 648	2, 147, 483, 647
unsigned long	integer	32 bits	0	4, 294, 967, 295
float	floating point	32 bits	about -10³⁸	about 10³⁸
double	floating point	32 bits	about -10³⁸	about 10³⁸

The predefined global variables like p1_x and pwm01 are all unsigned chars, with a range from 0 to 255.
You need to be very careful to avoid overflowing the range of these variables when performing arithmetic with them.
For example, if you wanted (for some reason) to compute the average of p1_x and p2_x, the simple approach fails

Simple approach that does not work:

average = (p1_x + p2_x) / 2;

Two better approaches:

average = ( ((int) p1_x) + ((int) p2_x) ) / 2;
int p1_x_int = (int) p1_x;
int p2_x_int = (int) p2_x;
average = (p1_x + p2_x) / 2;

The range for ints is much smaller than you may be used to when programming traditional computers.
There is no difference between the float and double types.
Floating point math is handled by a software library, so it is pretty slow.

Customizing IFI's Code

The code that IFI provides has a few problems that you may want to resolve before you start programming your robot.

IFI expects you to insert your code in the middle of functions that they have written, making it difficult to follow and maintain.
IFI includes "default code" that connects certain outputs to certain inputs in ways that you probably don't want.
IFI includes code that checks for tripped circuit breakers, something that isn't currently supported by FIRST robots.
IFI includes code that prints messages on your console, which slows down your program and may interfere with your own debugging.

So, here we are going to fix all of these problems.

First you should download the 2007 IFI Robot Code from the IFI Robot Controller web page.
Unzip the archive into your robotics working directory (I used C:\Robotics) to make the directory C:\Robotics\FrcCode_2007_8722.
Make a copy of this directory (so that you can always go back to the original) called C:\Robotics\FrcCode_2007_8722_Custom
Start MPLAB
From the Project menu, select Open….
Navigate to the C:\Robotics\FrcCode_2007_8722_Custom folder and select the FrcCode.mcp project file.

Start a new file, save it as custom_routines.h, and enter the following lines in it:

void Custom_Initializaion(void);

void Custom_Remote_Control(void);

void Custom_Autonomous(void);

Start a new file, save it as custom_routines.c, and enter the following lines in it:

#include <stdio.h>

#include "ifi_aliases.h"

#include "ifi_default.h"

#include "ifi_utilities.h"

#include "user_Serialdrv.h"

#include "custom_routines.h"

void Custom_Initialization(void)

{

}

void Custom_Remote_Control(void)

{

}

void Custom_Autonomous(void)

{

}

From the Project menu, select Add Files to Project and select both of the files that you just created.
Now open the file user_routines_fast.c and add the following red-colored lines. Note that the given line numbers are approximate.

Line

Code (existing code in gray, new code in red)

#include "user_routines.h"

#include "user_Serialdrv.h"

#include "custom_routines.h"

111

112

/* Add your own autonomous code here. */

Custom_Autonomous();

Now open the file user_routines.c and make the following changes. Again, note that the given line numbers are approximate.

Line

Code (existing code in gray, new code in red, code to remove or comment out is in strikethrough)

#include "user_routines.h"

#include "user_Serialdrv.h"

#include "custom_routines.h"

124

digital_io_17 = digital_io_18 = INPUT;

135

136

137

138

139

~~/* 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;~~

163

164

/* Add any other initialization code here. */

Custom_Initialization();

Still in the file user_routines.c, delete the function Process_Data_From_Master_uP and replace it with this version:

void Process_Data_From_Master_uP(void)

{

Getdata(&rxdata); /* Get fresh data from the master microprocessor. */

Custom_Remote_Control();

Generate_Pwms(pwm13,pwm14,pwm15,pwm16);

Putdata(&txdata); /* DO NOT CHANGE! */

}

Now you can do all of your programming in the custom_routines.c file:

Intialization code goes in the Custom_Initialization function.
Remote Control mode code goes in the Custom_Remote_Control function.
Autonomous mode code goes in the Custom_Autonomous function.

Building Disable and Autonomous Switches

We strongly recommend building a switchbox to allow you to use the Disable and Autonomous features of the robot.

· While disabled your program will still run, but all Robot Controller outputs will not affect your robot

· You will need two toggle switches, a small project box, and a cable that terminates in a DB15 connector.

· Your switchbox will connect to the COMPETITION port on the Operator Interface

· Follow the directions on pp. 11-2 of the IFI Operator Interface Reference Guide and the Competition Port Pinout Guide

Write software to allow the driver to drive straight forward and straight backward using the Y axis of joystick. Can you keep the robot from shaking or buzzing when the joystick is idle?
Write two functions, one to drive the right motors, and one to drive the left motors.

· Each function should take as an input a single int, where 255 means drive forward, 127 means stop, and 0 means drive backward.

· Your functions should clamp their input to the required range (for example, any values over 255 get clamped down to 255).

· Your functions should enforce a dead-zone around 127 (so any input values close enough to 127 are converted to exactly 127).

· Your functions should correct for the fact that the motors on one side of your robot probably need to be run in the opposite direction from the other.

· Your functions should cast the input number to an unsigned char before using it to set the appropriate PWM outputs.

· Optional alternative: take an input in a different range that you like better (like -127 to 127, or -1000 to 1000) and convert it into the standard PWM range.

Write software to support two-joystick "tank style" driving, where the Y axis of one joystick controls the left wheels, and the Y axis of a second joystick controls the right wheels.
Write software to support single-joystick drive, where the Y axis controls forward/backward motion and the X axis controls turning.

Further Resources

IFI Robotics
Kevin Watson - a programmer at NASA who has provided great sample code for using the camera, interrupts, encoders, etc.
Chief Delphi Forums - the most active FIRST discussion boards

check out the Programming Forum on Chief Delphi

FIRST Forums - the official Q&A source from the FIRST officials