Chief Delphi - Camera tracking demo in easyC or WPILib

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.

Code:

#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

            }

      }

}