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

@@ -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

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;
     }
 }

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);
     }
 }