diff --git a/src/com/stuypulse/stuylib/network/SmartBoolean.java b/src/com/stuypulse/stuylib/network/SmartBoolean.java index fae9459e..052afc7d 100644 --- a/src/com/stuypulse/stuylib/network/SmartBoolean.java +++ b/src/com/stuypulse/stuylib/network/SmartBoolean.java @@ -32,7 +32,9 @@ public class SmartBoolean implements BStream { * @param value the default / initialization value for the value */ public SmartBoolean(String id, boolean value) { - mHandle = NetworkTablesJNI.getEntry(NetworkTablesJNI.getDefaultInstance(), "SmartDashboard/" + id); + mHandle = + NetworkTablesJNI.getEntry( + NetworkTablesJNI.getDefaultInstance(), "SmartDashboard/" + id); mDefaultValue = value; reset(); } diff --git a/src/com/stuypulse/stuylib/network/SmartNumber.java b/src/com/stuypulse/stuylib/network/SmartNumber.java index 32e22608..cd1acda1 100644 --- a/src/com/stuypulse/stuylib/network/SmartNumber.java +++ b/src/com/stuypulse/stuylib/network/SmartNumber.java @@ -34,7 +34,9 @@ public final class SmartNumber extends Number implements IStream { * @param value the default / initialization value for the value */ public SmartNumber(String id, double value) { - mHandle = NetworkTablesJNI.getEntry(NetworkTablesJNI.getDefaultInstance(), "SmartDashboard/" + id); + mHandle = + NetworkTablesJNI.getEntry( + NetworkTablesJNI.getDefaultInstance(), "SmartDashboard/" + id); mDefaultValue = value; reset(); } diff --git a/src/com/stuypulse/stuylib/network/SmartString.java b/src/com/stuypulse/stuylib/network/SmartString.java index e996a326..ce7b60b3 100644 --- a/src/com/stuypulse/stuylib/network/SmartString.java +++ b/src/com/stuypulse/stuylib/network/SmartString.java @@ -31,7 +31,9 @@ public class SmartString implements Supplier { * @param value the default / initialization value for the value */ public SmartString(String id, String value) { - mHandle = NetworkTablesJNI.getEntry(NetworkTablesJNI.getDefaultInstance(), "SmartDashboard/" + value); + mHandle = + NetworkTablesJNI.getEntry( + NetworkTablesJNI.getDefaultInstance(), "SmartDashboard/" + value); mDefaultValue = value; reset(); } diff --git a/src/com/stuypulse/stuylib/streams/filters/DriveFilter.java b/src/com/stuypulse/stuylib/streams/filters/DriveFilter.java new file mode 100644 index 00000000..9d4f9c33 --- /dev/null +++ b/src/com/stuypulse/stuylib/streams/filters/DriveFilter.java @@ -0,0 +1,56 @@ +/* Copyright (c) 2023 StuyPulse Robotics. All rights reserved. */ +/* This work is licensed under the terms of the MIT license */ +/* found in the root directory of this project. */ + +package com.stuypulse.stuylib.streams.filters; + +/** + * A simple low-pass filter for smoothing joystick input values in robot driving code. + * + *

The filter helps robot drivers drive more smoothly, by smoothing out noisy input values from a + * controller, by filtering out high-frequency components, while perserving low-frequency + * components. + * + *

The filter is implemented as a first-order IIR filter with a configurable filter coefficient. + * The closer this value is to one, the more the filter represents a simple averaging filter, the + * closer this value is to zero, the more it represents a simple delay filter. Not every robot or + * driver should use the same coefficient, the team should determine the best coefficient for their + * driver through trial-and-error. + * + *

The filter state is initialized to zero when a new instance is created, and the filter + * function should be called repeatedly with new input values to obtain the filtered output. The + * filter function returns the filtered output value, which can be used as the input to the robot + * driving code. + * + * @author Mansour Quddus (mansourquddus@protonmail.com) (@devmanso on github) + */ +public class DriveFilter { + private double last; + + /** + * This implements a simple low-pass filter, it is meant to be used in your driving code. It + * helps robot drivers drive more smoothly, by smoothing out noisy input values from a + * controller, by filtering out high-frequency components, while perserving low-frequency + * components. + */ + public DriveFilter() { + last = 0.0; + } + + /** + * @param next - this should be the joystick input axis, i.e: the left or right axis of an xBox + * game controller + * @param coefficient - the constant value that represents the filter coefficient and determines + * the amount of filtering applied to the input value. + *

Not every robot or driver should use the same coefficient, your team should determine + * the best coefficient for your driver through trial-and-error. + *

The closer this value is to one, the more the filter represents a simple averaging + * filter, the closer this value is to zero, the more it represents a simple delay filter. + * @return the filtered value + */ + public double get(double next, double coefficient) { + double sign = (next > 0) ? 1 : -1; + next *= next * sign; + return last += (next - last) * (1 - coefficient); + } +} diff --git a/src/com/stuypulse/stuylib/util/RioAprilTagDetector.java b/src/com/stuypulse/stuylib/util/RioAprilTagDetector.java new file mode 100644 index 00000000..14b10fe7 --- /dev/null +++ b/src/com/stuypulse/stuylib/util/RioAprilTagDetector.java @@ -0,0 +1,203 @@ +/* Copyright (c) 2023 StuyPulse Robotics. All rights reserved. */ +/* This work is licensed under the terms of the MIT license */ +/* found in the root directory of this project. */ + +package com.stuypulse.stuylib.util; + +import edu.wpi.first.apriltag.AprilTagDetector; +import edu.wpi.first.cameraserver.CameraServer; +import edu.wpi.first.wpilibj.Timer; + +import java.util.HashSet; +import org.opencv.core.Mat; +import org.opencv.core.Point; +import org.opencv.core.Scalar; +import org.opencv.imgproc.Imgproc; + +/** + * This class is designed to detect AprilTags with OpenCV on a RoboRIO. It has + * methods for detecting AprilTags via a USB camera connected to any of the USB ports on the + * RoboRIO. As well as helper functions that retrieve some data from the detection. + * + *

The detector is optimized to use as little memory and processor power as possible, through the + * use of sets (avoids duplication of data), and a seperate Thread to perform the vision processing. + * + * @author Mansour Quddus (mansourquddus@protonmail.com) + */ +public class RioAprilTagDetector { + + static int height; + static int width; + static String name; + static int minClusterPixels; + static double criticalAngle; + + static int detectionResults; + static int detectionsPerSecond; + + /** + * Creates a new RioAprilTagDetection + * + * @param cameraWidth - width in pixels + * @param cameraHeight - height in pixels + * @param cameraName - name of the camera shown in SmartDashboard + * @param minClusterPix - minimum size of pixels of an AprilTag + * @param critAngle + */ + public RioAprilTagDetector( + int cameraWidth, + int cameraHeight, + String cameraName, + int minClusterPix, + double critAngle) { + height = cameraHeight; + width = cameraWidth; + name = cameraName; + + minClusterPixels = minClusterPix; + criticalAngle = critAngle; + } + + static Thread aprilTagDetectionThread; + + /** + * call this method in robotInit() + * + *

RoboRIO 2 is reccomended for best performance with the RIO. + * + *

Vision processing on the RoboRIO is computationally expensive, so don't expect performance + * to be stable. Try using a lower camera resolution to improve fps, and AprilTag detection, + * 16:9 ratio is reccomended, but all others should work. + * + *

This assumes that a Camera, like a Logitech C270, is connected to any of the RoboRIO's USB + * ports. + */ + public static void detectAprilTags() { + aprilTagDetectionThread = + new Thread( + () -> { + var camera = CameraServer.startAutomaticCapture(); + camera.setResolution(width, height); + + var cvSink = CameraServer.getVideo(); + var outputStream = CameraServer.putVideo(name, width, height); + + // Mats are memory expensive, it is best to use as little as possible + var mat = new Mat(); + var grayMat = new Mat(); + + // points that connect all corners of AprilTag and draw a rectangle + var pt0 = new Point(); + var pt1 = new Point(); + var pt2 = new Point(); + var pt3 = new Point(); + var center = new Point(); + var red = new Scalar(0, 0, 255); + var green = new Scalar(0, 255, 0); + + var aprilTagDetector = new AprilTagDetector(); + + var config = aprilTagDetector.getConfig(); + config.quadSigma = 0.8f; + aprilTagDetector.setConfig(config); + + var quadThreshParams = aprilTagDetector.getQuadThresholdParameters(); + quadThreshParams.minClusterPixels = minClusterPixels; // default is 400 + quadThreshParams.criticalAngle *= criticalAngle; // default is 10 or 5 + quadThreshParams.maxLineFitMSE *= 1.5; + + aprilTagDetector.setQuadThresholdParameters(quadThreshParams); + + // the AprilTag detector can only detect one family at a time + // to detect multiple tag fam's we must use multiple AprilTagDetectors + // this leads worse performance as AprilTagDetectors are expensive to + // setup + aprilTagDetector.addFamily("tag16h5"); + + var timer = new Timer(); + timer.start(); + var count = 0; + + // this can never be true the robot must be off for this to be true + while (!Thread.interrupted()) { + if (cvSink.grabFrame(mat) == 0) { + outputStream.notifyError(cvSink.getError()); + continue; + } + + // convert image to grayscale + Imgproc.cvtColor(mat, grayMat, Imgproc.COLOR_RGB2GRAY); + + var results = aprilTagDetector.detect(grayMat); + + var set = new HashSet<>(); + + for (var result : results) { + count += 1; + + detectionResults = result.getId(); + + pt0.x = result.getCornerX(0); + pt1.x = result.getCornerX(1); + pt2.x = result.getCornerX(2); + pt3.x = result.getCornerX(3); + + pt0.y = result.getCornerY(0); + pt1.y = result.getCornerY(1); + pt2.y = result.getCornerY(2); + pt3.y = result.getCornerY(3); + + center.x = result.getCenterX(); + center.y = result.getCenterY(); + + set.add(result.getId()); + + // Imgproc doesn't have a square/rectangle member function + // so we must use this awkward ass way of drawing squares around + // the tag + Imgproc.line(mat, pt0, pt1, red, 5); + Imgproc.line(mat, pt1, pt2, red, 5); + Imgproc.line(mat, pt2, pt3, red, 5); + Imgproc.line(mat, pt3, pt0, red, 5); + + Imgproc.circle(mat, center, 4, green); + // print id (number) of the tag + Imgproc.putText( + mat, + String.valueOf(result.getId()), + pt2, + Imgproc.FONT_HERSHEY_SIMPLEX, + 2, + green, + 7); + } + ; + + for (var id : set) { + System.out.println("Tag: " + String.valueOf(id)); + } + + if (timer.advanceIfElapsed(1.0)) { + detectionsPerSecond = count; + System.out.println( + "detections per second: " + String.valueOf(count)); + count = 0; + } + + outputStream.putFrame(mat); + } + + aprilTagDetector.close(); + }); + aprilTagDetectionThread.setDaemon(true); + aprilTagDetectionThread.start(); + } + + public int getAprilTagId() { + return detectionResults; + } + + public int getDetectionsPerSecond() { + return detectionsPerSecond; + } +}