From 23cb972710f400a5d1fba26567b0645fe125ce3d Mon Sep 17 00:00:00 2001
From: maxwtan <100314265+MaxwellTTan20@users.noreply.github.com>
Date: Sat, 7 Feb 2026 13:40:30 -0800
Subject: [PATCH] asdaf

---
 .../frc/robot/subsystems/shooter/Shooter.java | 42 ++++++++++++++++---
 .../subsystems/shooter/ShooterConstants.java  |  2 -
 2 files changed, 37 insertions(+), 7 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java
index 0e7652b..34ccfb0 100644
--- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java
+++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java
@@ -11,6 +11,10 @@ import com.ctre.phoenix6.controls.VelocityVoltage;
 import com.ctre.phoenix6.hardware.TalonFX;
 import com.ctre.phoenix6.signals.InvertedValue;
 import com.ctre.phoenix6.signals.NeutralModeValue;
+
+import edu.wpi.first.math.filter.Debouncer;
+import edu.wpi.first.math.filter.Debouncer.DebounceType;
+import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
@@ -37,6 +41,15 @@ public class Shooter extends SubsystemBase implements ShooterIO {
     // for tracking current phase: used to adjust the setting
     private FlywheelPhase phase;
 
+    private double torqueCurrentControlTolerance = 20.0;
+    private double torqueCurrentDebounceTime = 0.025;
+    private double atGoalStateDebounceTime = 0.2;
+    private Debouncer torqueCurrentDebouncer = new Debouncer(torqueCurrentDebounceTime, DebounceType.kFalling);
+    private Debouncer atGoalStateDebouncer = new Debouncer(atGoalStateDebounceTime, DebounceType.kFalling);
+    private boolean atGoal = false;
+    @AutoLogOutput private long launchCount = 0;
+    private boolean lastTorqueCurrentControl = false;
+
     public enum FlywheelPhase {
         MAX,
         CONSTANT_TORQUE,
@@ -77,6 +90,7 @@ public class Shooter extends SubsystemBase implements ShooterIO {
 
     public void periodic(){
         updateInputs();
+        //updatePhase();
         SmartDashboard.putNumber("Shot Power", shooterPower);
         shooterPower = SmartDashboard.getNumber("Shot Power", shooterPower);
 
@@ -102,6 +116,28 @@ public class Shooter extends SubsystemBase implements ShooterIO {
         }
     }
 
+    /** Run closed loop at the specified velocity. */
+    private void runVelocity(double velocityRadsPerSec) {
+        boolean inTolerance =
+            Math.abs(Units.rotationsToRadians(getShooterVelcoity()) - velocityRadsPerSec)
+                <= torqueCurrentControlTolerance;
+        boolean torqueCurrentControl = torqueCurrentDebouncer.calculate(inTolerance);
+        atGoal = atGoalStateDebouncer.calculate(inTolerance);
+
+        if (!torqueCurrentControl && lastTorqueCurrentControl) {
+        launchCount++;
+        }
+        lastTorqueCurrentControl = torqueCurrentControl;
+
+        outputs.mode =
+            torqueCurrentControl
+                ? FlywheelIOOutputMode.TORQUE_CURRENT_BANG_BANG
+                : FlywheelIOOutputMode.DUTY_CYCLE_BANG_BANG;
+        outputs.velocityRadsPerSec = velocityRadsPerSec;
+        Logger.recordOutput("Shooter/Setpoint", velocityRadsPerSec);
+    }
+
+
     public void deactivateShooterAndFeeder() {
         setShooter(0);
         System.out.println("Shooter deactivated");
@@ -116,10 +152,6 @@ public class Shooter extends SubsystemBase implements ShooterIO {
         this.shooterPower = power;
     }
 
-    public double getFeederVelocity() {
-        return inputs.feederVelocity;
-    }
-
     public double getShooterVelcoity() {
         return inputs.leftShooterVelocity; // assuming they are the same rn
     }
@@ -128,6 +160,6 @@ public class Shooter extends SubsystemBase implements ShooterIO {
         inputs.leftShooterVelocity = shooterMotorLeft.getVelocity().getValueAsDouble();
         inputs.rightShooterVelocity = shooterMotorRight.getVelocity().getValueAsDouble();
         inputs.leftShooterCurrent = shooterMotorLeft.getStatorCurrent().getValueAsDouble();
-        inputs.rightShooterVelocity = shooterMotorRight.getStatorCurrent().getValueAsDouble();
+        inputs.rightShooterCurrent = shooterMotorRight.getStatorCurrent().getValueAsDouble();
     }
 }
diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java
index 3c47d33..bc8ccb7 100644
--- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java
+++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java
@@ -1,7 +1,5 @@
 package frc.robot.subsystems.shooter;
 
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-
 public class ShooterConstants {
     //TODO: find these values
     public static final double FEEDER_RUN_POWER = 0.5; // meters per second??
-- 
2.39.5