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