WPILib Sample Programs
Here are a few sample programs to give you the idea of what the library can do. There is extensive documentation that is available that describes the functions in greater detail.
There are sample MPLab projects included with each of the samples. These samples assume that the WPILib code is installed in C:\WPILib. If it is installed somewhere else the projects will need to be modified to reference the libraries and linker file.
Driving towards green target with camera
This project shows how WPILib will help automatically respond to the field controls. You can simulate the field controls with a dongle connected to the competition port on the Operator Interface. The MPLab project for this can be found here.
#include "BuiltIns.h"
/*
* This function must
be here for a competition project. It is
* automatically referenced by
WPILib at startup and run. At that
* point the SetCompetitionMode function
sets the competition
* mode. Basically, a mode of 0 is the default (without
the
* IO_Initialization function) and runs a main function only.
*
SetCompetitionMode(1) runs a competition project as shown.
*/
void
IO_Initialization(void)
{
SetCompetitionMode(1);
}
/*
*
Initialize is run immediately when the robot is powered on
* regardless of
the field mode.
*/
void Initialize(void)
{
int i;
for (i = 0; i < 10; i++)
{
printf("Initialize %d\r", i);
Wait(500);
}
}
/*
* Autonomous is
run as soon as the field controls enable the
* robot. At the end of the
autonomous period, the Autonomous
* function will end (note: even if it is
in an infinite loop
* as in the example, it will be stopped).
*/
void
Autonomous(void)
{
while (1)
{
printf("In autonomous\r");
Wait(500);
}
}
/*
* The
OperatorControl function will be called when the field
* switches to
operator mode. If the field ever switches back
* to autonomous, then
OperatorControl will automatically exit
* and the program will transfer
control to the Autonomous
* function.
*/
void OperatorControl(void)
{
while (1)
{
printf("In OperatorControl\r");
Wait(500);
}
}
/*
* the main
program is not used, but needs to be here to
* statisfy a reference to it.
This requirement will probably
* go away in the next version of WPILib.
*/
void
main(void)
{
}
This example drives for exactly 238 teeth on the sensor. The actual distance driven depends on the gear that the sensor is connected to, the wheel diameters, and the gear ratios. The sample program is available here.
This program will drive in a straight line, correcting for bumps or crashes into obstacles. It is ideal for driving in autonomous mode and ensuring that the robot goes where it should. MPLab version of the program.
#include "BuiltIns.h"
/*
* Sample program to have the robot drive in a straight line using
* the kit gyro sensor (connected to analog port 1).
*/
void main(void)
{
}
void IO_Initialization(void)
{
SetCompetitionMode(1); // set to competition mode
}
void Initialize(void)
{
InitGyro(1);
// initialize gyro on port 1
TwoWheelDrive(2,1); // Setup 2
motor drive robot (ports 1 & 2)
SetInvertedMotor(1); // motors are
inverted (direct drive)
SetInvertedMotor(2);
}
void Autonomous(void)
{
int heading;
StartGyro(1);
// start gyro sampling
while (1)
{
heading = GetGyroAngle(1); // get the
current heading
Drive(80, heading/2); // turn towards
the error
}
}
This program initializes the camera using the color values for tracking the 2006 FRC Green Target. Then the robot will in circles until it sees the target. Then it will drive towards the target as long as it is in view, otherwise it doesn't drive. A sample program is here.
#include "BuiltIns.h"
//
camera initialization array - this is an array of CameraInitializationData
//
records. The
unsigned char numberOfCameraInitBlocks = 1;
CameraInitializationData
cameraData[] =
{
{ 85,115,15,17,100,145,0,1,0,1,0,0,128,128,128,1,16,8,5,30,15,5 }
};
/*
* this
function limits value in the to "limit" in magnitude. If it is less
* than -limit, the value is -limit. If it is more than limit, the value is
* limit.
*/
int limit(int value, int
limit)
{
if (value
> limit) return limit;
if (value <
-limit) return -limit;
return value;
}
/*
* Camera test program -
* Drives in circles until the
camera sees the green target, then it drives towards
* the
target. This assumse that the camera is mounted in the front of the robot
* without the servos connected. They are not used for this example.
*/
void main(void)
{
TPacket *tp;
unsigned char mx;
unsigned char conf;
int error;
TwoWheelDrive(2, 1);
InitCamera(1); // initialize the camera for the green
target
StartCamera();
// start the camera tracking
// this program spins the robot around in a circle until it sees
// the color target then drives towards it.
while (1)
{
// look for the target
tp = CopyTrackingData(); // get the most recent camera
data
conf = tp->confidence;
// get the confidence
if (conf > 10) break; // break out of
this loop if the target seen
Drive(0, 50);
// drive in circles
Wait(100);
// wait a while
}
while (1)
{
// drive towards target
tp = CopyTrackingData();
// get some camera data
error = limit(4 * (((int)tp->mx) - 80), 55); // limit the x value
if (tp->confidence > 0)
// only drive when we see something
Drive(70, error);
// drive towards the target
else
Drive(0, 0);
// don't drive when not seeing target
}
}