challenge-bot.com Git - 3501/stronghold-2016/blob - src/org/usfirst/frc/team3501/robot/subsystems/TowerTracker.java

   1 package org.usfirst.frc.team3501.robot.subsystems;
   2
   3 import java.util.ArrayList;
   4 import java.util.Iterator;
   5
   6 import edu.wpi.first.wpilibj.networktables.NetworkTable;
   7
   8 /**
   9  *
  10  */
  11
  12 /**
  13  *
  14  * @author Elijah Kaufman
  15  * @version 1.0
  16  * @description Uses opencv and network table 3.0 to detect the vision targets
  17  *
  18  */
  19 public class TowerTracker {
  20
  21   /**
  22    * static method to load opencv and networkTables
  23    */
  24   static {
  25     System
  26         .setProperty(
  27         "java.library.path",
  28         System.getProperty("java.library.path")
  29         + ":/home/cindy/Robotics/Workspace/opencv-3.1.0/cmake/lib"); // path
  30     // to
  31     // opencv_java310
  32     // System.out.println(System.getProperty("java.library.path"));
  33     System.loadLibrary(Core.NATIVE_LIBRARY_NAME);
  34     NetworkTable.setClientMode();
  35     NetworkTable.setIPAddress("roborio-3502.local");
  36   }
  37   // constants for the color rbg values
  38   public static final Scalar
  39   RED = new Scalar(0, 0, 255),
  40   BLUE = new Scalar(255, 0, 0),
  41   GREEN = new Scalar(0, 255, 0),
  42   BLACK = new Scalar(0, 0, 0),
  43   YELLOW = new Scalar(0, 255, 255),
  44   // these are the threshold values in order
  45   LOWER_BOUNDS = new Scalar(58, 0, 109),
  46   UPPER_BOUNDS = new Scalar(93, 255, 240);
  47
  48   // the size for resing the image
  49   public static final Size resize = new Size(320, 240);
  50
  51   // ignore these
  52   public static VideoCapture videoCapture;
  53   public static Mat matOriginal, matHSV, matThresh, clusters, matHeirarchy;
  54
  55   // Constants for known variables
  56   // the height to the top of the target in first stronghold is 97 inches
  57   public static final int TOP_TARGET_HEIGHT = 97;
  58   // the physical height of the camera lens
  59   public static final int TOP_CAMERA_HEIGHT = 32;
  60
  61   // camera details, can usually be found on the datasheets of the camera
  62   public static final double VERTICAL_FOV = 51;
  63   public static final double HORIZONTAL_FOV = 67;
  64   public static final double CAMERA_ANGLE = 10;
  65
  66   public static boolean shouldRun = true;
  67
  68   /**
  69    *
  70    * @param args
  71    *          command line arguments
  72    *          just the main loop for the program and the entry points
  73    */
  74   public static void main(String[] args) {
  75     // TODO Auto-generated method stub
  76     matOriginal = new Mat();
  77     matHSV = new Mat();
  78     matThresh = new Mat();
  79     clusters = new Mat();
  80     matHeirarchy = new Mat();
  81     // main loop of the program
  82     while (shouldRun) {
  83       try {
  84         // opens up the camera stream and tries to load it
  85         videoCapture = new VideoCapture();
  86         // replaces the ##.## with your team number
  87         videoCapture.open("http://10.35.01.11/mjpg/video.mjpg");
  88         // Example
  89         // cap.open("http://10.30.19.11/mjpg/video.mjpg");
  90         // wait until it is opened
  91         while (!videoCapture.isOpened()) {
  92         }
  93         // time to actually process the acquired images
  94         processImage();
  95       } catch (Exception e) {
  96         e.printStackTrace();
  97         break;
  98       }
  99     }
 100     // make sure the java process quits when the loop finishes
 101     videoCapture.release();
 102     System.exit(0);
 103   }
 104
 105   /**
 106    *
 107    * reads an image from a live image capture and outputs information to the
 108    * SmartDashboard or a file
 109    */
 110   public static void processImage() {
 111     ArrayList<MatOfPoint> contours = new ArrayList<MatOfPoint>();
 112     double x, y, targetX, targetY, distance, azimuth;
 113     // frame counter
 114     int FrameCount = 0;
 115     long before = System.currentTimeMillis();
 116     // only run for the specified time
 117     while (FrameCount < 100) {
 118       contours.clear();
 119       // capture from the axis camera
 120       videoCapture.read(matOriginal);
 121       // captures from a static file for testing
 122       // matOriginal = Imgcodecs.imread("someFile.png");
 123       Imgproc.cvtColor(matOriginal, matHSV, Imgproc.COLOR_BGR2HSV);
 124       Core.inRange(matHSV, LOWER_BOUNDS, UPPER_BOUNDS, matThresh);
 125       Imgproc.findContours(matThresh, contours, matHeirarchy,
 126           Imgproc.RETR_EXTERNAL,
 127           Imgproc.CHAIN_APPROX_SIMPLE);
 128       // make sure the contours that are detected are at least 20x20
 129       // pixels with an area of 400 and an aspect ration greater then 1
 130       for (Iterator<MatOfPoint> iterator = contours.iterator(); iterator
 131           .hasNext();) {
 132         MatOfPoint matOfPoint = iterator.next();
 133         Rect rec = Imgproc.boundingRect(matOfPoint);
 134         if (rec.height < 25 || rec.width < 25) {
 135           iterator.remove();
 136           continue;
 137         }
 138         float aspect = (float) rec.width / (float) rec.height;
 139         if (aspect < 1.0)
 140           iterator.remove(); // remove contour if width < height
 141       }
 142       for (MatOfPoint mop : contours) {
 143         Rect rec = Imgproc.boundingRect(mop);
 144         Imgproc.rectangle(matOriginal, rec.br(), rec.tl(), BLACK);
 145       }
 146       // if there is only 1 target, then we have found the target we want
 147       if (contours.size() == 1) {
 148         Rect rec = Imgproc.boundingRect(contours.get(0));
 149         // "fun" math brought to you by miss daisy (team 341)!
 150         y = rec.br().y + rec.height / 2;
 151         y = -((2 * (y / matOriginal.height())) - 1);
 152         distance = (TOP_TARGET_HEIGHT - TOP_CAMERA_HEIGHT) /
 153             Math.tan((y * VERTICAL_FOV / 2.0 + CAMERA_ANGLE) * Math.PI / 180);
 154         // angle to target...would not rely on this
 155         targetX = rec.tl().x + rec.width / 2;
 156         targetX = (2 * (targetX / matOriginal.width())) - 1;
 157         azimuth = normalize360(targetX * HORIZONTAL_FOV / 2.0 + 0);
 158         // drawing info on target
 159         Point center = new Point(rec.br().x - rec.width / 2 - 15, rec.br().y
 160             - rec.height / 2);
 161         Point centerw = new Point(rec.br().x - rec.width / 2 - 15, rec.br().y
 162             - rec.height / 2 - 20);
 163         // Imgproc.putText(matOriginal, "" + (int) distance, center,
 164         // Core.FONT_HERSHEY_PLAIN, 1, BLACK);
 165         // Imgproc.putText(matOriginal, "" + (int) azimuth, centerw,
 166         // Core.FONT_HERSHEY_PLAIN, 1, BLACK);
 167         System.out.println(distance + "m, " + azimuth + " degrees     "
 168             + center.x + ", " + center.y);
 169       }
 170       // output an image for debugging
 171       // Imgcodecs.imwrite("output.png", matOriginal);
 172       FrameCount++;
 173     }
 174     shouldRun = false;
 175   }
 176
 177   /**
 178    * @param angle
 179    *          a nonnormalized angle
 180    */
 181   public static double normalize360(double angle) {
 182     while (angle >= 360.0)
 183     {
 184       angle -= 360.0;
 185     }
 186     while (angle < 0.0)
 187     {
 188       angle += 360.0;
 189     }
 190     return angle;
 191   }
 192 }