Update ShooterSubsystem2.java

CyberCoyotes · Mar 19, 2024 · d3ab9af · d3ab9af
1 parent 58c9f83
commit d3ab9af
Showing 1 changed file with 53 additions and 44 deletions.
diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem2.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem2.java
@@ -9,23 +9,28 @@
 import com.ctre.phoenix6.controls.VelocityVoltage;
 import com.ctre.phoenix6.hardware.TalonFX;
 import com.ctre.phoenix6.signals.NeutralModeValue;
+
+import edu.wpi.first.wpilibj2.command.Subsystem;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants;
-import frc.robot.Constants.CANIDs;
 
+@SuppressWarnings("unused")
+
+/* Defining a new class named ShooterSubsystem2 in Java.
+This class is extending SubsystemBase, which means ShooterSubsystem2 is a subclass of SubsystemBase. */
 public class ShooterSubsystem2 extends SubsystemBase{
-
+    private static final double FLYWHEEL_VELOCITY = 60;
+    private static final double FLYWHEEL_IDLE_VELOCITY = 5;
+
     // Declare variables for the motors to be controlled
-    private TalonFX m_primaryMotor; // Right
-    private TalonFX m_secondaryMotor; // Left
+    private TalonFX m_primaryMotor = new TalonFX(Constants.CANIDs.RIGHT_FLYWHEEL_ID, "rio"); // Right
+    private TalonFX m_secondaryMotor = new TalonFX(Constants.CANIDs.LEFT_FLYWHEEL_ID, "rio"); // Left
 
     // Class member variables
-    private VelocityVoltage m_primaryVelocity = new VelocityVoltage(0);
-    private VelocityVoltage m_secondaryVelocity = new VelocityVoltage(0);
-
+    private VelocityVoltage m_velocity = new VelocityVoltage(0);
     private double flywheelVelocity;
-    private double targetFlywheelVelocity;
-    private boolean shooterDisabled = false;
+    private boolean flywheelDisabled = false;
+    private double targetFlywheelSpeed;
 
 
     public ShooterSubsystem2() {
@@ -40,61 +45,65 @@ public ShooterSubsystem2() {
         m_secondaryMotor.setInverted(false);       
 
         /* Various Configs */
-        var shooterConfigs0 = new Slot0Configs();        
-            shooterConfigs0
-                .withKP(0.11)   // TODO where did these come from anyway?
-                .withKI(0.5)    //
-                .withKS(0.0001) //
-                .withKV(0.12);  //
-
-
-        var shooterVelocityConfig = new VoltageConfigs();
-            shooterVelocityConfig
-                .withPeakForwardVoltage(8)  // FRC 2910 running 12
-                .withPeakReverseVoltage(8); // Originally -8, with negative the "helper" text goes away
+        var flywheelConfigs0 = new Slot0Configs();        
+            flywheelConfigs0
+                // https://github.com/CrossTheRoadElec/Phoenix6-Examples/blob/main/java/VelocityClosedLoop/src/main/java/frc/robot/Robot.java
+                .withKP(0.11) 
+                .withKI(0.5)
+                .withKS(0.0001)
+                .withKV(0.12);
+
+
+        var flywheelVelocityConfig = new VoltageConfigs();
+            flywheelVelocityConfig
+                .withPeakForwardVoltage(12)  // FRC 2910 running 12
+                .withPeakReverseVoltage(12); // Originally -8, with negative the "helper" text goes away
+
 
-        var shooterCurrentConfigs = new CurrentLimitsConfigs();
-            shooterCurrentConfigs
+        var flywheelCurrentConfigs = new CurrentLimitsConfigs();
+        flywheelCurrentConfigs
                 .withStatorCurrentLimit(60) 
                 .withStatorCurrentLimitEnable(true);
 
-        var shooterMotorOutputConfigs = new MotorOutputConfigs();
-            shooterMotorOutputConfigs
+        var flywheelMotorOutput = new MotorOutputConfigs();
+        flywheelMotorOutput
                 .withNeutralMode(NeutralModeValue.Coast);
 
         /* Apply Configs */
-        m_primaryMotor.getConfigurator().apply(shooterConfigs0);
-        m_secondaryMotor.getConfigurator().apply(shooterConfigs0);
-        m_primaryMotor.getConfigurator().apply(shooterVelocityConfig);
-        m_secondaryMotor.getConfigurator().apply(shooterVelocityConfig);
-        m_primaryMotor.getConfigurator().apply(shooterCurrentConfigs);
-        m_secondaryMotor.getConfigurator().apply(shooterCurrentConfigs);
-        m_primaryMotor.getConfigurator().apply(shooterMotorOutputConfigs);
-        m_secondaryMotor.getConfigurator().apply(shooterMotorOutputConfigs);
+        m_primaryMotor.getConfigurator().apply(flywheelConfigs0);
+        m_secondaryMotor.getConfigurator().apply(flywheelConfigs0);
+
+        m_primaryMotor.getConfigurator().apply(flywheelVelocityConfig);
+        m_secondaryMotor.getConfigurator().apply(flywheelVelocityConfig);
+
+        m_primaryMotor.getConfigurator().apply(flywheelCurrentConfigs);
+        m_secondaryMotor.getConfigurator().apply(flywheelCurrentConfigs);
+
+        m_primaryMotor.getConfigurator().apply(flywheelMotorOutput);
+        m_secondaryMotor.getConfigurator().apply(flywheelMotorOutput);
 
 
     } // end of constructor
 
     /* Sets the velocity the motors are using */
-    public void setTargetFlywheelVelocity(double targetFlywheelVelocity) {
-        // this.shooterVelocity = shooterVelocity;
-        m_primaryMotor.setControl(m_primaryVelocity.withVelocity(flywheelVelocity));
+    public void setTargetFlywheelVelocity(double flywheelVelocity) {
+        this.flywheelVelocity = flywheelVelocity;
+        // m_primaryMotor.setControl(m_primaryVelocity.withVelocity(flywheelVelocity));
 //        m_secondaryMotor.setControl(m_secondaryVelocity.withVelocity(flywheelVelocity * 0.95));
     }
-    /*
+
     public void setTargetFlywheelSpeed(double targetFlywheelSpeed) {
-        this.targetFlywheelSpeed = targetFlywheelSpeed + Units.rotationsPerMinuteToRadiansPerSecond(shootingOffset);
+        this.targetFlywheelSpeed = targetFlywheelSpeed;
     }
-     */
-
+
     /* Returns the velocity of the flywheel */
-    public double getTargetFlywheelVelocity() {
-        return targetFlywheelVelocity;
+    public double getTargetFlywheelSpeed(double targetFlywheelSpeed) {
+        return targetFlywheelSpeed;
     }
 
     // Returns the velocity of the flywheel 
-    public StatusSignal<Double> getFlywheelVelocity() {
-        return m_primaryMotor.getVelocity(); // TODO check if this is the right method
+    public double getFlywheelVelocity() {
+        return m_primaryMotor.getVelocity() ; // TODO check if this is the right method
     }
 
     public boolean isFlywheelAtTargetVelocity() {