Camera tracking demo in easyC or WPILib

[BradAMiller]

Here is the shortest program that I could write that will track to a green light. It assumes 2 motors connected to PWM ports 1 and 2. Also the camera is fixed (no servoing). The robot will follow the light as long as it is seeing it.

Here is the easyC version (just the C code), but there is a link to an easyC Pro project and an MPLab project. There is also a link to a video (http://www.wpi.edu/~bamiller/WPILib/camerademo.wmv)showing it running on a FRC controller on a Vex robot chassis.


#include "Main.h"

void main ( void )
{
unsigned char regionSize;
unsigned char mx;
int error;

TwoWheelDrive(2,1); // The following 4 functions are built into WPILib, but have no blocks
SetInvertedMotor(1); // So they have to be user code blocks
SetInvertedMotor(2);
SetCameraDebugMode(1);
InitCamera ( 1 ) ; // default FIRST green setup
StartCamera ( ) ;
while ( 1 )
{
CaptureTrackingData ( &mx , 0 , 0 , 0 , 0 , 0 , &regionSize , 0 , 0 , 0 ) ;
if ( regionSize > 10 )
{
error = ((int) mx) - 80 ;
Drive(50, error); // turn towards the light
}
else
{
Drive(0, 0); // no light - stop driving
}
}
}