R2 150 update shooter to use 2 flywheels

src/main/java/frc/robot/Constants.java

@@ -302,9 +302,9 @@ public class ShooterFlywheelConstants {
 
     // Velocity PID
     private static Slot0Configs SLOT0CONFIG =
-        new Slot0Configs().withKP(1000.0).withKI(0.0).withKD(0.0);
+        new Slot0Configs().withKP(0.0).withKI(0.0).withKD(0.0).withKS(10.0);
 
-    public static TalonFXConfiguration getFXConfig() {
+    public static TalonFXConfiguration getFXConfig(boolean invert) {
       TalonFXConfiguration config = new TalonFXConfiguration();
 
       config.CurrentLimits.SupplyCurrentLimitEnable = Robot.isReal();
@@ -319,7 +319,12 @@ public static TalonFXConfiguration getFXConfig() {
       config.Voltage.PeakReverseVoltage = -12.0;
 
       config.MotorOutput.NeutralMode = NeutralModeValue.Brake;
-      config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive;
+      // config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive;
+      if (invert == true) {
+        config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive;
+      } else {
+        config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive;
+      }
 
       config.SoftwareLimitSwitch.ForwardSoftLimitEnable = false;
 

src/main/java/frc/robot/RobotContainer.java

@@ -104,12 +104,17 @@ public RobotContainer() {
                 new FlywheelMechanismReal(
                     new MotorIOTalonFX(
                         ShooterFlywheelConstants.NAME,
-                        ShooterFlywheelConstants.getFXConfig(),
+                        ShooterFlywheelConstants.getFXConfig(false),
                         Ports.LeftFlywheel)),
                 new FlywheelMechanismReal(
                     new MotorIOTalonFX(
                         ShooterFlywheelConstants.NAME,
-                        ShooterFlywheelConstants.getFXConfig(),
+                        ShooterFlywheelConstants.getFXConfig(true),
+                        Ports.RightFlywheel)),
+                new FlywheelMechanismReal(
+                    new MotorIOTalonFX(
+                        ShooterFlywheelConstants.NAME,
+                        ShooterFlywheelConstants.getFXConfig(false),
                         Ports.TowerRoller)),
                 new RotaryMechanismReal(
                     new MotorIOTalonFX(
@@ -165,15 +170,23 @@ public RobotContainer() {
                 new FlywheelMechanismSim(
                     new MotorIOTalonFXSim(
                         ShooterFlywheelConstants.NAME,
-                        ShooterFlywheelConstants.getFXConfig(),
+                        ShooterFlywheelConstants.getFXConfig(false),
+                        Ports.LeftFlywheel),
+                    ShooterFlywheelConstants.DCMOTOR,
+                    ShooterFlywheelConstants.MOI,
+                    ShooterFlywheelConstants.TOLERANCE),
+                new FlywheelMechanismSim(
+                    new MotorIOTalonFXSim(
+                        ShooterFlywheelConstants.NAME,
+                        ShooterFlywheelConstants.getFXConfig(true),
                         Ports.LeftFlywheel),
                     ShooterFlywheelConstants.DCMOTOR,
                     ShooterFlywheelConstants.MOI,
                     ShooterFlywheelConstants.TOLERANCE),
                 new FlywheelMechanismSim(
                     new MotorIOTalonFXSim(
                         ShooterFlywheelConstants.NAME,
-                        ShooterFlywheelConstants.getFXConfig(),
+                        ShooterFlywheelConstants.getFXConfig(false),
                         Ports.TowerRoller),
                     ShooterFlywheelConstants.DCMOTOR,
                     ShooterFlywheelConstants.MOI,
@@ -234,6 +247,7 @@ public RobotContainer() {
 
         shooter =
             new Shooter(
+                new FlywheelMechanism() {},
                 new FlywheelMechanism() {},
                 new FlywheelMechanism() {},
                 new RotaryMechanism(null, null) {});
@@ -285,17 +299,17 @@ private void configureButtonBindings() {
             () -> -controller.getRightX()));
 
     // Lock to 0° when A button is held
-    controller
-        .a()
-        .whileTrue(
-            DriveCommands.joystickDriveAtAngle(
-                drive,
-                () -> -controller.getLeftY(),
-                () -> -controller.getLeftX(),
-                () -> new Rotation2d()));
+    // controller
+    //     .a()
+    //     .whileTrue(
+    //         DriveCommands.joystickDriveAtAngle(
+    //             drive,
+    //             () -> -controller.getLeftY(),
+    //             () -> -controller.getLeftX(),
+    //             () -> new Rotation2d()));
 
-    // Switch to X pattern when X button is pressed
-    controller.x().onTrue(Commands.runOnce(drive::stopWithX, drive));
+    // // Switch to X pattern when X button is pressed
+    // controller.x().onTrue(Commands.runOnce(drive::stopWithX, drive));
 
     // Reset gyro to 0° when B button is pressed
     controller
@@ -326,6 +340,7 @@ private void configureButtonBindings() {
     controller.y().onTrue(Commands.runOnce(() -> intake.setVelocity(1)));
     controller.leftBumper().onTrue(intake.intake());
     controller.rightBumper().onTrue(intake.stowAndStopRollers());
+    controller.a().onTrue(shooter.runFlywheel());
   }
 
   /**

src/main/java/frc/robot/subsystems/Shooter.java

@@ -28,16 +28,22 @@
 
 public class Shooter extends SubsystemBase {
 
-  private final FlywheelMechanism _flywheel;
+  private final FlywheelMechanism _rflywheel;
+  private final FlywheelMechanism _lflywheel;
   private final FlywheelMechanism _feeder;
   private final RotaryMechanism _hood;
 
   // desired target values
   private double desiredVelo;
   private double hoodAngle;
 
-  public Shooter(FlywheelMechanism flywheel, FlywheelMechanism feeder, RotaryMechanism hood) {
-    _flywheel = flywheel;
+  public Shooter(
+      FlywheelMechanism lflywheel,
+      FlywheelMechanism rflywheel,
+      FlywheelMechanism feeder,
+      RotaryMechanism hood) {
+    _lflywheel = lflywheel;
+    _rflywheel = rflywheel;
     _feeder = feeder;
     _hood = hood;
   }
@@ -53,8 +59,9 @@ public void setFlywheelVelocity(double velocity) {
     // store the desired velocity then send converted velocity to the mechanism
     this.desiredVelo = velocity;
     AngularVelocity angVelo = RotationsPerSecond.of(velocity);
-
-    _flywheel.runVelocity(angVelo, ShooterConstants.ACCELERATION, PIDSlot.SLOT_0);
+    AngularVelocity negangVelo = RotationsPerSecond.of(velocity);
+    _lflywheel.runVelocity(angVelo, ShooterConstants.ACCELERATION, PIDSlot.SLOT_0);
+    _rflywheel.runVelocity(negangVelo, ShooterConstants.ACCELERATION, PIDSlot.SLOT_0);
   }
 
   public enum State {
@@ -74,7 +81,9 @@ public enum State {
 
   // Checks if the flywheel is at speed and returns a boolean
   public boolean flyAtVelocity() {
-    return Math.abs(desiredVelo - _flywheel.getVelocity().in(RotationsPerSecond))
+    return ((Math.abs(desiredVelo - _lflywheel.getVelocity().in(RotationsPerSecond))
+                + Math.abs(desiredVelo - _rflywheel.getVelocity().in(RotationsPerSecond)))
+            / 2)
         <= ShooterConstants.FLYWHEEL_VELOCITY_TOLERANCE;
   }
 
@@ -129,6 +138,10 @@ public Command shoot(double velocity) {
         Commands.runOnce(() -> setFlywheelVelocity(0)));
   }
 
+  public Command runFlywheel() {
+    return Commands.runOnce(() -> setFlywheelVelocity(1));
+  }
+
   public void simShoot() {
     if (Robot.robotContainer.intake.simBalls <= 0) return;