From 7e447ed528b13f5426bd1051618e3076ff6f6410 Mon Sep 17 00:00:00 2001 From: Kevin Zhang Date: Sat, 23 Jan 2016 17:42:37 -0800 Subject: [PATCH] convert opencv code to NI code --- .../robot/subsystems/TowerTracker2.java | 159 ++++++++++++++++++ 1 file changed, 159 insertions(+) create mode 100644 src/org/usfirst/frc/team3501/robot/subsystems/TowerTracker2.java diff --git a/src/org/usfirst/frc/team3501/robot/subsystems/TowerTracker2.java b/src/org/usfirst/frc/team3501/robot/subsystems/TowerTracker2.java new file mode 100644 index 00000000..3695e388 --- /dev/null +++ b/src/org/usfirst/frc/team3501/robot/subsystems/TowerTracker2.java @@ -0,0 +1,159 @@ +package org.usfirst.frc.team3501.robot.subsystems; + +import com.ni.vision.NIVision.PointDouble; + +import edu.wpi.first.wpilibj.image.BinaryImage; +import edu.wpi.first.wpilibj.image.HSLImage; +import edu.wpi.first.wpilibj.image.NIVisionException; +import edu.wpi.first.wpilibj.image.ParticleAnalysisReport; +import edu.wpi.first.wpilibj.networktables.NetworkTable; +import edu.wpi.first.wpilibj.vision.AxisCamera; + +/** + * + */ + +/** + * + * @author Elijah Kaufman + * @version 1.0 + * @description Uses opencv and network table 3.0 to detect the vision targets + * + */ +public class TowerTracker2 { + + /** + * static method to load opencv and networkTables + */ + static { + System.out.println(System.getProperty("java.library.path")); + System.loadLibrary("libopencv_java310.so"); + NetworkTable.setClientMode(); + NetworkTable.setIPAddress("roborio-3502.local"); + } + // constants for the color rbg values + // 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); + + public static HSLImage originalImage; + public static AxisCamera camera; + public static BinaryImage filteredImage; + + // 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 + NetworkTable table = NetworkTable.getTable("SmartDashboard"); + // main loop of the program + while (shouldRun) { + try { + // opens up the camera stream and tries to load it + // videoCapture = new VideoCapture(); + // videoCapture.open("http://10.35.01.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 + * + * @throws NIVisionException + */ + public static void processImage() throws NIVisionException { + // ArrayList contours = new ArrayList(); + 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) { + originalImage = camera.getImage(); + filteredImage = originalImage.thresholdHSL(328, 360, 100, 255, 43, 255); + ParticleAnalysisReport[] reports = filteredImage + .getOrderedParticleAnalysisReports(); + // make sure the contours that are detected are at least 20x20 + // pixels with an area of 400 and an aspect ration greater then 1 + // Commented by HB to go past syntax errors + for (ParticleAnalysisReport report : reports) { + if (report.boundingRectHeight > 25 && report.boundingRectWidth > 25 + && report.boundingRectWidth > report.boundingRectHeight) { + y = reports[0].boundingRectTop - reports[0].boundingRectHeight / 2; + y = -((2 * (y / originalImage.getHeight())) - 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 = reports[0].boundingRectLeft + reports[0].boundingRectWidth + / 2; + targetX = (2 * (targetX / originalImage.getWidth())) - 1; + azimuth = normalize360(targetX * HORIZONTAL_FOV / 2.0 + 0); + // drawing info on target + x = reports[0].boundingRectLeft + originalImage.getWidth() / 2; + y = reports[0].boundingRectTop - originalImage.getHeight() / 2; + PointDouble center = new PointDouble(x, y); + System.out.println(distance + "m, " + azimuth + " degrees " + + center.x + ", " + center.y); + } + } + // Commented by HB to go past syntax errors + for (ParticleAnalysisReport report : reports) { + // Imgproc.rectangle(matOriginal, rec.br(), rec.tl(), BLACK); + // draw a rectangle on image matOriginal using bottom right vertex and + // top left vertex as input + } + // if there is only 1 target, then we have found the target we want + // 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; + } +} -- 2.30.2