Launcher additions

src/main/java/frc/robot/Robot.java

@@ -35,7 +35,9 @@
 import frc.robot.subsystems.drive.ModuleIOSim;
 import frc.robot.subsystems.drive.ModuleIOTalonFX;
 import frc.robot.subsystems.launcher.FlywheelIO;
+import frc.robot.subsystems.launcher.FlywheelIOSim;
 import frc.robot.subsystems.launcher.HoodIO;
+import frc.robot.subsystems.launcher.HoodIOSim;
 import frc.robot.subsystems.launcher.Launcher;
 import frc.robot.subsystems.launcher.TurretIO;
 import frc.robot.subsystems.launcher.TurretIOSim;
@@ -150,8 +152,8 @@ public Robot() {
                 drive::getPose,
                 drive::getRobotRelativeChassisSpeeds,
                 new TurretIOSim(),
-                new FlywheelIO() {},
-                new HoodIO() {});
+                new FlywheelIOSim(),
+                new HoodIOSim());
         break;
 
       case REPLAY: // Replaying a log
@@ -200,7 +202,9 @@ public Robot() {
     SmartDashboard.putData("Field", field);
     Field.plotRegions();
 
-    launcher.setDefaultCommand(Commands.run(launcher::aimAtHub, launcher).withName("Aim at hub"));
+    launcher.setDefaultCommand(
+        Commands.run(() -> launcher.aim(GameState.getMyHubPose().getTranslation()), launcher)
+            .withName("Aim at hub"));
   }
 
   /** This function is called periodically during all modes. */

src/main/java/frc/robot/subsystems/launcher/FlywheelIOSim.java

@@ -0,0 +1,66 @@
+package frc.robot.subsystems.launcher;
+
+import static edu.wpi.first.units.Units.*;
+import static frc.robot.subsystems.launcher.LauncherConstants.FlywheelConstants.*;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.system.plant.LinearSystemId;
+import edu.wpi.first.units.measure.AngularVelocity;
+import edu.wpi.first.wpilibj.simulation.DCMotorSim;
+import frc.robot.Constants.RobotConstants;
+import frc.robot.Robot;
+
+public class FlywheelIOSim implements FlywheelIO {
+  private final DCMotorSim flywheelSim;
+
+  private boolean closedLoop = false;
+  private PIDController velocityController = new PIDController(kPSim, 0.0, 0.0);
+  private double appliedVolts = 0.0;
+  private double feedforwardVolts = 0.0;
+
+  public FlywheelIOSim() {
+    flywheelSim =
+        new DCMotorSim(LinearSystemId.createDCMotorSystem(gearbox, 0.004, motorReduction), gearbox);
+  }
+
+  @Override
+  public void updateInputs(FlywheelIOInputs inputs) {
+    // Run closed-loop control
+    if (closedLoop) {
+      appliedVolts =
+          velocityController.calculate(flywheelSim.getAngularVelocityRadPerSec())
+              + feedforwardVolts;
+    } else {
+      velocityController.reset();
+    }
+
+    // Update simulation state
+    flywheelSim.setInputVoltage(
+        MathUtil.clamp(
+            appliedVolts, -RobotConstants.kNominalVoltage, RobotConstants.kNominalVoltage));
+    flywheelSim.update(Robot.defaultPeriodSecs);
+
+    // Update turn inputs
+    inputs.connected = true;
+    inputs.velocityRadPerSec = flywheelSim.getAngularVelocityRadPerSec();
+    inputs.appliedVolts = appliedVolts;
+    inputs.currentAmps = Math.abs(flywheelSim.getCurrentDrawAmps());
+  }
+
+  @Override
+  public void setOpenLoop(double output) {
+    closedLoop = false;
+    appliedVolts = output;
+  }
+
+  @Override
+  public void setVelocity(AngularVelocity angularVelocity) {
+    closedLoop = true;
+    this.feedforwardVolts =
+        RobotConstants.kNominalVoltage
+            * angularVelocity.in(RadiansPerSecond)
+            / maxAngularVelocity.in(RadiansPerSecond);
+    velocityController.setSetpoint(angularVelocity.in(RadiansPerSecond));
+  }
+}

