Flywheels

src/main/java/frc/robot/Controls.java

@@ -43,6 +43,7 @@ public Controls(Subsystems subsystems) {
     s = subsystems;
     configureIntake();
     // Configure the trigger bindings
+    configureFlywheelBindings();
     configureDrivebaseBindings();
   }
 
@@ -75,6 +76,14 @@ private double getDriveRotate() {
     return input * MaxSpeed;
   }
 
+  private void configureFlywheelBindings() {
+    if (s.flywheels == null) {
+      return;
+    } else {
+      driverController.rightTrigger().whileTrue(s.flywheels.runFlywheels());
+    }
+  }
+
   /* Setting up bindings for necessary control of the swerve drive platform */
   private final SwerveRequest.FieldCentric drive =
       new SwerveRequest.FieldCentric()

src/main/java/frc/robot/Flywheels.java

@@ -0,0 +1,110 @@
+package frc.robot;
+
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.controls.Follower;
+import com.ctre.phoenix6.controls.MotionMagicVelocityVoltage;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.InvertedValue;
+import com.ctre.phoenix6.signals.MotorAlignmentValue;
+import com.ctre.phoenix6.signals.NeutralModeValue;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import java.util.function.DoubleSupplier;
+
+public class Flywheels extends SubsystemBase {
+  private final TalonFX flywheelOne;
+  private final TalonFX flywheelTwo;
+
+  public static double speed = 1; // placeholder
+
+  // Configs: PIDS and limits
+  double kP = 0.0;
+  double kI = 0.0;
+  double kD = 0.0;
+  double kA = 0.0;
+  double kV = 0.0;
+  double kS = 0.0;
+  double kG = 0.0;
+
+  private static final int supplyCurrentLimit = 20;
+  private static final int statorCurrentLimit = 40;
+  private static final boolean supplyCurrentLimitEnable = true;
+  private static final boolean statorCurrentLimitEnable = true;
+  private static final int motionMagicAcceleration = 40;
+  private final MotionMagicVelocityVoltage request = new MotionMagicVelocityVoltage(0);
+  private final Follower follow =
+      new Follower(Hardware.FLYWHEEL_ONE_ID, MotorAlignmentValue.Opposed);
+  public final double FLYWHEEL_TOLERANCE = 5; // TODO: Tune tolerance as well
+
+  public Flywheels() {
+    flywheelOne = new TalonFX(Hardware.FLYWHEEL_ONE_ID);
+    flywheelTwo = new TalonFX(Hardware.FLYWHEEL_TWO_ID);
+
+    configureMotors();
+  }
+
+  private void configureMotors() {
+    TalonFXConfiguration config = new TalonFXConfiguration();
+
+    // TODO: tune configs and PIDS, currently placeholder
+    config.CurrentLimits.SupplyCurrentLimit = supplyCurrentLimit;
+    config.CurrentLimits.StatorCurrentLimit = statorCurrentLimit;
+    config.CurrentLimits.SupplyCurrentLimitEnable = supplyCurrentLimitEnable;
+    config.CurrentLimits.StatorCurrentLimitEnable = statorCurrentLimitEnable;
+
+    config.MotorOutput.NeutralMode = NeutralModeValue.Coast;
+    config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive;
+
+    config.Slot0.kP = kP;
+    config.Slot0.kI = kI;
+    config.Slot0.kD = kD;
+    config.Slot0.kA = kA;
+    config.Slot0.kV = kV;
+    config.Slot0.kS = kS;
+    config.Slot0.kG = kG;
+
+    config.MotionMagic.MotionMagicAcceleration = motionMagicAcceleration; // RPS^2
+
+    flywheelOne.getConfigurator().apply(config);
+    flywheelTwo.getConfigurator().apply(config);
+  }
+
+  public Command runFlywheels() {
+    return startEnd(
+            () -> {
+              setFlywheelVelocity(speed);
+            },
+            () -> {
+              stopCommand();
+            })
+        .withName("Run flywheels");
+  }
+
+  private void setFlywheelVelocity(double rps) {
+    request.Velocity = rps;
+    flywheelOne.setControl(request);
+    flywheelTwo.setControl(follow);
+  }
+
+  public Command suppliedSetVelocity(DoubleSupplier rps) {
+    return startEnd(
+            () -> {
+              request.Velocity = rps.getAsDouble();
+              flywheelOne.setControl(request);
+              flywheelTwo.setControl(follow);
+            },
+            () -> {
+              stopCommand();
+            })
+        .withName("Supplied velocity command");
+  }
+
+  public boolean isAtSpeed() {
+    return Math.abs(flywheelOne.getVelocity().getValueAsDouble() - request.Velocity)
+        <= FLYWHEEL_TOLERANCE;
+  }
+
+  private void stopCommand() {
+    flywheelOne.stopMotor();
+  }
+}

src/main/java/frc/robot/Hardware.java

@@ -10,4 +10,7 @@ public final class Hardware {
 
   // TODO: better number when get robot
   public static final int IntakeMotor = 0;
+  // TODO: make this accurate to the actual robot
+  public static final int FLYWHEEL_ONE_ID = 13;
+  public static final int FLYWHEEL_TWO_ID = 14;
 }

src/main/java/frc/robot/Subsystems.java

@@ -1,6 +1,7 @@
 package frc.robot;
 
 import static frc.robot.Subsystems.SubsystemConstants.DRIVEBASE_ENABLED;
+import static frc.robot.Subsystems.SubsystemConstants.FLYWHEELS_ENABLED;
 import static frc.robot.Subsystems.SubsystemConstants.INTAKE_ENABLED;
 
 import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d;
@@ -14,10 +15,12 @@ public class Subsystems {
   public static class SubsystemConstants {
     public static final boolean DRIVEBASE_ENABLED = true;
     public static final boolean INTAKE_ENABLED = true;
+    public static final boolean FLYWHEELS_ENABLED = true;
   }
 
   public final CommandSwerveDrivetrain drivebaseSubsystem;
   public final IntakeSubsystem intakeSubsystem;
+  public final Flywheels flywheels;
 
   public Subsystems(Mechanism2d mechanism2d) {
     // Initialize subsystems here (don't forget to check if they're enabled!)
@@ -43,5 +46,10 @@ public Subsystems(Mechanism2d mechanism2d) {
     } else {
       intakeSubsystem = null;
     }
+    if (FLYWHEELS_ENABLED) {
+      flywheels = new Flywheels();
+    } else {
+      flywheels = null;
+    }
   }
 }