2 files changed, 119 insertions, 0 deletions
diff --git a/extensions/ExampleSquareTrackerExtension/src/edu/wpi/first/smartdashboard/extensions/squaretracker/Square.pdf b/extensions/ExampleSquareTrackerExtension/src/edu/wpi/first/smartdashboard/extensions/squaretracker/Square.pdf
new file mode 100644
index 0000000..23cb55e
--- /dev/null
+++ b/extensions/ExampleSquareTrackerExtension/src/edu/wpi/first/smartdashboard/extensions/squaretracker/Square.pdf
diff --git a/extensions/ExampleSquareTrackerExtension/src/edu/wpi/first/smartdashboard/extensions/squaretracker/SquareTrackerExtension.java b/extensions/ExampleSquareTrackerExtension/src/edu/wpi/first/smartdashboard/extensions/squaretracker/SquareTrackerExtension.java
new file mode 100644
index 0000000..a9361d3
--- /dev/null
+++ b/extensions/ExampleSquareTrackerExtension/src/edu/wpi/first/smartdashboard/extensions/squaretracker/SquareTrackerExtension.java
@@ -0,0 +1,119 @@
+package edu.wpi.first.smartdashboard.extensions.squaretracker;
+
+import edu.wpi.first.smartdashboard.extensions.wpijavacvvideo.LaptopCameraExtension;
+import edu.wpi.first.smartdashboard.robot.Robot;
+import edu.wpi.first.wpijavacv.WPIBinaryImage;
+import edu.wpi.first.wpijavacv.WPIColor;
+import edu.wpi.first.wpijavacv.WPIColorImage;
+import edu.wpi.first.wpijavacv.WPIContour;
+import edu.wpi.first.wpijavacv.WPIImage;
+import edu.wpi.first.wpijavacv.WPIPoint;
+import edu.wpi.first.wpijavacv.WPIPolygon;
+import edu.wpi.first.wpilibj.networking.NetworkTable;
+import java.util.ArrayList;
+
+/**
+ *
+ * @author Greg Granito
+ */
+public class SquareTrackerExtension extends LaptopCameraExtension {
+    public static final String NAME = "Laptop Camera Square Tracker";
+
+    /* Uses the table to send information from the DS to the robot */
+    NetworkTable table = NetworkTable.getTable("camera");
+    WPIColor targetColor = new WPIColor(255, 0, 0);
+
+    @Override
+    public WPIImage processImage(WPIColorImage rawImage) {
+
+        WPIBinaryImage blueBin = rawImage.getBlueChannel().getThresholdInverted(60);
+        WPIBinaryImage greenBin = rawImage.getGreenChannel().getThresholdInverted(60);
+        WPIBinaryImage redBin = rawImage.getRedChannel().getThresholdInverted(60);
+
+        WPIBinaryImage finalBin = blueBin.getAnd(redBin).getAnd(greenBin);
+
+        finalBin.erode(2);
+        finalBin.dilate(6);
+
+        WPIContour[] contours = finalBin.findContours();
+
+        ArrayList<WPIPolygon> polygons = new ArrayList<WPIPolygon>();
+
+        for(WPIContour c : contours){
+            double ratio = ((double)c.getHeight()) / ((double)c.getWidth());
+            if(ratio < 1.5 && ratio> 0.75){
+                polygons.add(c.approxPolygon(45));
+            }
+        }
+
+        ArrayList<WPIPolygon> possiblePolygons = new ArrayList<WPIPolygon>();
+
+        for(WPIPolygon p : polygons){
+            if(p.isConvex() && p.getNumVertices() == 4){
+                possiblePolygons.add(p);
+            }else{
+                rawImage.drawPolygon(p, WPIColor.CYAN, 5);
+            }
+        }
+
+        WPIPolygon square = null;
+        int squareArea = 0;
+
+        for(WPIPolygon p : possiblePolygons){
+            rawImage.drawPolygon(p, WPIColor.GREEN, 5);
+            for(WPIPolygon q : possiblePolygons){
+                if(p == q) continue;
+
+               int pCenterX = (p.getX() + (p.getWidth()/2));
+
+               int qCenterX = q.getX() + (q.getWidth()/2);
+
+               int pCenterY = (p.getY() + (p.getHeight()/2));
+
+               int qCenterY = q.getY() + (q.getHeight()/2);
+
+               rawImage.drawPoint(new WPIPoint(pCenterX, pCenterY), targetColor, 5);
+               rawImage.drawPoint(new WPIPoint(qCenterX, qCenterY), targetColor, 5);
+
+                if(Math.abs(pCenterX - qCenterX) < 20 &&
+                    Math.abs(pCenterY - qCenterY) < 20){
+                    int pArea = Math.abs(p.getArea());
+                    int qArea = Math.abs(q.getArea());
+                    if(pArea > qArea){
+                        square = p;
+                        squareArea = pArea;
+                    }else{
+                        square = q;
+                        squareArea = qArea;
+                    }
+                    break;
+                }
+            }
+        }
+
+        if(square != null){
+            double x = square.getX() + (square.getWidth()/2);
+            x = (2 * (x/rawImage.getWidth())) - 1;
+
+            double area = ((double)squareArea) /  ((double)(rawImage.getWidth() * rawImage.getHeight()));
+
+            synchronized(table) {
+            table.beginTransaction();
+                table.putBoolean("found", true);
+                table.putDouble("x", x);
+                table.putDouble("area", area);
+            table.endTransaction();
+            }
+
+            Robot.getTable().putBoolean("found", true);
+            Robot.getTable().putDouble("X", x);
+            Robot.getTable().putDouble("Area", area);
+            rawImage.drawPolygon(square, targetColor, 7);
+        }else{
+            table.putBoolean("found", false);
+            Robot.getTable().putBoolean("found", false);
+        }
+
+        return rawImage;
+    }
+}