537's Hot Goal Detection Code

[themagic8ball]

537 would like to share its hot goal detection code with you. We are out of the competition now but the robot was able to pick the hot goal correctly in 13 of our 15 matches at Wisconsin this past weekend.

Instead of trying to track the vision targets, we pointed the camera at the goal itself and counted the number of yellow pixels on the screen. We did this after we drove forward, so there was no chance of mixing up left and right. All we do is a simple HSL threshold and then count the number of 1 pixels in the binary image. Check it out below:

#define YELLOW_PIXEL_THRESHOLD 1500
#define RATE 4

bool CameraManager::IsHotGoal()
{
  AxisCamera& camera = AxisCamera::GetInstance();
  SmartDashboard::PutNumber("counter", Counter);
  
  if(Counter * RATE >= CameraTimer.Get())
  {
    return LastValue;
  }
  
  ColorImage *CurrentImage = camera.GetImage();
  if (!CurrentImage)
  {
    return false;
  }
  
  if (Counter == 16)
  {
    CurrentImage->Write("AutoImage1.png");	
  }
  if (Counter == 32)
  {
    CurrentImage->Write("AutoImage2.png");
  }
  
  BinaryImage* ThresholdImage = CurrentImage->ThresholdHSL(0, 25, 100, 255, 50, 255);
  ImageInfo info;
  Image *img = ThresholdImage->GetImaqImage();
  imaqGetImageInfo(img, &info);
  
  int yellowCount = 0;
  unsigned char * pixels = (unsigned char *) info.imageStart;
  int y, x;
  for (y = 0; y < info.yRes; y++){
    for (x = 0; x < info.xRes; x++){
      int bitmapIndex = (info.pixelsPerLine *y) + x;
      if (pixels[bitmapIndex] == 1)
      {
        yellowCount++;
      }
    }
  }
  
  LastValue = yellowCount > YELLOW_PIXEL_THRESHOLD;
  
  SmartDashboard::PutNumber("Yellow Count", yellowCount);
  delete CurrentImage;
  delete ThresholdImage;
  Counter++;
  return LastValue;
}
void CameraManager::CameraInitialize()
{
  AxisCamera& camera = AxisCamera::GetInstance();
}
void CameraManager::CameraStart()
{
  CameraTimer.Start();
}
void CameraManager::CameraStop()
{
  CameraTimer.Stop();
  CameraTimer.Reset();
  AxisCamera::DeleteInstance();
}

The code limits itself to 4 calculations per second and since we aren't doing anything too complex it seemed to work just fine. It also saves 2 images to the CRIO so we could work through any issues of a false detection.

To get this code working for your team you will need to adjust your camera settings and play with the threshold values. We called the CameraInitialize method in RobotInit() so that there would be no delay in getting good images. Then we call start and stop at the beginning and end of autonomous, and IsHotGoal() throughout.

This code was copied from when we were trying to detect red (that's the LED strip we had) but can pretty much be calibrated for any color. The nice thing is there isn't a whole lot of other yellow to get confused with. The white balance setting is especially important and in fact our detection was more of a green hue, but it worked. Hopefully this will help some of you!