Camera demos using easyC or WPILib

Here is a quick camera demo that uses easyC or WPILib to track towards the green target. The robot follows around someone holding the light as seen in this video.

So 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 project and an MPLab project.

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