main.cpp

#include "WPILib.h"
#include "TrackingImage.h"

#define dashboard SmartDashboard::GetInstance()

class Robot : public IterativeRobot {
public:
    Servo *pan;
    Servo *tilt;
    RectangleMatch *target;
    CurveOptions curveOptions;
    ShapeDetectionOptions shapeOptions;
    RectangleDescriptor rectangleDescriptor;

    Robot() {
        pan  = new Servo(1);
        tilt = new Servo(2);

        // Default curve, shape, and rectangle descriptor options.
        curveOptions.extractionMode   = IMAQ_NORMAL_IMAGE;
        curveOptions.threshold        = 190;
        curveOptions.filterSize       = IMAQ_NORMAL;
        curveOptions.minLength        = 200;
        curveOptions.rowStepSize      = 3;
        curveOptions.columnStepSize   = 3;
        curveOptions.maxEndPointGap   = 20;
        curveOptions.onlyClosed       = false;
        curveOptions.subpixelAccuracy = false;

        RangeFloat orientationRange[2];
        orientationRange[0].minValue = 0;
        orientationRange[0].maxValue = 20;
        orientationRange[1].minValue = 340;
        orientationRange[1].maxValue = 360;

        shapeOptions.mode = IMAQ_GEOMETRIC_MATCH_ROTATION_INVARIANT;
        shapeOptions.angleRanges    = orientationRange;
        shapeOptions.numAngleRanges = 2;
        shapeOptions.minMatchScore  = 600;

        rectangleDescriptor.minWidth  = 100;
        rectangleDescriptor.maxWidth  = 400;
        rectangleDescriptor.minHeight = 100;
        rectangleDescriptor.maxHeight = 400;
    }

    virtual void TeleopInit() {
        AxisCamera &camera = AxisCamera::GetInstance();
        camera.WriteMaxFPS(5);

        dashboard->PutInt("Threshold", curveOptions.threshold);
        dashboard->PutInt("Minimum Length", curveOptions.minLength);
        dashboard->PutInt("Row Step Size", curveOptions.rowStepSize);
        dashboard->PutInt("Column Step Size", curveOptions.columnStepSize);
        dashboard->PutInt("Maximum Endpoint Gap", curveOptions.maxEndPointGap);
        dashboard->PutDouble("Minimum Score", shapeOptions.minMatchScore);
    }

    virtual void TeleopPeriodic() {
        curveOptions.threshold      = dashboard->GetInt("Threshold");
        curveOptions.minLength      = dashboard->GetInt("Minimum Length");
        curveOptions.rowStepSize    = dashboard->GetInt("Row Step Size");
        curveOptions.columnStepSize = dashboard->GetInt("Column Step Size");
        curveOptions.maxEndPointGap = dashboard->GetInt("Maximum Endpoint Gap");
        shapeOptions.minMatchScore  = dashboard->GetDouble("Minimum Score");

        TrackingImage *image = new TrackingImage(AxisCamera::GetInstance());
        image->GetRectangles(&rectangleDescriptor, &curveOptions, &shapeOptions);
        target = &image->matches->at(0);
        image->SendVisionData(target);
        delete image;
        target = NULL;
    }
};

START_ROBOT_CLASS(Robot);

TrackingImage.cpp

#include "TrackingImage.h"

static bool HeightSort(RectangleMatch a, RectangleMatch b) {
    return a.corner[0].y < b.corner[0].y;
}

TrackingImage::TrackingImage(AxisCamera &camera) {
    ColorImage *rawImage = new ColorImage(IMAQ_IMAGE_RGB);
    camera.GetImage(rawImage);
    grayscaleImage = rawImage->GetLuminancePlane();
    delete rawImage;
}

TrackingImage::~TrackingImage() {
    delete grayscaleImage;
    matches->erase(matches->begin(), matches->end());
    delete matches;
}

void TrackingImage::HandleError(int success) {
    if (!success && imaqGetLastError()) {
        char *errorMessage;
        sprintf(errorMessage, "Received error code %d from %s\n", imaqGetLastError(), imaqGetLastErrorFunc());
        printf(errorMessage);
        imaqClearError();
    }
}

void TrackingImage::GetRectangles(RectangleDescriptor *rectangleDescriptor, CurveOptions *curveOptions, ShapeDetectionOptions *shapeOptions) {
    int numCurrentMatches;
    RectangleMatch *temp;
    temp = imaqDetectRectangles(grayscaleImage->GetImaqImage(),
                                rectangleDescriptor,
                                curveOptions,
                                shapeOptions,
                                NULL,
                                &numCurrentMatches);
    HandleError((INT32)temp);
    matches->erase(matches->begin(), matches->end());
    for (int i = 0; i < numCurrentMatches; i++)
        matches->push_back(temp[i]);

    sort(matches->begin(), matches->end(), HeightSort);
    imaqDispose(temp);
}

void TrackingImage::SendVisionData(RectangleMatch *target) {
    Dashboard &d = DriverStation::GetInstance()->GetHighPriorityDashboardPacker();
    d.AddCluster(); {
        d.AddI32((INT32)target->corner[0].y);
        d.AddI32((INT32)target->corner[0].x);
        d.AddI32((INT32)target->corner[2].y);
        d.AddI32((INT32)target->corner[2].x);
    }
    d.FinalizeCluster();
    d.Finalize();
}

TrackingImage.h

#ifndef _TRACKING_IMAGE_H_
#define _TRACKING_IMAGE_H_

#include "WPILib.h"

class TrackingImage {
public:
    vector<RectangleMatch> *matches;

    TrackingImage(AxisCamera &camera);
    ~TrackingImage();

    void GetRectangles(RectangleDescriptor *rectangleDescriptor, CurveOptions *curveOptions, ShapeDetectionOptions *shapeOptions);
    void SendVisionData(RectangleMatch *target);

private:
    MonoImage *grayscaleImage;

    void HandleError(int success = 0);
};

#endif