convert opencv code to NI code

diff --git a/src/org/usfirst/frc/team3501/robot/subsystems/TowerTracker2.java b/src/org/usfirst/frc/team3501/robot/subsystems/TowerTracker2.java
new file mode 100644 (file)
index 0000000..3695e38
--- /dev/null
+++ b/src/org/usfirst/frc/team3501/robot/subsystems/TowerTracker2.java
@@ -0,0 +1,159 @@
+package org.usfirst.frc.team3501.robot.subsystems;
+
+import com.ni.vision.NIVision.PointDouble;
+
+import edu.wpi.first.wpilibj.image.BinaryImage;
+import edu.wpi.first.wpilibj.image.HSLImage;
+import edu.wpi.first.wpilibj.image.NIVisionException;
+import edu.wpi.first.wpilibj.image.ParticleAnalysisReport;
+import edu.wpi.first.wpilibj.networktables.NetworkTable;
+import edu.wpi.first.wpilibj.vision.AxisCamera;
+
+/**
+ *
+ */
+
+/**
+ *
+ * @author Elijah Kaufman
+ * @version 1.0
+ * @description Uses opencv and network table 3.0 to detect the vision targets
+ *
+ */
+public class TowerTracker2 {
+
+  /**
+   * static method to load opencv and networkTables
+   */
+  static {
+    System.out.println(System.getProperty("java.library.path"));
+    System.loadLibrary("libopencv_java310.so");
+    NetworkTable.setClientMode();
+    NetworkTable.setIPAddress("roborio-3502.local");
+  }
+  // constants for the color rbg values
+  // these are the threshold values in order
+  // LOWER_BOUNDS = new Scalar(58, 0, 109),
+  // UPPER_BOUNDS = new Scalar(93, 255, 240);
+
+  // the size for resing the image
+  // public static final Size resize = new Size(320, 240);
+
+  public static HSLImage originalImage;
+  public static AxisCamera camera;
+  public static BinaryImage filteredImage;
+
+  // Constants for known variables
+  // the height to the top of the target in first stronghold is 97 inches
+  public static final int TOP_TARGET_HEIGHT = 97;
+  // the physical height of the camera lens
+  public static final int TOP_CAMERA_HEIGHT = 32;
+
+  // camera details, can usually be found on the datasheets of the camera
+  public static final double VERTICAL_FOV = 51;
+  public static final double HORIZONTAL_FOV = 67;
+  public static final double CAMERA_ANGLE = 10;
+
+  public static boolean shouldRun = true;
+
+  /**
+   *
+   * @param args
+   *          command line arguments
+   *          just the main loop for the program and the entry points
+   */
+  public static void main(String[] args) {
+    // TODO Auto-generated method stub
+    NetworkTable table = NetworkTable.getTable("SmartDashboard");
+    // main loop of the program
+    while (shouldRun) {
+      try {
+        // opens up the camera stream and tries to load it
+        // videoCapture = new VideoCapture();
+        // videoCapture.open("http://10.35.01.11/mjpg/video.mjpg");
+        // wait until it is opened
+        // while (!videoCapture.isOpened()) {}
+        // time to actually process the acquired images
+        processImage();
+      } catch (Exception e) {
+        e.printStackTrace();
+        break;
+      }
+    }
+    // make sure the java process quits when the loop finishes
+    // videoCapture.release();
+    System.exit(0);
+  }
+
+  /**
+   *
+   * reads an image from a live image capture and outputs information to the
+   * SmartDashboard or a file
+   *
+   * @throws NIVisionException
+   */
+  public static void processImage() throws NIVisionException {
+    // ArrayList<MatOfPoint> contours = new ArrayList<MatOfPoint>();
+    double x, y, targetX, targetY, distance, azimuth;
+    // frame counter
+    int FrameCount = 0;
+    long before = System.currentTimeMillis();
+    // only run for the specified time
+    while (FrameCount < 100) {
+      originalImage = camera.getImage();
+      filteredImage = originalImage.thresholdHSL(328, 360, 100, 255, 43, 255);
+      ParticleAnalysisReport[] reports = filteredImage
+          .getOrderedParticleAnalysisReports();
+      // make sure the contours that are detected are at least 20x20
+      // pixels with an area of 400 and an aspect ration greater then 1
+      // Commented by HB to go past syntax errors
+      for (ParticleAnalysisReport report : reports) {
+        if (report.boundingRectHeight > 25 && report.boundingRectWidth > 25
+            && report.boundingRectWidth > report.boundingRectHeight) {
+          y = reports[0].boundingRectTop - reports[0].boundingRectHeight / 2;
+          y = -((2 * (y / originalImage.getHeight())) - 1);
+          distance = (TOP_TARGET_HEIGHT - TOP_CAMERA_HEIGHT) /
+              Math.tan((y * VERTICAL_FOV / 2.0 + CAMERA_ANGLE) * Math.PI / 180);
+          // angle to target...would not rely on this
+          targetX = reports[0].boundingRectLeft + reports[0].boundingRectWidth
+              / 2;
+          targetX = (2 * (targetX / originalImage.getWidth())) - 1;
+          azimuth = normalize360(targetX * HORIZONTAL_FOV / 2.0 + 0);
+          // drawing info on target
+          x = reports[0].boundingRectLeft + originalImage.getWidth() / 2;
+          y = reports[0].boundingRectTop - originalImage.getHeight() / 2;
+          PointDouble center = new PointDouble(x, y);
+          System.out.println(distance + "m, " + azimuth + " degrees     "
+              + center.x + ", " + center.y);
+        }
+      }
+      // Commented by HB to go past syntax errors
+      for (ParticleAnalysisReport report : reports) {
+        // Imgproc.rectangle(matOriginal, rec.br(), rec.tl(), BLACK);
+        // draw a rectangle on image matOriginal using bottom right vertex and
+        // top left vertex as input
+      }
+      // if there is only 1 target, then we have found the target we want
+      // output an image for debugging
+      // Imgcodecs.imwrite("output.png", matOriginal);
+      FrameCount++;
+    }
+    shouldRun = false;
+  }
+
+  /**
+   * @param angle
+   *          a nonnormalized angle
+   */
+  public static double normalize360(double angle) {
+    while (angle >= 360.0)
+    {
+      angle -= 360.0;
+    }
+    while (angle < 0.0)
+    {
+      angle += 360.0;
+    }
+    return angle;
+  }
+}