Target Tracking Skelton Code

Added by Kwabena Agyeman over 7 years ago

A forum member needed some code to fire a gun at a target when it stops moving. I'm re-posting it here because it might be useful to others:

#include <CMUcam4.h>
#include <CMUcom4.h>

/****************************************/

/*
The values below control the operation of the program. You have to use the 
CMUcam4GUI to figure out what the color tracking parameters should be. 

Picking good color thresholds is important. The way you do this is set the CMUcam4GUI
to YUV mode, then turn off autogain and autowhite balance. Once you do this take a picture
and then select the area of color you want to track in the image. It is important that you
just select only the area of color you want to track and not anything else.

Once you do that you'll have some starting YUV values to work with. You can expand these values
manually now yourself. The way to do this is do try and make the Y bound as large as possible. Go
for making it between 0 and 255. If you don't experience any problems doing that then you are good, 
otherwise try and find the largest bound possible that works.

Once you do that you can now expand the U and V values. Just keep making the range as big as possible
until you can't get it bigger. If you want to tell what the optium value should be then turn histogram
tracking on in the CMUcam4GUI and look at the histogram of the U and V color channels (with 32 bins).
If your bounds are bigger than necessary you'll notice that the histogram distribution does not fill
up the entire range that you are tracking for. So, you should shrink your color range to juse bound 
the color distribution. However, a larger range than necessary may be better because the U and V values
still shift arround (much less so than in RGB mode however) in changing lighting.

Now, once you have the tracking parameters you can feed them into the code below. However, the next step
is to figure out what pixels and confidence threshold values you should use. You can use the CMUcam4GUI
to help you with this too. First enable line mode in the options menu, then go to the tracking tab and
click track color. You should see a visualization of what yuour camera sees.

Look at the values in the pane below the visualization window. You should see a value for the pixels and
confidence. The values you see will be the same values that the camera reports to you on the Arduino. So,
just decide what a color lower threshold will be for you application and then set that in the code below.
Keep in mind that the pixels and confidence values change very easy given whatever setup you have the
CMUcam4 in. So, figure out what they should be once you have you system built.

Finally, you now just have to determine what a good movement threshold should be. This require experimentation
just like everything else. So, add some print statments to the below code and figure out what a good movement
threshold is. Then just change the below constants to reflect that new value.
*/

// V (normally red in rgb mode)
#define V_MIN 230
#define V_MAX 255

// Y (normally green in rgb mode)
#define Y_MIN 230
#define Y_MAX 255

// U (normally blue in rgb mode)
#define U_MIN 230
#define U_MAX 255

#define PIXELS_THRESHOLD 1
#define CONFIDENCE_THRESHOLD 50

#define PAN_MOVEMENT_THRESHOLD 10 // In us pulse lengths sent to the servo
#define TILT_MOVEMENT_THRESHOLD 10 // In us pulse lengths sent to the servo

#define NOISE_FILTER 2 // Figure out a good value with the CMUCam4GUI

#define PAN_REVERSED false // Make true to reverse the pan direction
#define TILT_REVERSED false // Make true to reverse the tilt direction

#define FIRING_PIN 4 // Pin used to trigger the gun

/****************************************/

#define LED_BLINK 5 // 5 Hz
#define WAIT_TIME 5000 // 5 Seconds

#define PAN_AVG_LEN 5 // Bigger for more averaging (updates slower), lower for less averaging (noisier)
#define TILT_AVG_LEN 5 // Bigger for more averaging (updates slower), lower for less averaging (noisier)

// Serial RX3 and TX3
CMUcam4 cam(CMUCOM4_SERIAL3);

int panServoOld;
int tiltServoOld;

int panAverage = 0;
int tiltAverage = 0;

boolean panMovement = false;
boolean tiltMovement = false;

