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