src/main/java/frc/robot/subsystems/launcher/FlywheelIOTalonFX.java

@@ -0,0 +1,80 @@
+package frc.robot.subsystems.launcher;
+
+import static frc.robot.subsystems.drive.DriveConstants.kCANBus;
+import static frc.robot.subsystems.launcher.LauncherConstants.FlywheelConstants.*;
+import static frc.robot.util.PhoenixUtil.tryUntilOk;
+
+import com.ctre.phoenix6.BaseStatusSignal;
+import com.ctre.phoenix6.StatusSignal;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.controls.PositionTorqueCurrentFOC;
+import com.ctre.phoenix6.controls.PositionVoltage;
+import com.ctre.phoenix6.controls.TorqueCurrentFOC;
+import com.ctre.phoenix6.controls.VelocityTorqueCurrentFOC;
+import com.ctre.phoenix6.controls.VelocityVoltage;
+import com.ctre.phoenix6.controls.VoltageOut;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.NeutralModeValue;
+import edu.wpi.first.math.filter.Debouncer;
+import edu.wpi.first.units.measure.Angle;
+import edu.wpi.first.units.measure.AngularVelocity;
+import edu.wpi.first.units.measure.Current;
+import edu.wpi.first.units.measure.Voltage;
+import frc.robot.subsystems.drive.PhoenixOdometryThread;
+import java.util.Queue;
+
+public class FlywheelIOTalonFX implements FlywheelIO {
+  private final TalonFX flywheelTalon;
+  private final TalonFXConfiguration config;
+  private final Debouncer turnConnectedDebounce =
+      new Debouncer(0.5, Debouncer.DebounceType.kFalling);
+
+  // Voltage control requests
+  private final VoltageOut voltageRequest = new VoltageOut(0);
+  private final PositionVoltage positionVoltageRequest = new PositionVoltage(0.0);
+  private final VelocityVoltage velocityVoltageRequest = new VelocityVoltage(0.0);
+
+  // Torque-current control requests
+  private final TorqueCurrentFOC torqueCurrentRequest = new TorqueCurrentFOC(0);
+  private final PositionTorqueCurrentFOC positionTorqueCurrentRequest =
+      new PositionTorqueCurrentFOC(0.0);
+  private final VelocityTorqueCurrentFOC velocityTorqueCurrentRequest =
+      new VelocityTorqueCurrentFOC(0.0);
+
+  // Timestamp inputs from Phoenix thread
+  private final Queue<Double> timestampQueue;
+
+  // Inputs from drive motor
+  private final StatusSignal<Angle> flywheelPosition;
+  private final Queue<Double> flywheelPositionQueue;
+  private final StatusSignal<AngularVelocity> flywheelVelocity;
+  private final StatusSignal<Voltage> flywheelAppliedVolts;
+  private final StatusSignal<Current> flywheelCurrent;
+
+  public FlywheelIOTalonFX() {
+    flywheelTalon = new TalonFX(port, kCANBus);
+    // Configuration
+    config = new TalonFXConfiguration();
+    config.MotorOutput.NeutralMode = NeutralModeValue.Brake;
+    config.Slot0 = flywheelGains;
+    tryUntilOk(5, () -> flywheelTalon.getConfigurator().apply(config, 0.25));
+
+    timestampQueue = PhoenixOdometryThread.getInstance().makeTimestampQueue();
+
+    flywheelPosition = flywheelTalon.getPosition();
+    flywheelPositionQueue =
+        PhoenixOdometryThread.getInstance().registerSignal(flywheelPosition.clone());
+    flywheelVelocity = flywheelTalon.getVelocity();
+    flywheelAppliedVolts = flywheelTalon.getMotorVoltage();
+    flywheelCurrent = flywheelTalon.getStatorCurrent();
+
+    BaseStatusSignal.setUpdateFrequencyForAll(
+        50.0,
+        flywheelVelocity,
+        flywheelAppliedVolts,
+        flywheelCurrent,
+        flywheelVelocity,
+        flywheelAppliedVolts,
+        flywheelCurrent);
+  }
+}