void setup()
{
  digitalWrite(FIRING_PIN, LOW);
  pinMode(FIRING_PIN, OUTPUT);

  cam.begin();

  // Wait for auto gain and auto white balance to run

  cam.colorTracking(true);

  cam.automaticPan(true, PAN_REVERSED);
  cam.automaticTilt(true, TILT_REVERSED);

  panServoOld = cam.getServoPosition(CMUCAM4_PAN_SERVO);
  tiltServoOld = cam.getServoPosition(CMUCAM4_TILT_SERVO);

  cam.LEDOn(LED_BLINK);
  delay(WAIT_TIME);

  // Turn auto gain and auto white balance off

  cam.autoGainControl(false);
  cam.autoWhiteBalance(false);

  // Reset the LED

  cam.LEDOn(CMUCAM4_LED_ON);

  // Apply mode settings

  cam.pollMode(true);
  cam.noiseFilter(NOISE_FILTER);
  cam.setTrackingParameters(V_MIN, V_MAX, Y_MIN, Y_MAX, U_MIN, U_MAX);
}

void loop()
{
  CMUcam4_tracking_data_t data;

  for(;;)
  {
    // Ask the camera for tracking information

    cam.trackColor();
    cam.getTypeTDataPacket(&data);

    // Check if there is a large enough and dense target

    if((data.pixels >= PIXELS_THRESHOLD) && (data.confidence >= CONFIDENCE_THRESHOLD))
    {
      // Target acquired...

      cam.automaticPan(true, PAN_REVERSED);
      cam.automaticTilt(true, TILT_REVERSED);

      // Take the differences in servo positions

      int panServoNew = cam.getServoPosition(CMUCAM4_PAN_SERVO);
      int tiltServoNew = cam.getServoPosition(CMUCAM4_TILT_SERVO);

      int panDifference = (panServoNew - panServoOld);
      int tiltDifference = (tiltServoNew - tiltServoOld);

      panServoOld = panServoNew;
      tiltServoOld = tiltServoNew; 

      // Average the differences in servo positions

      // Classic moving average - http://en.wikipedia.org/wiki/Moving_average
      panAverage = (panDifference + (PAN_AVG_LEN * panAverage)) / (PAN_AVG_LEN + 1);

      // Classic moving average - http://en.wikipedia.org/wiki/Moving_average
      tiltAverage = (tiltDifference + (TILT_AVG_LEN * tiltAverage)) / (TILT_AVG_LEN + 1);

      // panAverage now basically has a value that becomes non-zero as the target moves...
      // tiltAverage now basically has a value that becomes non-zero as the target moves...

      // Detect when movement starts or stops

      boolean panTriggered = false;
      boolean tiltTriggered = false;

      if((panAverage >= PAN_MOVEMENT_THRESHOLD) && (panMovement == false))
      {
        panMovement = true;

        // Do something here if you want to when the target starts to move...
      }

      if((panAverage < PAN_MOVEMENT_THRESHOLD) && (panMovement == true))
      {
        panMovement = false;

        // Do something here if you want to when the target stops moving...

        panTriggered = true; // Like this...
      }

      if((tiltAverage >= TILT_MOVEMENT_THRESHOLD) && (tiltMovement == false))
      {
        tiltMovement = true;

        // Do something here if you want to when the target starts to move...
      }

      if((tiltAverage < TILT_MOVEMENT_THRESHOLD) && (tiltMovement == true))
      {
        tiltMovement = false;

        // Do something here if you want to when the target stops moving...

        tiltTriggered = true; // Like this...
      } 

      // Activate the firing pin when the motion stops

      if(panTriggered || tiltTriggered)
      {
        panTriggered = false;
        tiltTriggered = false;

        digitalWrite(FIRING_PIN, HIGH);
        delay(500); // Wait for the shot.
        digitalWrite(FIRING_PIN, LOW);
        delay(500); // Wait for the shot.   
      }
    }
    else
    {
      // Target lost...

      cam.automaticPan(false, false);
      cam.automaticTilt(false, false);

      panAverage = 0;
      tiltAverage = 0;

      panMovement = false;
      tiltMovement = false;
    }
  }
}