From 804711d1e266cf7ac24963f52f89fd957ab8e7c8 Mon Sep 17 00:00:00 2001
From: Hayden Heroux <haydenheroux@gmail.com>
Date: Mon, 8 Jan 2024 17:36:05 -0500
Subject: [PATCH] feat(shooter): Add intake commands.

---
 src/main/java/frc/robot/RobotContainer.java   |  3 +++
 src/main/java/frc/robot/shooter/Shooter.java  | 24 +++++++++++++++----
 .../frc/robot/shooter/ShooterConstants.java   | 12 +++++++---
 3 files changed, 32 insertions(+), 7 deletions(-)

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index fdc4b0d..9f68c7b 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -27,6 +27,9 @@ private void initializeShuffleboards() {
   }
 
   private void configureBindings() {
+    operator.leftBumper().whileTrue(shooter.intake());
+    operator.leftTrigger().whileTrue(shooter.smartIntake());
+
     operator.rightBumper().whileTrue(shooter.shoot());
     operator.rightTrigger().whileTrue(shooter.smartShoot());
   }
diff --git a/src/main/java/frc/robot/shooter/Shooter.java b/src/main/java/frc/robot/shooter/Shooter.java
index ec8680c..6702c1e 100644
--- a/src/main/java/frc/robot/shooter/Shooter.java
+++ b/src/main/java/frc/robot/shooter/Shooter.java
@@ -117,8 +117,21 @@ public void addToShuffleboard(ShuffleboardTab tab) {
     flywheel.addDouble("Flywheel Current (A)", () -> flywheelMotorValues.currentAmps);
   }
 
+  public Command intake() {
+    return Commands.parallel(
+        Commands.run(() -> serializerMotor.setVoltage(SerializerConstants.INTAKE_VOLTAGE))
+            .finallyDo(serializerMotor::stop),
+        Commands.run(() -> flywheelMotor.setVoltage(FlywheelConstants.INTAKE_VOLTAGE))
+            .finallyDo(flywheelMotor::stop));
+  }
+
+  public Command smartIntake() {
+    return intake().until(this::isHoldingNote).unless(this::isHoldingNote);
+  }
+
   public Command serialize() {
-    return Commands.run(() -> serializerMotor.setVoltage(SerializerConstants.SERIALIZE_VOLTAGE)).finallyDo(serializerMotor::stop);
+    return Commands.run(() -> serializerMotor.setVoltage(SerializerConstants.SERIALIZE_VOLTAGE))
+        .finallyDo(serializerMotor::stop);
   }
 
   public Command smartSerialize() {
@@ -126,15 +139,18 @@ public Command smartSerialize() {
   }
 
   public Command spin() {
-    return Commands.run(() -> flywheelMotor.setVoltage(FlywheelConstants.SHOOT_VOLTAGE)).finallyDo(flywheelMotor::stop);
+    return Commands.run(() -> flywheelMotor.setVoltage(FlywheelConstants.SHOOT_VOLTAGE))
+        .finallyDo(flywheelMotor::stop);
   }
 
   public Command shoot() {
-    return Commands.deadline(Commands.waitSeconds(SerializerConstants.SHOOT_DELAY).andThen(serialize()), spin());
+    return Commands.deadline(
+        Commands.waitSeconds(SerializerConstants.SHOOT_DELAY).andThen(serialize()), spin());
   }
 
   public Command smartShoot() {
     return Commands.deadline(
-        Commands.waitSeconds(SerializerConstants.SHOOT_DELAY).andThen(smartSerialize()), spin().onlyIf(this::isHoldingNote));
+        Commands.waitSeconds(SerializerConstants.SHOOT_DELAY).andThen(smartSerialize()),
+        spin().onlyIf(this::isHoldingNote));
   }
 }
diff --git a/src/main/java/frc/robot/shooter/ShooterConstants.java b/src/main/java/frc/robot/shooter/ShooterConstants.java
index cc271ed..c568c10 100644
--- a/src/main/java/frc/robot/shooter/ShooterConstants.java
+++ b/src/main/java/frc/robot/shooter/ShooterConstants.java
@@ -36,11 +36,14 @@ public static class SerializerConstants {
     /** Radius of the serializer wheel in meters. */
     public static final double RADIUS = 0.5 * Units.inchesToMeters(4.0);
 
+    /** Voltage to apply while intaking in volts. */
+    public static final double INTAKE_VOLTAGE = -2.0; // TODO
+
     /** Voltage to apply while serializing in volts. */
-    public static final double SERIALIZE_VOLTAGE = 4.0;
+    public static final double SERIALIZE_VOLTAGE = 4.0; // TODO
 
     /** Delay between starting flywheel and serializing while shooting in seconds. */
-    public static final double SHOOT_DELAY = 1.0;
+    public static final double SHOOT_DELAY = 1.0; // TODO
   }
 
   /** Constants for the flywheel motor used in the shooter subsystem. */
@@ -60,7 +63,10 @@ public static class FlywheelConstants {
     /** Radius of the flywheel wheel in meters. */
     public static final double RADIUS = 0.5 * Units.inchesToMeters(4.0);
 
+    /** Voltage to apply while intaking in volts. */
+    public static final double INTAKE_VOLTAGE = -2.0; // TODO
+
     /** Voltage to apply while shooting in volts. */
-    public static final double SHOOT_VOLTAGE = 8.0;
+    public static final double SHOOT_VOLTAGE = 8.0; // TODO
   }
 }