bang bang

diff --git a/src/main/java/frc/robot/subsystems/shooter/FlywheelPhase.java b/src/main/java/frc/robot/subsystems/shooter/FlywheelPhase.java
new file mode 100644 (file)
index 0000000..f088f11
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/shooter/FlywheelPhase.java
@@ -0,0 +1,6 @@
+package frc.robot.subsystems.shooter;
+
+public enum FlywheelPhase {
+    MAX,
+    CONSTANT_TORQUE,
+}

diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java
index 9f5e34092db675ddbdedbc40fd3a91ed2e8dd04e..b26e1f7f85dec6bec829a74641b3a0bee9dbf457 100644 (file)
--- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java
+++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java
@@ -7,6 +7,8 @@ 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.VelocityVoltage;
 import com.ctre.phoenix6.hardware.TalonFX;
 import com.ctre.phoenix6.signals.InvertedValue;
@@ -37,12 +39,21 @@ public class Shooter extends SubsystemBase implements ShooterIO {
 
     private ShooterIOInputsAutoLogged inputs = new ShooterIOInputsAutoLogged();
 
+    // for tracking current phase: used to adjust the setting
+    private FlywheelPhase phase;
+
     public Shooter(){
         TalonFXConfiguration config = new TalonFXConfiguration();
-        config.Slot0.kP = 0.2; //tune p value
+        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.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;
+
         shooterMotorLeft.getConfigurator().apply(config);
         shooterMotorRight.getConfigurator().apply(config);
         
@@ -60,6 +71,9 @@ public class Shooter extends SubsystemBase implements ShooterIO {
             .withNeutralMode(NeutralModeValue.Coast)
         );
 
+        // set start up for phase initially:
+        phase = FlywheelPhase.START_UP;
+
         SmartDashboard.putData("Turn on shooter", new InstantCommand(() -> setShooter(ShooterConstants.SHOOTER_VELOCITY)));
         SmartDashboard.putData("Turn on feeder", new InstantCommand(() -> setFeeder(ShooterConstants.FEEDER_RUN_POWER)));
         SmartDashboard.putData("Turn ALL off", new InstantCommand(() -> deactivateShooterAndFeeder()));
@@ -72,13 +86,29 @@ public class Shooter extends SubsystemBase implements ShooterIO {
         SmartDashboard.putNumber("Shot Power", shooterPower);
         shooterPower = SmartDashboard.getNumber("Shot Power", shooterPower);
 
-        shooterMotorLeft.setControl(voltageRequest.withVelocity(shooterTargetSpeed));
-        shooterMotorRight.setControl(voltageRequest.withVelocity(shooterTargetSpeed));
+        if (phase == FlywheelPhase.MAX) { // shooter target speed is in RPS
+            // Duty-cycle bang-bang (apply to both)
+            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.set(-shooterPower);
         // shooterMotorRight.set(-shooterPower);
         feederMotor.set(feederPower);
     }
 
+    public void updatePhase() {
+        if (Math.abs(shooterMotorLeft.getVelocity().getValueAsDouble() - shooterTargetSpeed) < ShooterConstants.TORQUE_CURRENT_CONTROL_TOLERANCE) {
+            phase = FlywheelPhase.MAX; // we need to recover from ball velocity in order to be back to idle
+        } else {
+            phase = FlywheelPhase.CONSTANT_TORQUE; 
+        }
+    }
+
     public void deactivateShooterAndFeeder() {
         setFeeder(0);
         setShooter(0);

diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java
index 188e1e44d7d482188012544394ba05640b36b045..8783361b16bfaa25524ef50b387bcc1f0d31e635 100644 (file)
--- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java
+++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java
@@ -16,6 +16,8 @@ public class ShooterConstants {
     // in m/s
     public static final double EXIT_VELOCITY_TOLERANCE = 1.0;
 
+    // for bang bang
+    public static final double TORQUE_CURRENT_CONTROL_TOLERANCE = 20; // 20 amps
 
 }
 // 8 velcocity is too little