Merge pull request #1 from team422/Shooter

Shooter

src/main/java/frc/robot/Constants.java

@@ -15,6 +15,7 @@
 
 import edu.wpi.first.apriltag.AprilTagFieldLayout;
 import edu.wpi.first.apriltag.AprilTagFields;
+import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Rotation2d;
@@ -114,6 +115,27 @@ public static final class KickerConstants {
     public static final double kSimMOI = 0.5 * kSimMass * kSimRadius * kSimRadius;
   }
 
+  public static final class ShooterConstants {
+    public static final LoggedTunableNumber kIdleVoltage =
+        new LoggedTunableNumber("ShooterIdleVoltage", 0.0);
+    public static final LoggedTunableNumber kP = new LoggedTunableNumber("Flywheel P", 1.0);
+    public static final LoggedTunableNumber kI = new LoggedTunableNumber("Flywheel I", 0.0);
+    public static final LoggedTunableNumber kD = new LoggedTunableNumber("Flywheel D", 0.0);
+
+    public static final PIDController kTopController =
+        new PIDController(kP.get(), kI.get(), kD.get());
+    public static final PIDController kBottomController =
+        new PIDController(kP.get(), kI.get(), kD.get());
+
+    // Simulation constants
+    public static final DCMotor kSimTopGearbox = DCMotor.getNEO(1);
+    public static final DCMotor kSimBottomGearbox = DCMotor.getNEO(1);
+    public static final double kSimGearing = 0.3;
+    public static final double kSimRadius = Units.inchesToMeters(4);
+    public static final double kSimMass = 2 * Units.lbsToKilograms(0.23);
+    public static final double kSimMOI = 0.5 * kSimMass * kSimRadius * kSimRadius;
+  }
+
   public static final class Ports {
     public static final int kFrontLeftDrive = 0;
     public static final int kFrontLeftTurn = 1;
@@ -133,6 +155,8 @@ public static final class Ports {
 
     public static final int kIntakeNeo = 1;
     public static final int kKickerNeo = 2;
+    public static final int kTopFlywheel = 3;
+    public static final int kBottomFlywheel = 4;
   }
 
   public static final class FieldConstants {

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

@@ -19,9 +19,10 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import frc.robot.Constants.Ports;
+import frc.robot.Constants.ShooterConstants;
 import frc.robot.commands.DriveCommands;
 import frc.robot.oi.DriverControls;
-import frc.robot.oi.DriverControlsXbox;
+import frc.robot.oi.DriverControlsPS5;
 import frc.robot.subsystems.aprilTagVision.AprilTagVision;
 import frc.robot.subsystems.aprilTagVision.AprilTagVisionIONorthstar;
 import frc.robot.subsystems.drive.Drive;
@@ -36,6 +37,10 @@
 import frc.robot.subsystems.kicker.Kicker.KickerState;
 import frc.robot.subsystems.kicker.KickerIONeo;
 import frc.robot.subsystems.kicker.KickerIOSim;
+import frc.robot.subsystems.shooter.FlywheelIONeo;
+import frc.robot.subsystems.shooter.FlywheelIOSim;
+import frc.robot.subsystems.shooter.Shooter;
+import frc.robot.subsystems.shooter.Shooter.ShooterState;
 import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
 
 /**
@@ -49,10 +54,11 @@ public class RobotContainer {
   private Drive m_drive;
   private Intake m_intake;
   private Kicker m_kicker;
+  private Shooter m_shooter;
+  private AprilTagVision m_aprilTagVision;
 
   // Controller
   private DriverControls m_driverControls;
-  private AprilTagVision m_aprilTagVision;
 
   // Dashboard inputs
   private LoggedDashboardChooser<Command> m_autoChooser;
@@ -69,10 +75,10 @@ public RobotContainer() {
 
   /** Configure the subsystems. */
   private void configureSubsystems() {
-    m_aprilTagVision = new AprilTagVision(
-      new AprilTagVisionIONorthstar("northstar_0", null),
-      new AprilTagVisionIONorthstar("northstar_1", null)
-    );
+    m_aprilTagVision =
+        new AprilTagVision(
+            new AprilTagVisionIONorthstar("northstar_0", ""),
+            new AprilTagVisionIONorthstar("northstar_1", ""));
 
     if (RobotBase.isReal()) {
       m_drive =
@@ -86,6 +92,12 @@ private void configureSubsystems() {
       m_intake = new Intake(new IntakeIONeo(Ports.kIntakeNeo));
 
       m_kicker = new Kicker(new KickerIONeo(Ports.kKickerNeo));
+
+      m_shooter =
+          new Shooter(
+              new FlywheelIONeo(Ports.kTopFlywheel, Ports.kBottomFlywheel),
+              ShooterConstants.kTopController,
+              ShooterConstants.kBottomController);
     } else {
       m_drive =
           new Drive(
@@ -98,9 +110,15 @@ private void configureSubsystems() {
       m_intake = new Intake(new IntakeIOSim());
 
       m_kicker = new Kicker(new KickerIOSim());
+
+      m_shooter =
+          new Shooter(
+              new FlywheelIOSim(),
+              ShooterConstants.kTopController,
+              ShooterConstants.kBottomController);
     }
 
-    RobotState.start(m_drive, m_intake, m_kicker, m_aprilTagVision);
+    RobotState.start(m_drive, m_intake, m_kicker, m_shooter, m_aprilTagVision);
     m_robotState = RobotState.getInstance();
   }
 
@@ -113,8 +131,8 @@ private void configureCommands() {
 
   /** Configure the controllers. */
   private void configureControllers() {
-    // m_driverControls = new DriverControlsPS5(0);
-    m_driverControls = new DriverControlsXbox(0);
+    m_driverControls = new DriverControlsPS5(0);
+    // m_driverControls = new DriverControlsXbox(0);
   }
 
   /** Configure the button bindings. */
@@ -175,6 +193,19 @@ private void configureButtonBindings() {
                   m_kicker.updateState(KickerState.kIdle);
                   m_intake.updateState(IntakeState.kIdle);
                 }));
+
+    m_driverControls
+        .revShooter()
+        .onTrue(
+            Commands.runOnce(
+                () -> {
+                  m_shooter.updateState(ShooterState.kRevving);
+                }))
+        .onFalse(
+            Commands.runOnce(
+                () -> {
+                  m_shooter.updateState(ShooterState.kIdle);
+                }));
   }
 
   /**

src/main/java/frc/robot/RobotState.java

@@ -6,6 +6,7 @@
 import frc.robot.subsystems.drive.Drive;
 import frc.robot.subsystems.intake.Intake;
 import frc.robot.subsystems.kicker.Kicker;
+import frc.robot.subsystems.shooter.Shooter;
 
 @SuppressWarnings("unused")
 public class RobotState {
@@ -15,17 +16,21 @@ public class RobotState {
   private Drive m_drive;
   private Intake m_intake;
   private Kicker m_kicker;
+  private Shooter m_shooter;
   private AprilTagVision m_aprilTagVision;
 
-  private RobotState(Drive drive, Intake intake, Kicker kicker, AprilTagVision aprilTagVision) {
+  private RobotState(
+      Drive drive, Intake intake, Kicker kicker, Shooter shooter, AprilTagVision aprilTagVision) {
     m_drive = drive;
     m_intake = intake;
     m_kicker = kicker;
+    m_shooter = shooter;
     m_aprilTagVision = aprilTagVision;
   }
 
-  public static void start(Drive drive, Intake intake, Kicker kicker, AprilTagVision aprilTagVision) {
-    m_instance = new RobotState(drive, intake, kicker, aprilTagVision);
+  public static void start(
+      Drive drive, Intake intake, Kicker kicker, Shooter shooter, AprilTagVision aprilTagVision) {
+    m_instance = new RobotState(drive, intake, kicker, shooter, aprilTagVision);
   }
 
   public static RobotState getInstance() {

src/main/java/frc/robot/oi/DriverControls.java

@@ -16,4 +16,6 @@ public interface DriverControls {
   public Trigger runKicker();
 
   public Trigger ejectGamePiece();
+
+  public Trigger revShooter();
 }

src/main/java/frc/robot/oi/DriverControlsPS5.java

@@ -44,4 +44,9 @@ public Trigger runKicker() {
   public Trigger ejectGamePiece() {
     return m_controller.cross();
   }
+
+  @Override
+  public Trigger revShooter() {
+    return m_controller.L2();
+  }
 }

src/main/java/frc/robot/oi/DriverControlsXbox.java

@@ -44,4 +44,9 @@ public Trigger runKicker() {
   public Trigger ejectGamePiece() {
     return m_controller.b();
   }
+
+  @Override
+  public Trigger revShooter() {
+    return m_controller.leftTrigger();
+  }
 }

src/main/java/frc/robot/subsystems/aprilTagVision/AprilTagVision.java

@@ -1,14 +1,5 @@
 package frc.robot.subsystems.aprilTagVision;
 
-import java.util.ArrayList;
-import java.util.Comparator;
-import java.util.HashMap;
-import java.util.List;
-import java.util.Map;
-import java.util.Optional;
-
-import org.littletonrobotics.junction.Logger;
-
 import edu.wpi.first.apriltag.AprilTag;
 import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.VecBuilder;
@@ -27,6 +18,13 @@
 import frc.robot.RobotState;
 import frc.robot.subsystems.aprilTagVision.AprilTagVisionIO.AprilTagVisionInputs;
 import frc.robot.util.GeomUtil;
+import java.util.ArrayList;
+import java.util.Comparator;
+import java.util.HashMap;
+import java.util.List;
+import java.util.Map;
+import java.util.Optional;
+import org.littletonrobotics.junction.Logger;
 
 public class AprilTagVision extends SubsystemBase {
   public record VisionObservation(

src/main/java/frc/robot/subsystems/kicker/Kicker.java

@@ -51,4 +51,5 @@ public void updateState(KickerState state) {
   // Nikki P in the skibidi house!
   // Mr. Wood better than u - Aahil
   // I love God and Jesus - Patty lin
+  // James and Tommy better than you guys cause you're stinky and rotund like a clock
 }

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

@@ -0,0 +1,19 @@
+package frc.robot.subsystems.shooter;
+
+import org.littletonrobotics.junction.AutoLog;
+
+public interface FlywheelIO {
+  @AutoLog
+  public class FlywheelInputs {
+    public double topVelocityRPS;
+    public double topVoltage;
+    public double topCurrentAmps;
+    public double bottomVelocityRPS;
+    public double bottomVoltage;
+    public double bottomCurrentAmps;
+  }
+
+  public void setVoltage(double topVoltage, double bottomVoltage);
+
+  public void updateInputs(FlywheelInputs inputs);
+}

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

@@ -0,0 +1,35 @@
+package frc.robot.subsystems.shooter;
+
+import com.revrobotics.CANSparkLowLevel.MotorType;
+import com.revrobotics.CANSparkMax;
+import com.revrobotics.RelativeEncoder;
+
+public class FlywheelIONeo implements FlywheelIO {
+  private CANSparkMax m_topMotor;
+  private CANSparkMax m_bottomMotor;
+  private RelativeEncoder m_topEncoder;
+  private RelativeEncoder m_bottomEncoder;
+
+  public FlywheelIONeo(int topPort, int bottomPort) {
+    m_topMotor = new CANSparkMax(topPort, MotorType.kBrushless);
+    m_topEncoder = m_topMotor.getEncoder();
+    m_bottomMotor = new CANSparkMax(bottomPort, MotorType.kBrushless);
+    m_bottomEncoder = m_bottomMotor.getEncoder();
+  }
+
+  @Override
+  public void setVoltage(double topVoltage, double bottomVoltage) {
+    m_topMotor.setVoltage(topVoltage);
+    m_bottomMotor.setVoltage(bottomVoltage);
+  }
+
+  @Override
+  public void updateInputs(FlywheelInputs inputs) {
+    inputs.topVelocityRPS = m_topEncoder.getVelocity() / 60;
+    inputs.topVoltage = m_topMotor.getBusVoltage() * m_topMotor.getAppliedOutput();
+    inputs.topCurrentAmps = m_topMotor.getOutputCurrent();
+    inputs.bottomVelocityRPS = m_bottomEncoder.getVelocity() / 60;
+    inputs.bottomVoltage = m_bottomMotor.getBusVoltage() * m_bottomMotor.getAppliedOutput();
+    inputs.bottomCurrentAmps = m_bottomMotor.getOutputCurrent();
+  }
+}

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

@@ -0,0 +1,46 @@
+package frc.robot.subsystems.shooter;
+
+import edu.wpi.first.wpilibj.simulation.FlywheelSim;
+import frc.robot.Constants.ShooterConstants;
+
+public class FlywheelIOSim implements FlywheelIO {
+  private FlywheelSim m_topFlywheel;
+  private FlywheelSim m_bottomFlywheel;
+  private double m_topVoltage;
+  private double m_bottomVoltage;
+
+  public FlywheelIOSim() {
+    m_topFlywheel =
+        new FlywheelSim(
+            ShooterConstants.kSimTopGearbox,
+            ShooterConstants.kSimGearing,
+            ShooterConstants.kSimMOI);
+    m_bottomFlywheel =
+        new FlywheelSim(
+            ShooterConstants.kSimBottomGearbox,
+            ShooterConstants.kSimGearing,
+            ShooterConstants.kSimMOI);
+  }
+
+  @Override
+  public void setVoltage(double topVoltage, double bottomVoltage) {
+    m_topFlywheel.setInputVoltage(topVoltage);
+    m_bottomFlywheel.setInputVoltage(bottomVoltage);
+
+    m_topVoltage = topVoltage;
+    m_bottomVoltage = bottomVoltage;
+  }
+
+  @Override
+  public void updateInputs(FlywheelInputs inputs) {
+    m_topFlywheel.update(.02);
+    m_bottomFlywheel.update(.02);
+
+    inputs.topVelocityRPS = m_topFlywheel.getAngularVelocityRPM() / 60;
+    inputs.topVoltage = m_topVoltage;
+    inputs.topCurrentAmps = m_topFlywheel.getCurrentDrawAmps();
+    inputs.bottomVelocityRPS = m_bottomFlywheel.getAngularVelocityRPM() / 60;
+    inputs.bottomVoltage = m_bottomVoltage;
+    inputs.bottomCurrentAmps = m_bottomFlywheel.getCurrentDrawAmps();
+  }
+}