meof???

diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java
index 82a62c8cb28fdc498552fd2deb90ae4fa9f419cb..11f3b96d8873e865b080b9ceaca28e5ce8ac126a 100644 (file)
--- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java
+++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java
@@ -45,7 +45,6 @@ public class Shooter extends SubsystemBase implements ShooterIO {
     private FlywheelPhase phase;
 
     private double torqueCurrentDebounceTime = 0.5;
-    private double atGoalStateDebounceTime = 0.5;
 
     VelocityDutyCycle velocityDutyCycle = new VelocityDutyCycle(0);
     TorqueCurrentFOC torqueCurrentFOC = new TorqueCurrentFOC(0);
@@ -55,8 +54,7 @@ public class Shooter extends SubsystemBase implements ShooterIO {
      * 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;
+
     @AutoLogOutput private long launchCount = 0;
     private boolean lastTorqueCurrentControl = false;
 
@@ -64,19 +62,19 @@ public class Shooter extends SubsystemBase implements ShooterIO {
         BANG_BANG,
         CONSTANT_TORQUE,
         START_UP
-    }    
+    }
 
     public Shooter(){
         TalonFXConfiguration config = new TalonFXConfiguration();
-        config.Slot0.kP = 676767.0; //tune p value
-        config.Slot0.kI = 0;
-        config.Slot0.kD = 0;
-        config.Slot0.kV = 0.12; //Maximum rps = 100 --> 12V/100rps
+        config.Slot0.kP = ShooterConstants.kP; //tune p value
+        config.Slot0.kI = ShooterConstants.kI;
+        config.Slot0.kD = ShooterConstants.kD;
+        config.Slot0.kV = ShooterConstants.kV; //Maximum rps = 100 --> 12V/100rps
         
-        config.TorqueCurrent.PeakForwardTorqueCurrent = 40; // this is the constant torque velocity
-        config.TorqueCurrent.PeakReverseTorqueCurrent = 0; // we are making this a BANG BANG controller for talon fx
-        config.MotorOutput.PeakForwardDutyCycle = 1.0;
-        config.MotorOutput.PeakReverseDutyCycle = 0.0;
+        config.TorqueCurrent.PeakReverseTorqueCurrent = ShooterConstants.SHOOTER_MAX_TORQUE_CURRENT; // we are making this a BANG BANG controller for talon fx
+        config.MotorOutput.PeakForwardDutyCycle = ShooterConstants.MOTOR_BANG_BANG_MAX; // bang bang up
+        // tune this 
+        config.MotorOutput.PeakReverseDutyCycle = ShooterConstants.MOTOR_BANG_BANG_MIN; // bang bang down
 
         shooterMotorLeft.getConfigurator().apply(config);
         shooterMotorRight.getConfigurator().apply(config);
@@ -130,12 +128,10 @@ public class Shooter extends SubsystemBase implements ShooterIO {
         System.out.println(math);
         boolean inTolerance =
             math
-                <= ShooterConstants.TORQUE_CURRENT_CONTROL_TOLERANCE;
+                <= Units.rotationsToRadians(ShooterConstants.MOTOR_VELOCITY_TOLERANCE);
         // 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) {
             launchCount++;
         }
@@ -143,8 +139,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 381b788e76872d9ad0a1c77f1796311288ce0468..3143a416b186c2a859e5239bb819926fe2f655a6 100644 (file)
--- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java
+++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java
@@ -15,7 +15,14 @@ public class ShooterConstants {
     public static final double EXIT_VELOCITY_TOLERANCE = 1.0;
 
     // for bang bang
-    public static final double TORQUE_CURRENT_CONTROL_TOLERANCE = 50; // velocity (rotations per second)
-
+    // public static final double TORQUE_CURRENT_CONTROL_TOLERANCE = 50; // velocity (rotations per second)
+    public static final double MOTOR_VELOCITY_TOLERANCE = 3; // velocity (rotations per second)
     public static final double TORQUE_CURRENT_CONTROL_GOAL_AMP = 150; // TUNE
+    public static final double MOTOR_BANG_BANG_MAX = 1.0;
+    public static final double MOTOR_BANG_BANG_MIN = 0.0;
+    public static final double kP = 676767676.0;
+    public static final double kD = 0.02;
+    public static final double kI = 0.0;
+    public static final double kV = 0.12;
+    public static final double SHOOTER_MAX_TORQUE_CURRENT = TORQUE_CURRENT_CONTROL_GOAL_AMP + 10.0;
 }