Merge branch 'main' of https://github.com/DevilBotz2876/Crescendo2024 …

…into SwerveYAGSL

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

@@ -9,18 +9,31 @@
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
+import frc.robot.commands.ShooterEnable;
 import frc.robot.commands.drive.DriveCommand;
 import frc.robot.subsystems.drive.DriveSwerveYAGSL;
+import frc.robot.subsystems.shooter.ShooterIOSim;
+import frc.robot.subsystems.shooter.ShooterSubsystem;
 
 public class RobotContainer {
-  CommandXboxController controller = new CommandXboxController(0);
-  DriveSwerveYAGSL drive = new DriveSwerveYAGSL();
+  public final CommandXboxController controller;
+  public final ShooterSubsystem shooter;
+  public final DriveSwerveYAGSL drive;
 
   public RobotContainer() {
+    controller = new CommandXboxController(0);
+
+    shooter = new ShooterSubsystem(new ShooterIOSim());
+    drive = new DriveSwerveYAGSL();
+
     configureBindings();
   }
 
   private void configureBindings() {
+    shooter.setDefaultCommand(new InstantCommand(() -> shooter.disable(), shooter));
+
+    controller.rightTrigger().whileTrue(new ShooterEnable(shooter));
+
     drive.setDefaultCommand(
         new DriveCommand(
             drive,

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

@@ -0,0 +1,48 @@
+package frc.robot.commands;
+
+import edu.wpi.first.networktables.GenericEntry;
+import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
+import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
+import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.shooter.ShooterSubsystem;
+import java.util.Map;
+
+public class ShooterEnable extends Command {
+  ShooterSubsystem shooter;
+  ShuffleboardTab tab;
+  GenericEntry voltsEntry;
+
+  public ShooterEnable(ShooterSubsystem shooter) {
+    this.shooter = shooter;
+
+    // create shooter tab on ShuffleBoard
+    tab = Shuffleboard.getTab("Shooter");
+    // Create volt entry under Shooter tab as a number sider with min = -1 and max = 1
+    voltsEntry =
+        tab.add("Volts", 0)
+            .withWidget(BuiltInWidgets.kNumberSlider)
+            .withProperties(Map.of("min", -12, "max", 12))
+            .getEntry();
+
+    // Sets default value to 0.0
+    voltsEntry.setValue(0.0);
+
+    addRequirements(shooter);
+  }
+
+  @Override
+  public void initialize() {}
+
+  @Override
+  public void execute() {
+    // Checks the volt Entry for the volt and sets the voltage of motors
+    shooter.setVoltage(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) {}
+}

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

@@ -0,0 +1,20 @@
+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.
+   *
+   * @param volts The volts to set. Value should be between -12.0 and 12.0.
+   */
+  public default void setVoltage(double volts) {}
+
+  public double getCurrentSpeed();
+
+  public double getVoltage();
+}

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

@@ -0,0 +1,17 @@
+package frc.robot.subsystems.shooter;
+
+import org.littletonrobotics.junction.AutoLog;
+
+public interface ShooterIO {
+  @AutoLog
+  public class ShooterIOInputs {
+    public double velocityRadPerSec = 0.0;
+    public double appliedVolts = 0.0;
+  }
+
+  /** Updates the set of loggable inputs. */
+  public default void updateInputs(ShooterIOInputs inputs) {}
+
+  /** Run open loop at the specified voltage. */
+  public default void setVoltage(double volts) {}
+}

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

@@ -0,0 +1,27 @@
+package frc.robot.subsystems.shooter;
+
+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 double appliedVolts = 0.0;
+
+  @Override
+  public void updateInputs(ShooterIOInputs inputs) {
+
+    // Update sim
+    sim.update(0.02);
+
+    // Update inputs
+    inputs.velocityRadPerSec = sim.getAngularVelocityRadPerSec();
+    inputs.appliedVolts = appliedVolts;
+  }
+
+  @Override
+  public void setVoltage(double volts) {
+    appliedVolts = volts;
+    sim.setInputVoltage(volts);
+  }
+}

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

@@ -0,0 +1,52 @@
+package frc.robot.subsystems.shooter;
+
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import org.littletonrobotics.junction.AutoLogOutput;
+import org.littletonrobotics.junction.Logger;
+
+public class ShooterSubsystem extends SubsystemBase implements Shooter {
+  ShooterIO io;
+  private final ShooterIOInputsAutoLogged inputs = new ShooterIOInputsAutoLogged();
+  @AutoLogOutput private double voltage;
+
+  public ShooterSubsystem(ShooterIO io) {
+    this.io = io;
+    voltage = 0;
+  }
+
+  @Override
+  // Disable the shooter
+  public void disable() {
+    voltage = 0;
+    io.setVoltage(voltage);
+  }
+
+  @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 setVoltage(double volts) {
+    voltage = volts;
+  }
+
+  @Override
+  public double getVoltage() {
+    return inputs.appliedVolts;
+  }
+
+  @Override
+  public double getCurrentSpeed() {
+    return inputs.velocityRadPerSec;
+  }
+
+  @Override
+  public void periodic() {
+    // Updates the inputs
+    io.updateInputs(inputs);
+    Logger.processInputs("Shooter", inputs);
+  }
+}