From f5b45913cca9d36deda804e6d32dcd517c949f3b Mon Sep 17 00:00:00 2001 From: Deven Speyrer Date: Thu, 21 Nov 2024 17:34:53 -0700 Subject: [PATCH] Implement auto-shooting NOTE: there is currently a race condition which may cause the shooter to be stuck on until one of its controls are pressed. --- src/main/java/frc/Robot.java | 49 +++++++++-- src/main/java/frc/system/Shooter.java | 45 +++++----- src/main/java/frc/system/Vision.java | 122 ++++++++++++++++++-------- 3 files changed, 151 insertions(+), 65 deletions(-) diff --git a/src/main/java/frc/Robot.java b/src/main/java/frc/Robot.java index 519c5e4..efa227e 100644 --- a/src/main/java/frc/Robot.java +++ b/src/main/java/frc/Robot.java @@ -4,7 +4,9 @@ package frc; +import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj.TimedRobot; @@ -19,11 +21,14 @@ public class Robot extends TimedRobot { private final Swerve drivetrain = SwerveConfig.swerve; private final Intake intake = new Intake(); - private final Vision vision = new Vision(); - private final Shooter shooterLeft = new Shooter(15); private final Shooter shooterRight = new Shooter(16); + private final Vision vision = new Vision(new Vision.Target[] { + new Vision.Target(0.2, 0.2, 0.3, 0.3, 200, shooterLeft.runHiInterrupt()), + new Vision.Target(0.5, 0.5, 0.3, 0.3, 200, shooterRight.runHiInterrupt()) + }); + // Utilities static double squareInput(double input) { return Math.copySign(input * input, input); @@ -32,18 +37,46 @@ static double squareInput(double input) { @Override public void robotInit() { // Configure our controller bindings. + + // Driving drivetrain.setDefaultCommand(drivetrain.driveDutyCycle( () -> squareInput(driver.getLeftY()), () -> squareInput(driver.getLeftX()), // Note: rotation does not need to be inverted based on alliance side, since // rotational direction is not orientation-dependent. () -> -driver.getRightX())); - - driver.rightTrigger().whileTrue(intake.runOut()); - - driver.leftBumper().onTrue(drivetrain.zeroGyro()); - - driver.x().whileTrue(drivetrain.brake()); + driver.start().onTrue(drivetrain.zeroGyro()); + driver.x().whileTrue(drivetrain.brake()); + + // Intake + driver.leftTrigger().whileTrue(intake.runIn()); + driver.a().whileTrue(intake.runOut()); + + // Shooter + driver.rightTrigger().whileTrue(new ParallelCommandGroup( + shooterLeft.runLo(), + shooterRight.runLo() + )); + driver.rightBumper().whileTrue(new ParallelCommandGroup( + shooterLeft.runHi(), + shooterRight.runHi() + )); + + Command autoShooter = new Command() { + @Override + public void initialize() { + // Start the vision thread + vision.start(); + } + + @Override + public void end(boolean interrupted) { + // Kill the vision thread + vision.requestStop(); + } + }.alongWith(shooterLeft.runLo(), shooterRight.runLo()); + + driver.leftBumper().whileTrue(autoShooter); } @Override diff --git a/src/main/java/frc/system/Shooter.java b/src/main/java/frc/system/Shooter.java index d2f24f3..c876620 100644 --- a/src/main/java/frc/system/Shooter.java +++ b/src/main/java/frc/system/Shooter.java @@ -12,7 +12,8 @@ public class Shooter implements Subsystem { // Hardware private final TalonSRX shooterMotor; - private double defaultShooterSpeed = .8; + private double hi = 0.8; + private double lo = 0.2; public Shooter(int id) { // Motor @@ -33,17 +34,14 @@ public Shooter(int id) { register(); } - private Command feed(boolean forwards, double percentMaxSpeed) { + private Command feed(double speed) { Command cmd = new Command() { public void initialize() { - double speed = forwards ? defaultShooterSpeed : -defaultShooterSpeed; - speed *= percentMaxSpeed; - shooterMotor.set(TalonSRXControlMode.PercentOutput, speed); } public void end(boolean interrupted) { - disable(); + shooterMotor.set(TalonSRXControlMode.Disabled, 0); } }; @@ -52,25 +50,30 @@ public void end(boolean interrupted) { return cmd; } - private void disable() { - shooterMotor.set(TalonSRXControlMode.Disabled, 0); + public Command runHi() { + return feed(hi); } - /** - * returns a command that runs the Shooter FORWARD - * when it ends it disables the Shooter - * Shooter command requires itself - */ - public Command runIn() { - return feed(true, 1); + public Command runLo() { + return feed(lo); } - /** - * returns a command that runs the Shooter SLOWER - * when it ends it disables the Shooter - * Shooter command requires itself + /* + * Runs the shooter to the high speed, lowering to the low speed on end. */ - public Command runOut() { - return feed(true, .8); + public Command runHiInterrupt() { + Command cmd = new Command() { + public void initialize() { + shooterMotor.set(TalonSRXControlMode.PercentOutput, hi); + } + + public void end(boolean interrupted) { + shooterMotor.set(TalonSRXControlMode.PercentOutput, lo); + } + }; + + cmd.addRequirements(this); + + return cmd; } } diff --git a/src/main/java/frc/system/Vision.java b/src/main/java/frc/system/Vision.java index 8468b46..5498985 100644 --- a/src/main/java/frc/system/Vision.java +++ b/src/main/java/frc/system/Vision.java @@ -1,50 +1,100 @@ package frc.system; +import java.util.concurrent.atomic.AtomicBoolean; + import org.opencv.core.Core; import org.opencv.core.Mat; +import org.opencv.core.Rect; import org.opencv.core.Scalar; +import org.opencv.imgproc.Imgproc; import edu.wpi.first.cameraserver.CameraServer; import edu.wpi.first.cscore.CvSink; import edu.wpi.first.cscore.CvSource; +import edu.wpi.first.wpilibj2.command.Command; + +public class Vision extends Thread { + final Target[] targets; + final AtomicBoolean stop = new AtomicBoolean(false); + + final CvSink source = CameraServer.getVideo(); + final CvSource stream = CameraServer.putVideo("Rectangle", 640, 480); + + public static class Target { + // Top-left point + public double x1; + public double y1; + // Bottom-right point + public double x2; + public double y2; + // Detection threshold + public int threshold; + // The command to run during detection + public Command cmd; + + public Target(double x1, double y1, double x2, double y2, int threshold, Command cmd) { + this.x1 = x1; + this.y1 = y1; + this.x2 = x2; + this.y2 = y2; + this.threshold = threshold; + this.cmd = cmd; + } + } + + public Vision(Target targets[]) { + this.targets = targets; + + setDaemon(true); -public class Vision { - public Vision() { - Thread visionThread = new Thread(() -> { - // Get the UsbCamera from CameraServer - CameraServer.startAutomaticCapture(); - - // Get a CvSink. This will capture Mats from the camera - CvSink cvSink = CameraServer.getVideo(); - - // Setup a CvSource. This will send images back to the Dashboard - CvSource outputStream = CameraServer.putVideo("Rectangle", 640, 480); - - // Mats are very memory expensive. Lets reuse this Mat. - Mat mat1 = new Mat(); - Mat mat2 = new Mat(); - - // This cannot be 'true'. The program will never exit if it is. This - // lets the robot stop this thread when restarting robot code or - // deploying. - while (!Thread.interrupted()) { - // Tell the CvSink to grab a frame from the camera and put it - // in the source mat. If there is an error notify the output. - if (cvSink.grabFrame(mat1) == 0) { - // Send the output the error. - outputStream.notifyError(cvSink.getError()); - // skip the rest of the current iteration - continue; - } - - Core.inRange(mat1, new Scalar(245, 245, 245), new Scalar(255, 255, 255), mat2); - - // Give the output stream a new image to display - outputStream.putFrame(mat2); + CameraServer.startAutomaticCapture(); + } + + @Override + public void run() { + // Mats are very memory expensive. Lets reuse this Mat. + Mat mat = new Mat(); + + while (!Thread.interrupted() && !stop.compareAndSet(true, false)) { + // Tell the CvSink to grab a frame from the camera and put it + // in the source mat. If there is an error notify the output. + if (source.grabFrame(mat) == 0) { + // Send the output the error. + stream.notifyError(source.getError()); + // skip the rest of the current iteration + continue; + } + + int c = mat.cols(); + int r = mat.rows(); + + for(Target t : targets) { + Rect rect = new Rect( + (int)(t.x1 * c), + (int)(t.y1 * r), + (int)(t.x2 * c), + (int)(t.y2 * r) + ); + + // Slice the mat to the target rectangle + Mat submat = mat.submat(rect); + // Find the average pixel value + double[] mean = Core.mean(submat).val; + // Check if the threshold is met + boolean detected = mean[0] > t.threshold && mean[1] > t.threshold && mean[2] > t.threshold; + + if (detected) t.cmd.schedule(); + else t.cmd.cancel(); + + Imgproc.rectangle(mat, rect, new Scalar(0, detected ? 255 : 0, detected ? 0 : 255), 5); } - }); + + // Give the output stream a new image to display + stream.putFrame(mat); + } + } - visionThread.setDaemon(true); - visionThread.start(); + public void requestStop() { + stop.set(true); } }