From 05af220a86248187c81ffb9656e094de992843a5 Mon Sep 17 00:00:00 2001
From: Aidan <>
Date: Tue, 13 Aug 2024 16:48:32 -0700
Subject: [PATCH] start shooter

---
 .../java/frc/robot/shooter/ShooterState.java  |  13 ++
 .../frc/robot/shooter/ShooterSubsystem.java   | 145 ++++++++++++++++++
 2 files changed, 158 insertions(+)
 create mode 100644 src/main/java/frc/robot/shooter/ShooterState.java
 create mode 100644 src/main/java/frc/robot/shooter/ShooterSubsystem.java

diff --git a/src/main/java/frc/robot/shooter/ShooterState.java b/src/main/java/frc/robot/shooter/ShooterState.java
new file mode 100644
index 0000000..a9de2a3
--- /dev/null
+++ b/src/main/java/frc/robot/shooter/ShooterState.java
@@ -0,0 +1,13 @@
+package main.java.frc.robot.shooter;
+
+public enum ShooterState {
+  IDLE_NO_GP,
+  IDLE_WITH_GP,
+  DROP,
+  SPEAKER_SHOT,
+  SUBWOOFER_SHOT,
+  FEEDING,
+  PODIUM_SHOT;
+
+
+}
diff --git a/src/main/java/frc/robot/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/shooter/ShooterSubsystem.java
new file mode 100644
index 0000000..6e53590
--- /dev/null
+++ b/src/main/java/frc/robot/shooter/ShooterSubsystem.java
@@ -0,0 +1,145 @@
+
+
+package main.java.frc.robot.shooter;
+
+import com.ctre.phoenix6.controls.VelocityTorqueCurrentFOC;
+import com.ctre.phoenix6.hardware.TalonFX;
+import dev.doglog.DogLog;
+import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap;
+import edu.wpi.first.wpilibj.DriverStation;
+import frc.robot.config.RobotConfig;
+import frc.robot.util.scheduling.LifecycleSubsystem;
+import frc.robot.util.scheduling.SubsystemPriority;
+
+public class ShooterSubsystem extends StateMachine {
+  private final TalonFX leftMotor;
+  private final TalonFX rightMotor;
+  private boolean usingNoteSpin = true;
+  private final VelocityTorqueCurrentFOC velocityRequest =
+      new VelocityTorqueCurrentFOC(0).withSlot(0).withLimitReverseMotion(true);
+  private double speakerDistance = 0;
+  private double floorSpotDistance = 0;
+  private double goalRPM = 0;
+  private double usedTolerance = ShooterRPMs.TOLERANCE;
+
+  private final InterpolatingDoubleTreeMap speakerDistanceToRPM = new InterpolatingDoubleTreeMap();
+  private final InterpolatingDoubleTreeMap floorSpotDistanceToRPM =
+      new InterpolatingDoubleTreeMap();
+
+  private ShooterMode goalMode = ShooterMode.IDLE;
+
+  public ShooterSubsystem(TalonFX leftMotor, TalonFX rightMotor) {
+    super(SubsystemPriority.SHOOTER);
+
+    this.leftMotor = leftMotor;
+    this.rightMotor = rightMotor;
+
+    RobotConfig.get().shooter().speakerShotRpms().accept(speakerDistanceToRPM);
+    RobotConfig.get().shooter().floorShotRpms().accept(floorSpotDistanceToRPM);
+
+    leftMotor.getConfigurator().apply(RobotConfig.get().shooter().leftMotorConfig());
+    rightMotor.getConfigurator().apply(RobotConfig.get().shooter().rightMotorConfig());
+  }
+
+  @Override
+  public void robotPeriodic() {
+    if (goalMode == ShooterMode.SHOOTER_AMP) {
+      usingNoteSpin = false;
+      usedTolerance = ShooterRPMs.AMP_TOLERANCE;
+    } else {
+      usingNoteSpin = true;
+      usedTolerance = ShooterRPMs.TOLERANCE;
+    }
+    switch (goalMode) {
+      case SPEAKER_SHOT:
+        goalRPM = speakerDistanceToRPM.get(speakerDistance);
+        break;
+      case SUBWOOFER_SHOT:
+        goalRPM = ShooterRPMs.SUBWOOFER;
+        break;
+      case PODIUM_SHOT:
+        goalRPM = ShooterRPMs.PODIUM;
+        break;
+      case FLOOR_SHOT:
+        goalRPM = floorSpotDistanceToRPM.get(floorSpotDistance);
+        break;
+      case DROPPING:
+        goalRPM = ShooterRPMs.DROPPING;
+        break;
+      case OUTTAKE:
+        goalRPM = ShooterRPMs.OUTTAKE;
+        break;
+      case SHOOTER_AMP:
+        goalRPM = ShooterRPMs.SHOOTER_AMP;
+        break;
+      case IDLE:
+        if (DriverStation.isAutonomous()) {
+          goalRPM = speakerDistanceToRPM.get(speakerDistance);
+        } else {
+          goalRPM = ShooterRPMs.IDLE;
+        }
+        break;
+      case FULLY_STOPPED:
+        goalRPM = ShooterRPMs.FULLY_STOPPED;
+        break;
+      default:
+        break;
+    }
+
+    DogLog.log("Shooter/Mode", goalMode);
+    DogLog.log("Shooter/GoalRPM", goalRPM);
+    DogLog.log(
+        "Shooter/GoalRPMForRightMotor", goalRPM * (usingNoteSpin ? ShooterRPMs.SPIN_RATIO : 1.0));
+    DogLog.log("Shooter/LeftMotor/RPM", getRPM(leftMotor));
+    DogLog.log("Shooter/RightMotor/RPM", getRPM(rightMotor));
+    DogLog.log("Shooter/AtGoal", atGoal(goalMode));
+
+    if (goalMode == ShooterMode.FULLY_STOPPED) {
+      leftMotor.disable();
+      rightMotor.disable();
+    } else {
+      leftMotor.setControl(velocityRequest.withVelocity((goalRPM) / 60));
+      rightMotor.setControl(
+          velocityRequest.withVelocity(
+              (goalRPM * (usingNoteSpin ? ShooterRPMs.SPIN_RATIO : 1.0)) / 60));
+    }
+  }
+
+  public boolean atGoal(ShooterMode mode) {
+    if (mode != goalMode) {
+      return false;
+    }
+
+    if (goalMode == ShooterMode.IDLE || goalMode == ShooterMode.FULLY_STOPPED) {
+      return true;
+    }
+
+    if (Math.abs((goalRPM * (usingNoteSpin ? ShooterRPMs.SPIN_RATIO : 1.0)) - getRPM(rightMotor))
+            < ShooterRPMs.TOLERANCE
+        && Math.abs(goalRPM - getRPM(leftMotor)) < ShooterRPMs.TOLERANCE) {
+      return true;
+    }
+
+    return false;
+  }
+
+  private double getRPM(TalonFX motor) {
+    return motor.getVelocity().getValue() * 60.0;
+  }
+
+  public void setGoalMode(ShooterMode newMode) {
+    goalMode = newMode;
+  }
+
+  public ShooterMode getGoalMode() {
+    return goalMode;
+  }
+
+  public void setSpeakerDistance(double distance) {
+    speakerDistance = distance;
+  }
+
+  public void setFloorSpotDistance(double distance) {
+    floorSpotDistance = distance;
+  }
+}