Camera.cpp

#include "Camera.h"
#include "../Configuration.h"
#include <math.h>

#define fuzzy(a, b) static Camera::FuzzyDistance *(a) = new Camera::FuzzyDistance;\
                    *(a) = (b);

Camera::Camera():
    Subsystem("Camera"),
    panValue(0),
    tiltValue(0),
    fuzzyDistanceChoice(),
    fuzzyDistance(Camera::kMid),
    pan(PAN_PORT),
    tilt(TILT_PORT),
    camera(AxisCamera::GetInstance()),
    matches(NULL),
    target(NULL),
    distanceToTarget(0)
{
    fuzzy(close, Camera::kClose);
    fuzzy(mid, Camera::kMid);
    fuzzy(far, Camera::kFar);
    fuzzyDistanceChoice.AddObject("Close", close);
    fuzzyDistanceChoice.AddDefault("Mid", mid);
    fuzzyDistanceChoice.AddObject("Far", far);
    dashboard->PutData("Manual Camera Range", &fuzzyDistanceChoice);
    camera.WriteCompression(80);
    camera.WriteMaxFPS(5);
    WriteResolutionFromFuzzy();
}

void Camera::Status() {
    fuzzyDistance = *(Camera::FuzzyDistance*)fuzzyDistanceChoice.GetSelected();
    dashboard->PutInt("Pan Value", panValue);
    dashboard->PutInt("Tilt Value", tiltValue);
    dashboard->PutDouble("Distance to Target", distanceToTarget);
}

void Camera::WriteResolutionFromFuzzy() {
    switch (fuzzyDistance) {
    case Camera::kClose:
        camera.WriteResolution(AxisCamera::kResolution_160x120);
        break;
    case Camera::kFar:
        camera.WriteResolution(AxisCamera::kResolution_640x480);
        break;
    default:
        camera.WriteResolution(AxisCamera::kResolution_320x240);
    }
}

void Camera::InitDefaultCommand() {
}

void Camera::FindRectangles() {
    HSLImage  *rawImage  = camera.GetImage();
    MonoImage *monoImage = rawImage->GetLuminancePlane();
    Image     *image     = monoImage->GetImaqImage();
    workingImageWidth  = monoImage->GetWidth();
    workingImageHeight = monoImage->GetHeight();
    delete monoImage;
    delete rawImage;

    RectangleDescriptor *rectangleDescriptor = new RectangleDescriptor;
    rectangleDescriptor->minWidth  = 0;
    rectangleDescriptor->minHeight = 0;
    rectangleDescriptor->maxWidth  = workingImageWidth;
    rectangleDescriptor->maxHeight = workingImageHeight;

    int numCurrentMatches;
    RectangleMatch *temp;
    temp = imaqDetectRectangles(image,
                                rectangleDescriptor,
                                NULL, // Default curve options as per manual.
                                NULL, // Default shape detection options.
                                NULL, // (ROI) Whole image should be searched.
                                &numCurrentMatches);
    matches->erase(matches->begin(), matches->end());
    matches = new vector<RectangleMatch>;
    for (int i = 0; i < numCurrentMatches; i++)
        matches->push_back(temp[i]);
    
    imaqDispose(temp);
}

void Camera::PickRectangle() {
    if (matches->size() == 0)
        target = NULL;
    else if (matches->size() == 1)
        target = &matches->at(0);
    else
        target = GetHighestScoringRectangle();
}

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

RectangleMatch * Camera::GetHighestScoringRectangle() {
    sort(matches->begin(), matches->end(), HeightSort);
    return &matches->at(0);

    // TODO: Integrate compass and pick a more intelligent scoring target.
}

int Camera::ConvertToAimingSystem(int coord, int resolution) {
    int center = resolution / 2;
    return (coord - center) / center;
}

void Camera::AimAtTarget() {
    int cX = (int)(target->corner[0].x + target->corner[1].x) / 2;
    int cY = (int)(target->corner[0].y + target->corner[2].y) / 2;
    panValue  = ConvertToAimingSystem(cX, workingImageWidth);
    tiltValue = ConvertToAimingSystem(cY, workingImageHeight);

    pan.Set(panValue);
    tilt.Set(tiltValue);
}

void Camera::CalculateDistance() {
    float imageWidthInFeet = (TARGET_WIDTH_IN_FEET * workingImageWidth) / target->width;
    distanceToTarget = (imageWidthInFeet / 2) / tan(VIEWING_ANGLE / 2);
}

void Camera::Shoot() {
}

Camera.h

#ifndef _CAMERA_H_
#define _CAMERA_H_

#include "Commands/Subsystem.h"
#include "WPILib.h"

class Camera : public Subsystem {
public:
    enum FuzzyDistance { kClose, kMid, kFar };

    int panValue, tiltValue;

    Camera();
    void InitDefaultCommand();
    void Status();

    void FindRectangles();
    void PickRectangle();
    void AimAtTarget();
    void CalculateDistance();
    void Shoot();

private:
    SendableChooser fuzzyDistanceChoice;
    FuzzyDistance fuzzyDistance;
    Servo pan;
    Servo tilt;
    AxisCamera &camera;
    vector<RectangleMatch> *matches;
    RectangleMatch *target;
    int workingImageWidth, workingImageHeight;
    float distanceToTarget;
    
    void WriteResolutionFromFuzzy();
    RectangleMatch * GetHighestScoringRectangle();
    int ConvertToAimingSystem(int coord, int resolution);
};

#endif