From f661626fd47ad3fa3b6c744e84c6d0b4a4130857 Mon Sep 17 00:00:00 2001 From: Yamini Adusumelli Date: Thu, 28 Jan 2016 19:13:58 -0800 Subject: [PATCH] delete second potentiometer for defense hand --- .../team3501/robot/subsystems/DefenseArm.java | 18 +- .../robot/subsystems/TowerTracker.java | 192 ++++++++++++++++++ 2 files changed, 208 insertions(+), 2 deletions(-) create mode 100755 src/org/usfirst/frc/team3501/robot/subsystems/TowerTracker.java diff --git a/src/org/usfirst/frc/team3501/robot/subsystems/DefenseArm.java b/src/org/usfirst/frc/team3501/robot/subsystems/DefenseArm.java index b9fd25bc..8e495089 100755 --- a/src/org/usfirst/frc/team3501/robot/subsystems/DefenseArm.java +++ b/src/org/usfirst/frc/team3501/robot/subsystems/DefenseArm.java @@ -12,6 +12,7 @@ public class DefenseArm extends Subsystem { public AnalogPotentiometer defenseHandPotentiometer; public CANTalon defenseArmMotor; public CANTalon defenseHandMotor; + public double hookHeight; // Defense arm specific constants that relate to the degrees per pulse value // for the potentiometers @@ -19,6 +20,7 @@ public class DefenseArm extends Subsystem { public final static double FULL_RANGE = 270.0; // in degrees public final static double OFFSET = -135.0; // in degrees public final static double[] armPotValue = { 0.0, 45.0, 90.0 }; // 3 level + // array; // do we want to use a hashmap?? @@ -29,12 +31,24 @@ public class DefenseArm extends Subsystem { defenseArmPotentiometer = new AnalogPotentiometer( Constants.DefenseArm.ARM_CHANNEL, FULL_RANGE, OFFSET); - defenseHandPotentiometer = new AnalogPotentiometer( - Constants.DefenseArm.HAND_CHANNEL); + Constants.DefenseArm.HAND_CHANNEL); defenseArmMotor = new CANTalon(Constants.DefenseArm.ARM_PORT); defenseHandMotor = new CANTalon(Constants.DefenseArm.HAND_PORT); } + /*** + * This method gets the height of the hook from the ground. The hook is + * attached to the end of the hand, which is attached to the arm. + * + * @return hookHeight gets height of hook from ground. The hook is attached to + * the end of the hand, which is attached the arm. + * + */ + + public double getHookHeight() { + return hookHeight; + } + @Override protected void initDefaultCommand() { } diff --git a/src/org/usfirst/frc/team3501/robot/subsystems/TowerTracker.java b/src/org/usfirst/frc/team3501/robot/subsystems/TowerTracker.java new file mode 100755 index 00000000..1105371d --- /dev/null +++ b/src/org/usfirst/frc/team3501/robot/subsystems/TowerTracker.java @@ -0,0 +1,192 @@ +package org.usfirst.frc.team3501.robot.subsystems; + +import java.util.ArrayList; +import java.util.Iterator; + +import edu.wpi.first.wpilibj.networktables.NetworkTable; + +/** + * + */ + +/** + * + * @author Elijah Kaufman + * @version 1.0 + * @description Uses opencv and network table 3.0 to detect the vision targets + * + */ +public class TowerTracker { + + /** + * static method to load opencv and networkTables + */ + static { + System + .setProperty( + "java.library.path", + System.getProperty("java.library.path") + + ":/home/cindy/Robotics/Workspace/opencv-3.1.0/cmake/lib"); // path + // to + // opencv_java310 + // System.out.println(System.getProperty("java.library.path")); + System.loadLibrary(Core.NATIVE_LIBRARY_NAME); + NetworkTable.setClientMode(); + NetworkTable.setIPAddress("roborio-3502.local"); + } + // constants for the color rbg values + public static final Scalar + RED = new Scalar(0, 0, 255), + BLUE = new Scalar(255, 0, 0), + GREEN = new Scalar(0, 255, 0), + BLACK = new Scalar(0, 0, 0), + YELLOW = new Scalar(0, 255, 255), + // 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); + + // ignore these + public static VideoCapture videoCapture; + public static Mat matOriginal, matHSV, matThresh, clusters, matHeirarchy; + + // 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 + matOriginal = new Mat(); + matHSV = new Mat(); + matThresh = new Mat(); + clusters = new Mat(); + matHeirarchy = new Mat(); + // main loop of the program + while (shouldRun) { + try { + // opens up the camera stream and tries to load it + videoCapture = new VideoCapture(); + // replaces the ##.## with your team number + videoCapture.open("http://10.35.01.11/mjpg/video.mjpg"); + // Example + // cap.open("http://10.30.19.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 + */ + public static void processImage() { + 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) { + contours.clear(); + // capture from the axis camera + videoCapture.read(matOriginal); + // captures from a static file for testing + // matOriginal = Imgcodecs.imread("someFile.png"); + Imgproc.cvtColor(matOriginal, matHSV, Imgproc.COLOR_BGR2HSV); + Core.inRange(matHSV, LOWER_BOUNDS, UPPER_BOUNDS, matThresh); + Imgproc.findContours(matThresh, contours, matHeirarchy, + Imgproc.RETR_EXTERNAL, + Imgproc.CHAIN_APPROX_SIMPLE); + // make sure the contours that are detected are at least 20x20 + // pixels with an area of 400 and an aspect ration greater then 1 + for (Iterator iterator = contours.iterator(); iterator + .hasNext();) { + MatOfPoint matOfPoint = iterator.next(); + Rect rec = Imgproc.boundingRect(matOfPoint); + if (rec.height < 25 || rec.width < 25) { + iterator.remove(); + continue; + } + float aspect = (float) rec.width / (float) rec.height; + if (aspect < 1.0) + iterator.remove(); // remove contour if width < height + } + for (MatOfPoint mop : contours) { + Rect rec = Imgproc.boundingRect(mop); + Imgproc.rectangle(matOriginal, rec.br(), rec.tl(), BLACK); + } + // if there is only 1 target, then we have found the target we want + if (contours.size() == 1) { + Rect rec = Imgproc.boundingRect(contours.get(0)); + // "fun" math brought to you by miss daisy (team 341)! + y = rec.br().y + rec.height / 2; + y = -((2 * (y / matOriginal.height())) - 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 = rec.tl().x + rec.width / 2; + targetX = (2 * (targetX / matOriginal.width())) - 1; + azimuth = normalize360(targetX * HORIZONTAL_FOV / 2.0 + 0); + // drawing info on target + Point center = new Point(rec.br().x - rec.width / 2 - 15, rec.br().y + - rec.height / 2); + Point centerw = new Point(rec.br().x - rec.width / 2 - 15, rec.br().y + - rec.height / 2 - 20); + // Imgproc.putText(matOriginal, "" + (int) distance, center, + // Core.FONT_HERSHEY_PLAIN, 1, BLACK); + // Imgproc.putText(matOriginal, "" + (int) azimuth, centerw, + // Core.FONT_HERSHEY_PLAIN, 1, BLACK); + System.out.println(distance + "m, " + azimuth + " degrees " + + center.x + ", " + center.y); + } + // 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