src/main/java/frc/robot/subsystems/launcher/HoodIOSim.java

@@ -0,0 +1,68 @@
+package frc.robot.subsystems.launcher;
+
+import static edu.wpi.first.units.Units.*;
+import static frc.robot.subsystems.launcher.LauncherConstants.HoodConstants.*;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.system.plant.LinearSystemId;
+import edu.wpi.first.units.measure.AngularVelocity;
+import edu.wpi.first.wpilibj.simulation.DCMotorSim;
+import frc.robot.Constants.RobotConstants;
+import frc.robot.Robot;
+
+public class HoodIOSim implements HoodIO {
+
+  private final DCMotorSim hoodSim;
+
+  private boolean closedLoop = false;
+  private PIDController positionController = new PIDController(kPSim, 0.0, kDSim);
+  private double appliedVolts = 0.0;
+  private double feedforwardVolts = 0.0;
+
+  public HoodIOSim() {
+    hoodSim =
+        new DCMotorSim(LinearSystemId.createDCMotorSystem(gearbox, 0.004, motorReduction), gearbox);
+  }
+
+  @Override
+  public void updateInputs(HoodIOInputs inputs) {
+    // Run closed-loop control
+    if (closedLoop) {
+      appliedVolts =
+          positionController.calculate(hoodSim.getAngularPositionRad()) + feedforwardVolts;
+    } else {
+      positionController.reset();
+    }
+
+    // Update simulation state
+    hoodSim.setInputVoltage(
+        MathUtil.clamp(
+            appliedVolts, -RobotConstants.kNominalVoltage, RobotConstants.kNominalVoltage));
+    hoodSim.update(Robot.defaultPeriodSecs);
+
+    // Update turn inputs
+    inputs.connected = true;
+    inputs.position = new Rotation2d(hoodSim.getAngularPositionRad());
+    inputs.velocityRadPerSec = hoodSim.getAngularVelocityRadPerSec();
+    inputs.appliedVolts = appliedVolts;
+    inputs.currentAmps = Math.abs(hoodSim.getCurrentDrawAmps());
+  }
+
+  @Override
+  public void setOpenLoop(double output) {
+    closedLoop = false;
+    appliedVolts = output;
+  }
+
+  @Override
+  public void setPosition(Rotation2d rotation, AngularVelocity angularVelocity) {
+    closedLoop = true;
+    this.feedforwardVolts =
+        RobotConstants.kNominalVoltage
+            * angularVelocity.in(RadiansPerSecond)
+            / maxAngularVelocity.in(RadiansPerSecond);
+    positionController.setSetpoint(rotation.getRadians());
+  }
+}

src/main/java/frc/robot/subsystems/launcher/HoodIOSpark.java

