delete second potentiometer for defense hand

diff --git a/src/org/usfirst/frc/team3501/robot/subsystems/DefenseArm.java b/src/org/usfirst/frc/team3501/robot/subsystems/DefenseArm.java
index b9fd25bc275f9127070a5904aa2f1c5fdb5af827..8e49508981cc1bc5d7a2444787b567b421758b5f 100755 (executable)
--- a/src/org/usfirst/frc/team3501/robot/subsystems/DefenseArm.java
+++ b/src/org/usfirst/frc/team3501/robot/subsystems/DefenseArm.java
@@ -12,6 +12,7 @@ public class DefenseArm extends Subsystem {
   public AnalogPotentiometer defenseHandPotentiometer;\r
   public CANTalon defenseArmMotor;\r
   public CANTalon defenseHandMotor;\r
+  public double hookHeight;\r
 \r
   // Defense arm specific constants that relate to the degrees per pulse value\r
   // for the potentiometers\r
@@ -19,6 +20,7 @@ public class DefenseArm extends Subsystem {
   public final static double FULL_RANGE = 270.0; // in degrees\r
   public final static double OFFSET = -135.0; // in degrees\r
   public final static double[] armPotValue = { 0.0, 45.0, 90.0 }; // 3 level\r
+\r
   // array;\r
 \r
   // do we want to use a hashmap??\r
@@ -29,12 +31,24 @@ public class DefenseArm extends Subsystem {
     defenseArmPotentiometer = new AnalogPotentiometer(\r
         Constants.DefenseArm.ARM_CHANNEL,\r
         FULL_RANGE, OFFSET);\r
-    defenseHandPotentiometer = new AnalogPotentiometer(\r
-        Constants.DefenseArm.HAND_CHANNEL);\r
+    Constants.DefenseArm.HAND_CHANNEL);\r
     defenseArmMotor = new CANTalon(Constants.DefenseArm.ARM_PORT);\r
     defenseHandMotor = new CANTalon(Constants.DefenseArm.HAND_PORT);\r
   }\r
 \r
+  /***\r
+   * This method gets the height of the hook from the ground. The hook is\r
+   * attached to the end of the hand, which is attached to the arm.\r
+   * \r
+   * @return hookHeight gets height of hook from ground. The hook is attached to\r
+   *         the end of the hand, which is attached the arm.\r
+   * \r
+   */\r
+\r
+  public double getHookHeight() {\r
+    return hookHeight;\r
+  }\r
+\r
   @Override\r
   protected void initDefaultCommand() {\r
   }\r

diff --git a/src/org/usfirst/frc/team3501/robot/subsystems/TowerTracker.java b/src/org/usfirst/frc/team3501/robot/subsystems/TowerTracker.java
new file mode 100755 (executable)
index 0000000..1105371
--- /dev/null
+++ b/src/org/usfirst/frc/team3501/robot/subsystems/TowerTracker.java
@@ -0,0 +1,192 @@
+package org.usfirst.frc.team3501.robot.subsystems;
+
+import java.util.ArrayList;
+import java.util.Iterator;
+
+import edu.wpi.first.wpilibj.networktables.NetworkTable;
+
+/**
+ *
+ */
+
+/**
+ *
+ * @author Elijah Kaufman
+ * @version 1.0
+ * @description Uses opencv and network table 3.0 to detect the vision targets
+ *
+ */
+public class TowerTracker {
+
+  /**
+   * static method to load opencv and networkTables
+   */
+  static {
+    System
+        .setProperty(
+        "java.library.path",
+        System.getProperty("java.library.path")
+        + ":/home/cindy/Robotics/Workspace/opencv-3.1.0/cmake/lib"); // path
+    // to
+    // opencv_java310
+    // System.out.println(System.getProperty("java.library.path"));
+    System.loadLibrary(Core.NATIVE_LIBRARY_NAME);
+    NetworkTable.setClientMode();
+    NetworkTable.setIPAddress("roborio-3502.local");
+  }
+  // constants for the color rbg values
+  public static final Scalar
+  RED = new Scalar(0, 0, 255),
+  BLUE = new Scalar(255, 0, 0),
+  GREEN = new Scalar(0, 255, 0),
+  BLACK = new Scalar(0, 0, 0),
+  YELLOW = new Scalar(0, 255, 255),
+  // 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);
+
+  // ignore these
+  public static VideoCapture videoCapture;
+  public static Mat matOriginal, matHSV, matThresh, clusters, matHeirarchy;
+
+  // 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
+    matOriginal = new Mat();
+    matHSV = new Mat();
+    matThresh = new Mat();
+    clusters = new Mat();
+    matHeirarchy = new Mat();
+    // main loop of the program
+    while (shouldRun) {
+      try {
+        // opens up the camera stream and tries to load it
+        videoCapture = new VideoCapture();
+        // replaces the ##.## with your team number
+        videoCapture.open("http://10.35.01.11/mjpg/video.mjpg");
+        // Example
+        // cap.open("http://10.30.19.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
+   */
+  public static void processImage() {
+    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) {
+      contours.clear();
+      // capture from the axis camera
+      videoCapture.read(matOriginal);
+      // captures from a static file for testing
+      // matOriginal = Imgcodecs.imread("someFile.png");
+      Imgproc.cvtColor(matOriginal, matHSV, Imgproc.COLOR_BGR2HSV);
+      Core.inRange(matHSV, LOWER_BOUNDS, UPPER_BOUNDS, matThresh);
+      Imgproc.findContours(matThresh, contours, matHeirarchy,
+          Imgproc.RETR_EXTERNAL,
+          Imgproc.CHAIN_APPROX_SIMPLE);
+      // make sure the contours that are detected are at least 20x20
+      // pixels with an area of 400 and an aspect ration greater then 1
+      for (Iterator<MatOfPoint> iterator = contours.iterator(); iterator
+          .hasNext();) {
+        MatOfPoint matOfPoint = iterator.next();
+        Rect rec = Imgproc.boundingRect(matOfPoint);
+        if (rec.height < 25 || rec.width < 25) {
+          iterator.remove();
+          continue;
+        }
+        float aspect = (float) rec.width / (float) rec.height;
+        if (aspect < 1.0)
+          iterator.remove(); // remove contour if width < height
+      }
+      for (MatOfPoint mop : contours) {
+        Rect rec = Imgproc.boundingRect(mop);
+        Imgproc.rectangle(matOriginal, rec.br(), rec.tl(), BLACK);
+      }
+      // if there is only 1 target, then we have found the target we want
+      if (contours.size() == 1) {
+        Rect rec = Imgproc.boundingRect(contours.get(0));
+        // "fun" math brought to you by miss daisy (team 341)!
+        y = rec.br().y + rec.height / 2;
+        y = -((2 * (y / matOriginal.height())) - 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 = rec.tl().x + rec.width / 2;
+        targetX = (2 * (targetX / matOriginal.width())) - 1;
+        azimuth = normalize360(targetX * HORIZONTAL_FOV / 2.0 + 0);
+        // drawing info on target
+        Point center = new Point(rec.br().x - rec.width / 2 - 15, rec.br().y
+            - rec.height / 2);
+        Point centerw = new Point(rec.br().x - rec.width / 2 - 15, rec.br().y
+            - rec.height / 2 - 20);
+        // Imgproc.putText(matOriginal, "" + (int) distance, center,
+        // Core.FONT_HERSHEY_PLAIN, 1, BLACK);
+        // Imgproc.putText(matOriginal, "" + (int) azimuth, centerw,
+        // Core.FONT_HERSHEY_PLAIN, 1, BLACK);
+        System.out.println(distance + "m, " + azimuth + " degrees     "
+            + center.x + ", " + center.y);
+      }
+      // 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;
+  }
+}