1 package org
.usfirst
.frc
.team3501
.robot
.subsystems
;
3 import java
.util
.ArrayList
;
4 import java
.util
.Iterator
;
6 import edu
.wpi
.first
.wpilibj
.networktables
.NetworkTable
;
14 * @author Elijah Kaufman
16 * @description Uses opencv and network table 3.0 to detect the vision targets
19 public class TowerTracker
{
22 * static method to load opencv and networkTables
28 System
.getProperty("java.library.path")
29 + ":/home/cindy/Robotics/Workspace/opencv-3.1.0/cmake/lib"); // path
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");
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);
48 // the size for resing the image
49 public static final Size resize
= new Size(320, 240);
52 public static VideoCapture videoCapture
;
53 public static Mat matOriginal
, matHSV
, matThresh
, clusters
, matHeirarchy
;
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;
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;
66 public static boolean shouldRun
= true;
71 * command line arguments
72 * just the main loop for the program and the entry points
74 public static void main(String
[] args
) {
75 // TODO Auto-generated method stub
76 matOriginal
= new Mat();
78 matThresh
= new Mat();
80 matHeirarchy
= new Mat();
81 // main loop of the program
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");
89 // cap.open("http://10.30.19.11/mjpg/video.mjpg");
90 // wait until it is opened
91 while (!videoCapture
.isOpened()) {
93 // time to actually process the acquired images
95 } catch (Exception e
) {
100 // make sure the java process quits when the loop finishes
101 videoCapture
.release();
107 * reads an image from a live image capture and outputs information to the
108 * SmartDashboard or a file
110 public static void processImage() {
111 ArrayList
<MatOfPoint
> contours
= new ArrayList
<MatOfPoint
>();
112 double x
, y
, targetX
, targetY
, distance
, azimuth
;
115 long before
= System
.currentTimeMillis();
116 // only run for the specified time
117 while (FrameCount
< 100) {
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
132 MatOfPoint matOfPoint
= iterator
.next();
133 Rect rec
= Imgproc
.boundingRect(matOfPoint
);
134 if (rec
.height
< 25 || rec
.width
< 25) {
138 float aspect
= (float) rec
.width
/ (float) rec
.height
;
140 iterator
.remove(); // remove contour if width < height
142 for (MatOfPoint mop
: contours
) {
143 Rect rec
= Imgproc
.boundingRect(mop
);
144 Imgproc
.rectangle(matOriginal
, rec
.br(), rec
.tl(), BLACK
);
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
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
);
170 // output an image for debugging
171 // Imgcodecs.imwrite("output.png", matOriginal);
179 * a nonnormalized angle
181 public static double normalize360(double angle
) {
182 while (angle
>= 360.0)