@@ -0,0 +1,109 @@
+package frc.robot.subsystems.launcher;
+
+import static edu.wpi.first.units.Units.RadiansPerSecond;
+import static frc.robot.subsystems.launcher.LauncherConstants.HoodConstants.*;
+import static frc.robot.subsystems.launcher.LauncherConstants.HoodConstants.port;
+import static frc.robot.util.SparkUtil.*;
+
+import com.revrobotics.AbsoluteEncoder;
+import com.revrobotics.PersistMode;
+import com.revrobotics.ResetMode;
+import com.revrobotics.spark.ClosedLoopSlot;
+import com.revrobotics.spark.FeedbackSensor;
+import com.revrobotics.spark.SparkBase;
+import com.revrobotics.spark.SparkBase.ControlType;
+import com.revrobotics.spark.SparkClosedLoopController;
+import com.revrobotics.spark.SparkLowLevel.MotorType;
+import com.revrobotics.spark.SparkMax;
+import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
+import com.revrobotics.spark.config.SparkMaxConfig;
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.filter.Debouncer;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.units.measure.AngularVelocity;
+import frc.robot.Constants.MotorConstants.NEO550Constants;
+import frc.robot.Constants.RobotConstants;
+import java.util.function.DoubleSupplier;
+
+public class HoodIOSpark implements HoodIO {
+
+  private final SparkBase hoodSpark;
+  private final AbsoluteEncoder hoodEncoder;
+  private final SparkClosedLoopController hoodController;
+  private final Debouncer turnConnectedDebounce =
+      new Debouncer(0.5, Debouncer.DebounceType.kFalling);
+
+  public HoodIOSpark() {
+    hoodSpark = new SparkMax(port, MotorType.kBrushless);
+    hoodEncoder = hoodSpark.getAbsoluteEncoder();
+    hoodController = hoodSpark.getClosedLoopController();
+
+    var hoodConfig = new SparkMaxConfig();
+
+    hoodConfig
+        .inverted(false)
+        .idleMode(IdleMode.kBrake)
+        .smartCurrentLimit(NEO550Constants.kDefaultSupplyCurrentLimit)
+        .voltageCompensation(RobotConstants.kNominalVoltage);
+
+    hoodConfig
+        .absoluteEncoder
+        .inverted(false)
+        .positionConversionFactor(encoderPositionFactor)
+        .velocityConversionFactor(encoderVelocityFactor)
+        .averageDepth(2);
+
+    hoodConfig
+        .closedLoop
+        .feedbackSensor(FeedbackSensor.kAbsoluteEncoder)
+        .positionWrappingEnabled(true)
+        .positionWrappingInputRange(minInput, maxInput)
+        .pid(kPReal, 0.0, 0.0);
+
+    hoodConfig
+        .signals
+        .absoluteEncoderPositionAlwaysOn(true)
+        .absoluteEncoderPositionPeriodMs(20)
+        .absoluteEncoderVelocityAlwaysOn(true)
+        .absoluteEncoderVelocityPeriodMs(20)
+        .appliedOutputPeriodMs(20)
+        .busVoltagePeriodMs(20)
+        .outputCurrentPeriodMs(20);
+
+    tryUntilOk(
+        hoodSpark,
+        5,
+        () ->
+            hoodSpark.configure(
+                hoodConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters));
+  }
+
+  @Override
+  public void updateInputs(HoodIOInputs inputs) {
+    sparkStickyFault = false;
+    ifOk(hoodSpark, hoodEncoder::getPosition, (value) -> inputs.position = new Rotation2d(value));
+    ifOk(hoodSpark, hoodEncoder::getVelocity, (value) -> inputs.velocityRadPerSec = value);
+    ifOk(
+        hoodSpark,
+        new DoubleSupplier[] {hoodSpark::getAppliedOutput, hoodSpark::getBusVoltage},
+        (values) -> inputs.appliedVolts = values[0] * values[1]);
+    ifOk(hoodSpark, hoodSpark::getOutputCurrent, (value) -> inputs.currentAmps = value);
+    inputs.connected = turnConnectedDebounce.calculate(!sparkStickyFault);
+  }
+
+  @Override
+  public void setOpenLoop(double output) {
+    hoodSpark.setVoltage(output);
+  }
+
+  @Override
+  public void setPosition(Rotation2d rotation, AngularVelocity angularVelocity) {
+    double setpoint = MathUtil.inputModulus(rotation.getRadians(), minInput, maxInput);
+    double feedforwardVolts =
+        RobotConstants.kNominalVoltage
+            * angularVelocity.in(RadiansPerSecond)
+            / maxAngularVelocity.in(RadiansPerSecond);
+    hoodController.setSetpoint(
+        setpoint, ControlType.kPosition, ClosedLoopSlot.kSlot0, feedforwardVolts);
+  }
+}