Create GRIP target qualification algorithm

[gerth2]

Read in the reports published to NetworkTables by GRIP.

For each contour reported from GRIP, qualify it. This means to decide whether it's actually a target or not.

Suggestions on how to this:

For 2016 qualification, use the reported overall width and height to calculate the expected area of the target. This area calculation must account for the fact the target is a U shape, not a solid square.

Compare the expected area to the reported area from GRIP. If these match within a certain percent error, call that particular contour a target and track it.

When the driver commands an auto-align routine to run, at the start, pick the biggest target in range, if any. For each control loop, record the x/y coordinates where the chosen target was. On the next iteration, return the X/Y coordinates of the target closest to the previous one.

All this work is probably best done in its own class. Call the class something like "visionTracker()" The class should have methods to do the following:

--readTargets() - reads in all the identified targets from Grip and qualifies which ones are valid targets.

--beginTracking() - picks the largets target it can find and records and returns the pixel x/y coordinates.

--trackingIterate() - picks the target closes to the previous known location of the target and returns its new x/y coordinates.

For integration, the idea is that the readTargets() method is called every loop. When the driver commands an auto-align routine, beginTracking() is called once. every loop during the auto-align routine, the trackingIterate() function gets called.

[gerth2]

grip java example: https://github.com/WPIRoboticsProjects/GRIP/wiki/Tutorial:-Run-GRIP-from-a-CPP-or-Java-FRC-program

[imdunne8]

FYI to students working on this - a new GRIP release just came out that fixes lots of bugs. Update next time you're working on it. https://github.com/WPIRoboticsProjects/GRIP/releases/latest (Edit: Updated with a link that will always bring up the latest installer. Previous link had a bug that was already fixed)

[gerth2]

Easiest suggested method: Create a class for vision targets:

class visionTarget{
    public double width;
    public double height;
    public double center_X;
    public double center_Y;
    public double area;
}

Once this is in place, here is some skeleton for the readTargets() class:

class visionTracker()
    private visionTarget[] = sorted_tgts;
    private visionTarget[] = prev_sorted_tgts;

    //Class Constructor
    public visionTracker(){
        sorted_tgts = new visionTarget[3];
        prev_sorted_tgts = new visionTarget[3];

        //Set all elements of both arrays to Null

    }

    public int readTargets(){
        //Copy sorted_tgts into prev_sorted_tgts for safekeeping

        for(all obj identified by GRIP){
            //make a new array to hold all the Area errors

            //Calculate error in area for all objects identified by GRIP
            error = abs(obj.area - obj.height*obj.width*RATIO);
                //RATIO must be replaced by ratio of filled in area to total area on target

        }

        //create new array valid_objs with the three lowest
        //errors. If less than three objects are identified,
        //the remaining entries should be Null.

        for(non-null entries in prev_sorted_tgts){
            //Find index of element in valid_objs which is closest 
            //in x/y center distance to the element from prev_sorted_tgts
            //and store it into sorted_tgts at the same index
        }

        //put remaining entries from valid_objs into sorted_tgts array
        //in numerical order.

    }

}