From: Wesley28w <wesleycwong@gmail.com>
Date: Tue, 10 Feb 2026 00:15:36 +0000 (-0800)
Subject: I ACTUALLY GET IT NOW. i think.. should work
X-Git-Url: https://git.taranathan.com/?a=commitdiff_plain;h=4e0e738fa6ef3ee9cfd8039b8d629bdf877d8387;p=FRC2026.git

I ACTUALLY GET IT NOW. i think.. should work
---

diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java
index 05156fc..781e771 100644
--- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java
+++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java
@@ -6,7 +6,7 @@ import org.littletonrobotics.junction.Logger;
 import com.ctre.phoenix6.configs.MotorOutputConfigs;
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
 import com.ctre.phoenix6.controls.VelocityDutyCycle;
-import com.ctre.phoenix6.controls.VelocityTorqueCurrentFOC;
+import com.ctre.phoenix6.controls.TorqueCurrentFOC;
 import com.ctre.phoenix6.controls.VelocityVoltage;
 import com.ctre.phoenix6.hardware.TalonFX;
 import com.ctre.phoenix6.signals.InvertedValue;
@@ -44,6 +44,11 @@ public class Shooter extends SubsystemBase implements ShooterIO {
     private double torqueCurrentControlTolerance = 20.0;
     private double torqueCurrentDebounceTime = 0.025;
     private double atGoalStateDebounceTime = 0.2;
+
+    /*
+     * Debouncers take in certain statement. If it is false: it checks for how long (the first parameter) 
+     * If it is long enough then return True. 
+     */
     private Debouncer torqueCurrentDebouncer = new Debouncer(torqueCurrentDebounceTime, DebounceType.kFalling);
     private Debouncer atGoalStateDebouncer = new Debouncer(atGoalStateDebounceTime, DebounceType.kFalling);
     private boolean atGoal = false;
@@ -94,26 +99,25 @@ public class Shooter extends SubsystemBase implements ShooterIO {
         SmartDashboard.putNumber("Shot Power", shooterPower);
         shooterPower = SmartDashboard.getNumber("Shot Power", shooterPower);
 
-        if (phase == FlywheelPhase.BANG_BANG) { // shooter target speed is in RPS
-            // Duty-cycle bang-bang (apply to both)
+        if (phase == FlywheelPhase.BANG_BANG || phase == FlywheelPhase.START_UP) { // shooter target speed is in RPS
+            // Duty-cycle bang-bang (apply to both) 100% speeds
             shooterMotorLeft.setControl(new VelocityDutyCycle(shooterTargetSpeed)); // spin as fast as possible shooter target speed
             shooterMotorRight.setControl(new VelocityDutyCycle(shooterTargetSpeed)); // same here
         } else {
             // Torque-current bang-bang
-            shooterMotorLeft.setControl(new VelocityTorqueCurrentFOC(shooterTargetSpeed)); // apply constant torque current
-            shooterMotorRight.setControl(new VelocityTorqueCurrentFOC(shooterTargetSpeed)); // again
+            shooterMotorLeft.setControl(new TorqueCurrentFOC(ShooterConstants.TORQUE_CURRENT_CONTROL_GOAL_AMP));
         }
-
-        // shooterMotorLeft.set(-shooterPower);
-        // shooterMotorRight.set(-shooterPower);
     }
 
     /** Run closed loop at the specified velocity. */
     private void runVelocity(double velocityRadsPerSec) {
+        // if we are not in the tolerance
         boolean inTolerance =
             Math.abs(Units.rotationsToRadians(getShooterVelcoity()) - velocityRadsPerSec)
                 <= torqueCurrentControlTolerance;
-        boolean torqueCurrentControl = torqueCurrentDebouncer.calculate(inTolerance);
+        // If we are not in tolerance we calculate for how long
+        boolean torqueCurrentControl = torqueCurrentDebouncer.calculate(inTolerance); // this calculates if the logic has been occuring long enough
+        
         atGoal = atGoalStateDebouncer.calculate(inTolerance);
 
         if (!torqueCurrentControl && lastTorqueCurrentControl) {
@@ -123,8 +127,8 @@ public class Shooter extends SubsystemBase implements ShooterIO {
 
         phase =
             torqueCurrentControl
-                ? FlywheelPhase.BANG_BANG
-                : FlywheelPhase.CONSTANT_TORQUE;
+                ? FlywheelPhase.CONSTANT_TORQUE
+                : FlywheelPhase.BANG_BANG;
         shooterTargetSpeed = Units.radiansToRotations(velocityRadsPerSec);
         Logger.recordOutput("Shooter/Setpoint", velocityRadsPerSec);
     }
diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java
index bc8ccb7..c49f49e 100644
--- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java
+++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java
@@ -17,6 +17,7 @@ public class ShooterConstants {
     // for bang bang
     public static final double TORQUE_CURRENT_CONTROL_TOLERANCE = 10; // velocity (rotations per second)
 
+    public static final double TORQUE_CURRENT_CONTROL_GOAL_AMP = 40; // TUNE
 }
 // 8 velcocity is too little
 // 16 is too much
\ No newline at end of file
diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java
index 8a1630f..47f6746 100644
--- a/src/main/java/frc/robot/subsystems/turret/Turret.java
+++ b/src/main/java/frc/robot/subsystems/turret/Turret.java
@@ -51,7 +51,6 @@ public class Turret extends SubsystemBase implements TurretIO{
 	//private static final PIDController longVelocityPID = new PIDController(15, 0, 1.0);
 	private static final PIDController velocityPID = new PIDController(0.0, 0.0, 0.0);
 
-
     private final TurretIOInputsAutoLogged inputs = new TurretIOInputsAutoLogged();
 
 	private double lastFrameVelocity = 0.0;