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 05156fcd0b195b304ae2a27642d58de47cae2b42..781e771f3ca29945d46d3736f3d0a90d620ac1a7 100644 (file)
--- 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 bc8ccb7facc564c0a991933d51e0d8c03d14abec..c49f49ec0258cc7111f16cb5ec131116dffe0f98 100644 (file)
--- 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 8a1630f5467651fe6d928aab3a59b1d46c3c572e..47f6746860611ee274e6c944379c483eea14cf45 100644 (file)
--- 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;