Simplified Shooter Subsystem and IO interfaces

Fixed simulation and added PID control.

src/main/java/frc/robot/RobotContainer.java

@@ -105,7 +105,9 @@ private void configureBindings() {
         .a()
         .whileTrue(
             Commands.startEnd(
-                () -> shooter.runVelocity(shooterSpeedInput.get()), shooter::disable, shooter));
+                () -> shooter.runVelocity(shooterSpeedInput.get()),
+                () -> shooter.runVelocity(0),
+                shooter));
 
     intake.setDefaultCommand(
         new IntakeBaseCommand(

src/main/java/frc/robot/commands/ShooterEnable.java

@@ -38,11 +38,10 @@ public void initialize() {}
   public void execute() {
     // Checks the volt Entry for the volt and sets the voltage of motors
     shooter.runVoltage(voltsEntry.getDouble(0.0));
-
-    // Enable motors, It has to be called regularly for voltage compensation to work properly
-    shooter.enable();
   }
 
   @Override
-  public void end(boolean interrupted) {}
+  public void end(boolean interrupted) {
+    shooter.runVoltage(0);
+  }
 }

src/main/java/frc/robot/subsystems/shooter/Shooter.java

@@ -1,12 +1,6 @@
 package frc.robot.subsystems.shooter;
 
 public interface Shooter {
-  /** Disable the voltage of a motors for the shooter. */
-  public default void disable() {}
-
-  /** Enable the voltage of a motors for the shooter. */
-  public default void enable() {}
-
   /**
    * Set the voltage of motors for the shooter.
    *

src/main/java/frc/robot/subsystems/shooter/ShooterIO.java

@@ -5,11 +5,8 @@
 public interface ShooterIO {
   @AutoLog
   public class ShooterIOInputs {
-    public double velocityRadPerSecTop = 0.0;
-    public double appliedVoltsTop = 0.0;
-
-    public double velocityRadPerSecBottom = 0.0;
-    public double appliedVoltsBottom = 0.0;
+    public double velocityRadPerSec = 0.0;
+    public double appliedVolts = 0.0;
   }
 
   /** Updates the set of loggable inputs. */
@@ -20,6 +17,4 @@ public default void setVoltage(double volts) {}
 
   /** Run closed loop at the specified velocity */
   public default void setVelocity(double velocityRadPerSec, double ffVolts) {}
-
-  public default void stop() {}
 }

src/main/java/frc/robot/subsystems/shooter/ShooterIOSim.java

@@ -1,33 +1,47 @@
 package frc.robot.subsystems.shooter;
 
+import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.system.plant.DCMotor;
 import edu.wpi.first.wpilibj.simulation.FlywheelSim;
 
 public class ShooterIOSim implements ShooterIO {
-  private FlywheelSim sim = new FlywheelSim(DCMotor.getNEO(1), 1.5, 0.004);
+  private FlywheelSim wheel = new FlywheelSim(DCMotor.getNEO(1), 1.5, 0.004);
 
+  private PIDController pid = new PIDController(1, 0, 0);
+  private double feedForwardVoltage = 0.0;
   private double appliedVolts = 0.0;
+  private double targetVelocityRadPerSec = 0.0;
 
   @Override
   public void updateInputs(ShooterIOInputs inputs) {
 
     // Update sim
-    sim.update(0.02);
+    wheel.update(0.02);
 
     // Update inputs
-    inputs.velocityRadPerSecTop = sim.getAngularVelocityRadPerSec();
-    inputs.appliedVoltsTop = appliedVolts;
+    inputs.velocityRadPerSec = wheel.getAngularVelocityRadPerSec();
+    inputs.appliedVolts = appliedVolts;
+
+    if (targetVelocityRadPerSec != 0) {
+      appliedVolts =
+          feedForwardVoltage + pid.calculate(inputs.velocityRadPerSec, targetVelocityRadPerSec);
+      wheel.setInputVoltage(appliedVolts);
+    }
   }
 
   @Override
   public void setVoltage(double volts) {
     appliedVolts = volts;
-    sim.setInputVoltage(volts);
+    wheel.setInputVoltage(volts);
   }
 
   @Override
-  public void stop() {
+  public void setVelocity(double velocityRadPerSec, double ffVolts) {
     appliedVolts = 0;
-    sim.setInputVoltage(0);
+    targetVelocityRadPerSec = velocityRadPerSec;
+    feedForwardVoltage = ffVolts;
+    if (velocityRadPerSec == 0) {
+      wheel.setInputVoltage(0);
+    }
   }
 }

src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java

@@ -94,25 +94,19 @@ public ShooterIOSparkMax() {
     // SmartDashboard.putNumber("Shooter/bot/Min Output", bkMinOutput);
   }
 
-  @Override
-  public void stop() {
-    top.stopMotor();
-    bottom.stopMotor();
-  }
-
   @Override
   public void updateInputs(ShooterIOInputs inputs) {
     // Set velocityRadPerSec to the encoder velocity(rotationsPerMinute) divided by the gear ratio
     // and converted into Radians Per Second
-    inputs.velocityRadPerSecTop =
+    inputs.velocityRadPerSec =
         Units.rotationsPerMinuteToRadiansPerSecond(topEncoder.getVelocity() / GEAR_RATIO);
     // Get applied voltage from the top motor
-    inputs.appliedVoltsTop = top.getAppliedOutput() * top.getBusVoltage();
+    inputs.appliedVolts = top.getAppliedOutput() * top.getBusVoltage();
 
-    inputs.velocityRadPerSecBottom =
-        Units.rotationsPerMinuteToRadiansPerSecond(bottomEncoder.getVelocity() / GEAR_RATIO);
+//    inputs.velocityRadPerSecBottom =
+//        Units.rotationsPerMinuteToRadiansPerSecond(bottomEncoder.getVelocity() / GEAR_RATIO);
     // Get applied voltage from the top motor
-    inputs.appliedVoltsBottom = bottom.getAppliedOutput() * bottom.getBusVoltage();
+//    inputs.appliedVoltsBottom = bottom.getAppliedOutput() * bottom.getBusVoltage();
 
     double tp = SmartDashboard.getNumber("Shooter/top/P Gain", 0);
     double ti = SmartDashboard.getNumber("Shooter/top/I Gain", 0);

src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java

@@ -22,27 +22,17 @@ public ShooterSubsystem(ShooterIO io) {
     velocityRPM = 0.0;
   }
 
-  @Override
-  // Disable the shooter
-  public void disable() {
-    targetVelocityRadPerSec = 0;
-    io.stop();
-  }
-
-  @Override
-  // Enable the shooter
-  public void enable() {
-    // io.setVoltage(voltage);
-  }
-
   @Override
   // Sets the voltage to volts. the volts value is -12 to 12
   public void runVoltage(double volts) {
     voltage = volts;
+    targetVelocityRadPerSec = 0;
+    io.setVoltage(voltage);
   }
 
   @Override
   public void runVelocity(double velocityRPM) {
+    voltage = 0;
     this.velocityRPM = velocityRPM;
     targetVelocityRadPerSec = Units.rotationsPerMinuteToRadiansPerSecond(velocityRPM);
 
@@ -51,12 +41,12 @@ public void runVelocity(double velocityRPM) {
 
   @Override
   public double getVoltage() {
-    return inputs.appliedVoltsTop;
+    return inputs.appliedVolts;
   }
 
   @Override
   public double getCurrentSpeed() {
-    return inputs.velocityRadPerSecTop;
+    return inputs.velocityRadPerSec;
   }
 
   @Override