[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 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
            }
      }
}