From 5c19f26fbf9d8fa66864c7248a02580afbff16bd Mon Sep 17 00:00:00 2001 From: BenGamer3 Date: Tue, 27 Jan 2026 19:12:31 -0500 Subject: [PATCH 01/13] added hood sim and spark --- .../robot/subsystems/launcher/HoodIOSim.java | 71 +++++++++++ .../subsystems/launcher/HoodIOSpark.java | 117 ++++++++++++++++++ .../launcher/LauncherConstants.java | 26 ++++ 3 files changed, 214 insertions(+) create mode 100644 src/main/java/frc/robot/subsystems/launcher/HoodIOSim.java create mode 100644 src/main/java/frc/robot/subsystems/launcher/HoodIOSpark.java diff --git a/src/main/java/frc/robot/subsystems/launcher/HoodIOSim.java b/src/main/java/frc/robot/subsystems/launcher/HoodIOSim.java new file mode 100644 index 0000000..c321a8f --- /dev/null +++ b/src/main/java/frc/robot/subsystems/launcher/HoodIOSim.java @@ -0,0 +1,71 @@ +package frc.robot.subsystems.launcher; + +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.AngularVelocityUnit; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.wpilibj.simulation.DCMotorSim; +import frc.robot.Constants.RobotConstants; + +import static frc.robot.subsystems.launcher.LauncherConstants.HoodConstants.*; + +public class HoodIOSim implements HoodIO { + private static final AngularVelocityUnit RadiansPerSecond = null; + +private final DCMotorSim hoodSim; + + private boolean turnClosedLoop = false; + private PIDController turnController = new PIDController(hoodKpSim, 0.0, hoodKdSim); + private double turnAppliedVolts = 0.0; + private double feedforwardVolts = 0.0; + + public HoodIOSim() { + hoodSim = + new DCMotorSim( + LinearSystemId.createDCMotorSystem(hoodGearbox, 0.004, hoodMotorReduction), + hoodGearbox); + + // Enable wrapping for turn PID + turnController.enableContinuousInput(-Math.PI, Math.PI); + } + + @Override + public void updateInputs(HoodIOInputs inputs) { + // Run closed-loop control + if (turnClosedLoop) { + turnAppliedVolts = + turnController.calculate(hoodSim.getAngularPositionRad()) + feedforwardVolts; + } else { + turnController.reset(); + } + + // Update simulation state + hoodSim.setInputVoltage(MathUtil.clamp(turnAppliedVolts, -12.0, 12.0)); + hoodSim.update(0.02); + + // Update turn inputs + inputs.connected = true; + inputs.position = new Rotation2d(hoodSim.getAngularPositionRad()); + inputs.velocityRadPerSec = hoodSim.getAngularVelocityRadPerSec(); + inputs.appliedVolts = turnAppliedVolts; + inputs.currentAmps = Math.abs(hoodSim.getCurrentDrawAmps()); + } + + @Override + public void setOpenLoop(double output) { + turnClosedLoop = false; + turnAppliedVolts = output; + } + + @Override + public void setPosition(Rotation2d rotation, AngularVelocity angularVelocity) { + turnClosedLoop = true; + this.feedforwardVolts = + RobotConstants.kNominalVoltage + * angularVelocity.in(RadiansPerSecond) + / hoodMaxAngularVelocity.in(RadiansPerSecond); + turnController.setSetpoint(rotation.getRadians()); + } +} diff --git a/src/main/java/frc/robot/subsystems/launcher/HoodIOSpark.java b/src/main/java/frc/robot/subsystems/launcher/HoodIOSpark.java new file mode 100644 index 0000000..9b67399 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/launcher/HoodIOSpark.java @@ -0,0 +1,117 @@ +package frc.robot.subsystems.launcher; + +import static edu.wpi.first.units.Units.RadiansPerSecond; +import static frc.robot.subsystems.launcher.LauncherConstants.HoodConstants.hoodPort; +import static frc.robot.util.SparkUtil.*; + +import java.util.function.DoubleSupplier; + +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 static frc.robot.subsystems.launcher.LauncherConstants.HoodConstants.*; + +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(hoodPort, 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(hoodEncoderPositionFactor) + .velocityConversionFactor(hoodEncoderVelocityFactor) + .averageDepth(2); + + hoodConfig + .closedLoop + .feedbackSensor(FeedbackSensor.kAbsoluteEncoder) + .positionWrappingEnabled(true) + .positionWrappingInputRange(hoodPIDMinInput, hoodPIDMaxInput) + .pid(hoodKp, 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(), hoodPIDMinInput, hoodPIDMaxInput); + double feedforwardVolts = + RobotConstants.kNominalVoltage + * angularVelocity.in(RadiansPerSecond) + / hoodMaxAngularVelocity.in(RadiansPerSecond); + hoodController.setSetpoint( + setpoint, ControlType.kPosition, ClosedLoopSlot.kSlot0, feedforwardVolts); + } +} + diff --git a/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java b/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java index 594533f..581e9e3 100644 --- a/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java +++ b/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java @@ -2,6 +2,8 @@ import static edu.wpi.first.units.Units.*; +import javax.sound.sampled.Port; + import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; @@ -55,4 +57,28 @@ public static final class TurretConstants { public static final class FlywheelConstants { public static final Distance wheelRadius = Inches.of(1.5); } + + public static final class HoodConstants { + //Absolute encoder + public static final double hoodEncoderPositionFactor = 2 * Math.PI; //Radians + public static final double hoodEncoderVelocityFactor = (2 * Math.PI) / 60.0; //Rad/sec + + //Position controller + public static final double hoodPIDMinInput = 0.0; + public static final double hoodPIDMaxInput = 2 * Math.PI; + public static final double hoodKp = 0.35; + + //Motor controller + public static final int hoodPort = 1; + public static final double hoodMotorReduction = 275.0; + public static final AngularVelocity hoodMaxAngularVelocity = + NEO550Constants.kFreeSpeed.div(hoodMotorReduction); + + //Simulation + public static final double hoodKpSim = 1; + public static final double hoodKdSim = 0; + public static final DCMotor hoodGearbox = DCMotor.getNeo550(1); + + } } + From c65be5e8fb9d7e12276b75b679298cad8c556a3e Mon Sep 17 00:00:00 2001 From: DylanTaylor29 <132412179+DylanTaylor29@users.noreply.github.com> Date: Tue, 27 Jan 2026 20:23:03 -0500 Subject: [PATCH 02/13] created flywheel sim file and started working on talonfx file for flywheel --- .../subsystems/launcher/FlywheelIOSim.java | 61 ++++++++++++++ .../launcher/FlywheelIOTalonFX.java | 83 +++++++++++++++++++ .../launcher/LauncherConstants.java | 34 ++++++-- 3 files changed, 172 insertions(+), 6 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/launcher/FlywheelIOSim.java create mode 100644 src/main/java/frc/robot/subsystems/launcher/FlywheelIOTalonFX.java diff --git a/src/main/java/frc/robot/subsystems/launcher/FlywheelIOSim.java b/src/main/java/frc/robot/subsystems/launcher/FlywheelIOSim.java new file mode 100644 index 0000000..9412470 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/launcher/FlywheelIOSim.java @@ -0,0 +1,61 @@ +package frc.robot.subsystems.launcher; + +import static edu.wpi.first.units.Units.RadiansPerSecond; +import static frc.robot.subsystems.launcher.LauncherConstants.FlywheelConstants.flywheelGearbox; +import static frc.robot.subsystems.launcher.LauncherConstants.FlywheelConstants.flywheelKpSim; +import static frc.robot.subsystems.launcher.LauncherConstants.FlywheelConstants.flywheelMotorReduction; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.wpilibj.simulation.DCMotorSim; +import edu.wpi.first.math.system.plant.LinearSystemId; +import edu.wpi.first.units.measure.AngularVelocity; + +public class FlywheelIOSim implements FlywheelIO { + private final DCMotorSim flywheelSim; + + private boolean flywheelClosedLoop = false; + private PIDController flywheelController = new PIDController(flywheelKpSim, 0.0, 0.0); + private double flywheelAppliedVolts = 0.0; + private double feedforwardVolts = 0.0; + + public FlywheelIOSim() { + flywheelSim = + new DCMotorSim( + LinearSystemId.createDCMotorSystem(flywheelGearbox, 0.004, flywheelMotorReduction), + flywheelGearbox); + } + + @Override + public void updateInputs(FlywheelIOInputs inputs) { + // Run closed-loop control + if (flywheelClosedLoop) { + flywheelAppliedVolts = + flywheelController.calculate(flywheelSim.getAngularPositionRad()) + feedforwardVolts; + } else { + flywheelController.reset(); + } + + // Update simulation state + flywheelSim.setInputVoltage(MathUtil.clamp(flywheelAppliedVolts, -12.0, 12.0)); + flywheelSim.update(0.02); + + // Update turn inputs + inputs.connected = true; + inputs.velocityRadPerSec = flywheelSim.getAngularVelocityRadPerSec(); + inputs.appliedVolts = flywheelAppliedVolts; + inputs.currentAmps = Math.abs(flywheelSim.getCurrentDrawAmps()); + } + + @Override + public void setOpenLoop(double output) { + flywheelClosedLoop = false; + flywheelAppliedVolts = output; + } + + @Override + public void setVelocity(AngularVelocity angularVelocity) { + flywheelClosedLoop = true; + flywheelController.setSetpoint(angularVelocity.in(RadiansPerSecond)); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/launcher/FlywheelIOTalonFX.java b/src/main/java/frc/robot/subsystems/launcher/FlywheelIOTalonFX.java new file mode 100644 index 0000000..41ee28d --- /dev/null +++ b/src/main/java/frc/robot/subsystems/launcher/FlywheelIOTalonFX.java @@ -0,0 +1,83 @@ +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 java.util.Queue; + +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.InvertedValue; +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.Drive; +import frc.robot.subsystems.drive.PhoenixOdometryThread; + +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 timestampQueue; + + // Inputs from drive motor + private final StatusSignal flywheelPosition; + private final Queue flywheelPositionQueue; + private final StatusSignal flywheelVelocity; + private final StatusSignal flywheelAppliedVolts; + private final StatusSignal flywheelCurrent; + + public FlywheelIOTalonFX() { + flywheelTalon = new TalonFX(flywheelPort, 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); + } +} diff --git a/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java b/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java index 581e9e3..9c1c315 100644 --- a/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java +++ b/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java @@ -2,7 +2,8 @@ import static edu.wpi.first.units.Units.*; -import javax.sound.sampled.Port; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.signals.StaticFeedforwardSignValue; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation3d; @@ -11,6 +12,7 @@ import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Distance; import edu.wpi.first.units.measure.LinearVelocity; +import frc.robot.Constants.MotorConstants.KrakenX60Constants; import frc.robot.Constants.MotorConstants.NEO550Constants; public final class LauncherConstants { @@ -56,29 +58,49 @@ public static final class TurretConstants { public static final class FlywheelConstants { public static final Distance wheelRadius = Inches.of(1.5); + + // Velocity Controller + public static final Slot0Configs flywheelGains = + new Slot0Configs() + .withKP(1) + .withKI(0) + .withKD(1) + .withKS(0) + .withKV(0) + .withKA(0) + .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); + + // Motor controller + public static final int flywheelPort = 2; + public static final double flywheelMotorReduction = 1.0; + public static final AngularVelocity flywheelMaxAngularVelocity = + KrakenX60Constants.kFreeSpeed.div(flywheelMotorReduction); + + // Simulation + public static final double flywheelKpSim = 1; + public static final DCMotor flywheelGearbox = DCMotor.getKrakenX60(1); } public static final class HoodConstants { - //Absolute encoder + // Absolute encoder public static final double hoodEncoderPositionFactor = 2 * Math.PI; //Radians public static final double hoodEncoderVelocityFactor = (2 * Math.PI) / 60.0; //Rad/sec - //Position controller + // Position controller public static final double hoodPIDMinInput = 0.0; public static final double hoodPIDMaxInput = 2 * Math.PI; public static final double hoodKp = 0.35; - //Motor controller + // Motor controller public static final int hoodPort = 1; public static final double hoodMotorReduction = 275.0; public static final AngularVelocity hoodMaxAngularVelocity = NEO550Constants.kFreeSpeed.div(hoodMotorReduction); - //Simulation + // Simulation public static final double hoodKpSim = 1; public static final double hoodKdSim = 0; public static final DCMotor hoodGearbox = DCMotor.getNeo550(1); - } } From 1eef3ca7f505737400c55660c0515de16ca9fd61 Mon Sep 17 00:00:00 2001 From: nlaverdure Date: Thu, 29 Jan 2026 18:49:39 -0500 Subject: [PATCH 03/13] use actual positions for plotting the actual trajectory --- .../subsystems/launcher/FlywheelIOSim.java | 92 +++++++++--------- .../launcher/FlywheelIOTalonFX.java | 95 +++++++++---------- .../robot/subsystems/launcher/HoodIOSim.java | 6 +- .../subsystems/launcher/HoodIOSpark.java | 19 ++-- .../robot/subsystems/launcher/Launcher.java | 6 +- .../launcher/LauncherConstants.java | 31 +++--- 6 files changed, 117 insertions(+), 132 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/launcher/FlywheelIOSim.java b/src/main/java/frc/robot/subsystems/launcher/FlywheelIOSim.java index 9412470..4dfedcc 100644 --- a/src/main/java/frc/robot/subsystems/launcher/FlywheelIOSim.java +++ b/src/main/java/frc/robot/subsystems/launcher/FlywheelIOSim.java @@ -7,55 +7,55 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.wpilibj.simulation.DCMotorSim; import edu.wpi.first.math.system.plant.LinearSystemId; import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.wpilibj.simulation.DCMotorSim; public class FlywheelIOSim implements FlywheelIO { - private final DCMotorSim flywheelSim; - - private boolean flywheelClosedLoop = false; - private PIDController flywheelController = new PIDController(flywheelKpSim, 0.0, 0.0); - private double flywheelAppliedVolts = 0.0; - private double feedforwardVolts = 0.0; - - public FlywheelIOSim() { - flywheelSim = - new DCMotorSim( - LinearSystemId.createDCMotorSystem(flywheelGearbox, 0.004, flywheelMotorReduction), - flywheelGearbox); - } - - @Override - public void updateInputs(FlywheelIOInputs inputs) { - // Run closed-loop control - if (flywheelClosedLoop) { - flywheelAppliedVolts = - flywheelController.calculate(flywheelSim.getAngularPositionRad()) + feedforwardVolts; - } else { - flywheelController.reset(); - } - - // Update simulation state - flywheelSim.setInputVoltage(MathUtil.clamp(flywheelAppliedVolts, -12.0, 12.0)); - flywheelSim.update(0.02); - - // Update turn inputs - inputs.connected = true; - inputs.velocityRadPerSec = flywheelSim.getAngularVelocityRadPerSec(); - inputs.appliedVolts = flywheelAppliedVolts; - inputs.currentAmps = Math.abs(flywheelSim.getCurrentDrawAmps()); + private final DCMotorSim flywheelSim; + + private boolean flywheelClosedLoop = false; + private PIDController flywheelController = new PIDController(flywheelKpSim, 0.0, 0.0); + private double flywheelAppliedVolts = 0.0; + private double feedforwardVolts = 0.0; + + public FlywheelIOSim() { + flywheelSim = + new DCMotorSim( + LinearSystemId.createDCMotorSystem(flywheelGearbox, 0.004, flywheelMotorReduction), + flywheelGearbox); + } + + @Override + public void updateInputs(FlywheelIOInputs inputs) { + // Run closed-loop control + if (flywheelClosedLoop) { + flywheelAppliedVolts = + flywheelController.calculate(flywheelSim.getAngularPositionRad()) + feedforwardVolts; + } else { + flywheelController.reset(); } - @Override - public void setOpenLoop(double output) { - flywheelClosedLoop = false; - flywheelAppliedVolts = output; - } - - @Override - public void setVelocity(AngularVelocity angularVelocity) { - flywheelClosedLoop = true; - flywheelController.setSetpoint(angularVelocity.in(RadiansPerSecond)); - } -} \ No newline at end of file + // Update simulation state + flywheelSim.setInputVoltage(MathUtil.clamp(flywheelAppliedVolts, -12.0, 12.0)); + flywheelSim.update(0.02); + + // Update turn inputs + inputs.connected = true; + inputs.velocityRadPerSec = flywheelSim.getAngularVelocityRadPerSec(); + inputs.appliedVolts = flywheelAppliedVolts; + inputs.currentAmps = Math.abs(flywheelSim.getCurrentDrawAmps()); + } + + @Override + public void setOpenLoop(double output) { + flywheelClosedLoop = false; + flywheelAppliedVolts = output; + } + + @Override + public void setVelocity(AngularVelocity angularVelocity) { + flywheelClosedLoop = true; + flywheelController.setSetpoint(angularVelocity.in(RadiansPerSecond)); + } +} diff --git a/src/main/java/frc/robot/subsystems/launcher/FlywheelIOTalonFX.java b/src/main/java/frc/robot/subsystems/launcher/FlywheelIOTalonFX.java index 41ee28d..a57d4e2 100644 --- a/src/main/java/frc/robot/subsystems/launcher/FlywheelIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/launcher/FlywheelIOTalonFX.java @@ -4,8 +4,6 @@ import static frc.robot.subsystems.launcher.LauncherConstants.FlywheelConstants.*; import static frc.robot.util.PhoenixUtil.tryUntilOk; -import java.util.Queue; - import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.TalonFXConfiguration; @@ -16,68 +14,67 @@ import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.InvertedValue; 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.Drive; 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); + 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); + // 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); + // 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 timestampQueue; + // Timestamp inputs from Phoenix thread + private final Queue timestampQueue; - // Inputs from drive motor - private final StatusSignal flywheelPosition; - private final Queue flywheelPositionQueue; - private final StatusSignal flywheelVelocity; - private final StatusSignal flywheelAppliedVolts; - private final StatusSignal flywheelCurrent; + // Inputs from drive motor + private final StatusSignal flywheelPosition; + private final Queue flywheelPositionQueue; + private final StatusSignal flywheelVelocity; + private final StatusSignal flywheelAppliedVolts; + private final StatusSignal flywheelCurrent; - public FlywheelIOTalonFX() { - flywheelTalon = new TalonFX(flywheelPort, kCANBus); - // Configuration - config = new TalonFXConfiguration(); - config.MotorOutput.NeutralMode = NeutralModeValue.Brake; - config.Slot0 = flywheelGains; - tryUntilOk(5, () -> flywheelTalon.getConfigurator().apply(config, 0.25)); + public FlywheelIOTalonFX() { + flywheelTalon = new TalonFX(flywheelPort, 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(); + timestampQueue = PhoenixOdometryThread.getInstance().makeTimestampQueue(); - flywheelPosition = flywheelTalon.getPosition(); - flywheelPositionQueue = PhoenixOdometryThread.getInstance().registerSignal(flywheelPosition.clone()); - flywheelVelocity = flywheelTalon.getVelocity(); - flywheelAppliedVolts = flywheelTalon.getMotorVoltage(); - flywheelCurrent = flywheelTalon.getStatorCurrent(); + 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); - } + BaseStatusSignal.setUpdateFrequencyForAll( + 50.0, + flywheelVelocity, + flywheelAppliedVolts, + flywheelCurrent, + flywheelVelocity, + flywheelAppliedVolts, + flywheelCurrent); + } } diff --git a/src/main/java/frc/robot/subsystems/launcher/HoodIOSim.java b/src/main/java/frc/robot/subsystems/launcher/HoodIOSim.java index c321a8f..8597097 100644 --- a/src/main/java/frc/robot/subsystems/launcher/HoodIOSim.java +++ b/src/main/java/frc/robot/subsystems/launcher/HoodIOSim.java @@ -1,5 +1,7 @@ package frc.robot.subsystems.launcher; +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; @@ -9,12 +11,10 @@ import edu.wpi.first.wpilibj.simulation.DCMotorSim; import frc.robot.Constants.RobotConstants; -import static frc.robot.subsystems.launcher.LauncherConstants.HoodConstants.*; - public class HoodIOSim implements HoodIO { private static final AngularVelocityUnit RadiansPerSecond = null; -private final DCMotorSim hoodSim; + private final DCMotorSim hoodSim; private boolean turnClosedLoop = false; private PIDController turnController = new PIDController(hoodKpSim, 0.0, hoodKdSim); diff --git a/src/main/java/frc/robot/subsystems/launcher/HoodIOSpark.java b/src/main/java/frc/robot/subsystems/launcher/HoodIOSpark.java index 9b67399..951f468 100644 --- a/src/main/java/frc/robot/subsystems/launcher/HoodIOSpark.java +++ b/src/main/java/frc/robot/subsystems/launcher/HoodIOSpark.java @@ -1,11 +1,10 @@ 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.hoodPort; import static frc.robot.util.SparkUtil.*; -import java.util.function.DoubleSupplier; - import com.revrobotics.AbsoluteEncoder; import com.revrobotics.PersistMode; import com.revrobotics.ResetMode; @@ -18,14 +17,13 @@ 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 static frc.robot.subsystems.launcher.LauncherConstants.HoodConstants.*; +import java.util.function.DoubleSupplier; public class HoodIOSpark implements HoodIO { @@ -73,8 +71,8 @@ public HoodIOSpark() { .outputCurrentPeriodMs(20); tryUntilOk( - hoodSpark, - 5, + hoodSpark, + 5, () -> hoodSpark.configure( hoodConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters)); @@ -83,10 +81,7 @@ public HoodIOSpark() { @Override public void updateInputs(HoodIOInputs inputs) { sparkStickyFault = false; - ifOk( - hoodSpark, - hoodEncoder::getPosition, - (value) -> inputs.position = new Rotation2d(value)); + ifOk(hoodSpark, hoodEncoder::getPosition, (value) -> inputs.position = new Rotation2d(value)); ifOk(hoodSpark, hoodEncoder::getVelocity, (value) -> inputs.velocityRadPerSec = value); ifOk( hoodSpark, @@ -104,8 +99,7 @@ public void setOpenLoop(double output) { @Override public void setPosition(Rotation2d rotation, AngularVelocity angularVelocity) { double setpoint = - MathUtil.inputModulus( - rotation.getRadians(), hoodPIDMinInput, hoodPIDMaxInput); + MathUtil.inputModulus(rotation.getRadians(), hoodPIDMinInput, hoodPIDMaxInput); double feedforwardVolts = RobotConstants.kNominalVoltage * angularVelocity.in(RadiansPerSecond) @@ -114,4 +108,3 @@ public void setPosition(Rotation2d rotation, AngularVelocity angularVelocity) { setpoint, ControlType.kPosition, ClosedLoopSlot.kSlot0, feedforwardVolts); } } - diff --git a/src/main/java/frc/robot/subsystems/launcher/Launcher.java b/src/main/java/frc/robot/subsystems/launcher/Launcher.java index ca95ad4..a1f182e 100644 --- a/src/main/java/frc/robot/subsystems/launcher/Launcher.java +++ b/src/main/java/frc/robot/subsystems/launcher/Launcher.java @@ -119,9 +119,8 @@ public void aimAtHub() { var v_base = getTurretBaseSpeeds(turretBasePose.toPose2d().getRotation(), fieldRelative); // Get actual flywheel speed - // TODO: Replace with actual speed when flywheel simulation is added LinearVelocity flywheelSpeed = - MetersPerSecond.of(flywheelSetpoint.in(RadiansPerSecond) * wheelRadius.in(Meters)); + MetersPerSecond.of(flywheelInputs.velocityRadPerSec * wheelRadius.in(Meters)); // Replan shot using actual flywheel speed var v0_total = getV0(vectorTurretBaseToHub, flywheelSpeed, replannedKey); @@ -137,8 +136,7 @@ public void aimAtHub() { hoodIO.setPosition(hoodSetpoint, RadiansPerSecond.of(0)); // Get actual hood & turret position - // TODO: Replace with actual hood position when hood simulation is added - Rotation2d hoodPosition = hoodSetpoint; + Rotation2d hoodPosition = hoodInputs.position; Rotation2d turretPosition = turretInputs.position.plus(turretBasePose.toPose2d().getRotation()); // Build actual velocities diff --git a/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java b/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java index 9c1c315..3f6d1b1 100644 --- a/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java +++ b/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java @@ -4,7 +4,6 @@ import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.signals.StaticFeedforwardSignValue; - import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; @@ -21,8 +20,9 @@ public final class LauncherConstants { public static final Distance fuelRadius = Inches.of(3); public static final Distance ceilingHeight = Feet.of(11).plus(Inches.of(2)); public static final double g = 9.81; - public static final double fuelSpawnPeriod = 0.1; // seconds + // For logging + public static final double fuelSpawnPeriod = 0.1; // seconds public static final String nominalKey = "Nominal"; public static final String replannedKey = "Replanned"; public static final String actualKey = "Actual"; @@ -32,7 +32,6 @@ public static final class TurretConstants { public static final Transform3d chassisToTurretBase = new Transform3d(Inches.of(0), Inches.of(10), Inches.of(22), Rotation3d.kZero); public static final Rotation2d rotationOffset = new Rotation2d(0.44); - public static final LinearVelocity fuelVelocityRadial = MetersPerSecond.of(5); // Absolute encoder public static final double turnEncoderPositionFactor = 2 * Math.PI; // Radians @@ -42,7 +41,6 @@ public static final class TurretConstants { public static final double turnPIDMinInput = 0.0; public static final double turnPIDMaxInput = 2 * Math.PI; public static final double turnKp = 0.35; - public static final Distance hubWidth = Inches.of(41.73); // Motor controller public static final int port = 12; @@ -61,19 +59,19 @@ public static final class FlywheelConstants { // Velocity Controller public static final Slot0Configs flywheelGains = - new Slot0Configs() - .withKP(1) - .withKI(0) - .withKD(1) - .withKS(0) - .withKV(0) - .withKA(0) - .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); + new Slot0Configs() + .withKP(1) + .withKI(0) + .withKD(1) + .withKS(0) + .withKV(0) + .withKA(0) + .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); // Motor controller public static final int flywheelPort = 2; public static final double flywheelMotorReduction = 1.0; - public static final AngularVelocity flywheelMaxAngularVelocity = + public static final AngularVelocity flywheelMaxAngularVelocity = KrakenX60Constants.kFreeSpeed.div(flywheelMotorReduction); // Simulation @@ -83,8 +81,8 @@ public static final class FlywheelConstants { public static final class HoodConstants { // Absolute encoder - public static final double hoodEncoderPositionFactor = 2 * Math.PI; //Radians - public static final double hoodEncoderVelocityFactor = (2 * Math.PI) / 60.0; //Rad/sec + public static final double hoodEncoderPositionFactor = 2 * Math.PI; // Radians + public static final double hoodEncoderVelocityFactor = (2 * Math.PI) / 60.0; // Rad/sec // Position controller public static final double hoodPIDMinInput = 0.0; @@ -95,7 +93,7 @@ public static final class HoodConstants { public static final int hoodPort = 1; public static final double hoodMotorReduction = 275.0; public static final AngularVelocity hoodMaxAngularVelocity = - NEO550Constants.kFreeSpeed.div(hoodMotorReduction); + NEO550Constants.kFreeSpeed.div(hoodMotorReduction); // Simulation public static final double hoodKpSim = 1; @@ -103,4 +101,3 @@ public static final class HoodConstants { public static final DCMotor hoodGearbox = DCMotor.getNeo550(1); } } - From 6504382a7718623c71e37be21588983a65d41aa8 Mon Sep 17 00:00:00 2001 From: nlaverdure Date: Thu, 29 Jan 2026 18:51:02 -0500 Subject: [PATCH 04/13] use sim mechanisms in sim --- src/main/java/frc/robot/Robot.java | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index ec231b1..494b7e1 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/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 From e169c64fa299084c1c1ad6055cee906374741bb2 Mon Sep 17 00:00:00 2001 From: Christopher Larrieu Date: Thu, 29 Jan 2026 19:23:01 -0500 Subject: [PATCH 05/13] Fix null pointer and return early when flywheel velocity has zero horizontal component. --- .../java/frc/robot/subsystems/launcher/HoodIOSim.java | 3 +-- .../java/frc/robot/subsystems/launcher/Launcher.java | 11 +++++++++-- .../robot/subsystems/launcher/LauncherConstants.java | 1 - 3 files changed, 10 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/launcher/HoodIOSim.java b/src/main/java/frc/robot/subsystems/launcher/HoodIOSim.java index 8597097..e8a8a1d 100644 --- a/src/main/java/frc/robot/subsystems/launcher/HoodIOSim.java +++ b/src/main/java/frc/robot/subsystems/launcher/HoodIOSim.java @@ -1,18 +1,17 @@ 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.AngularVelocityUnit; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.wpilibj.simulation.DCMotorSim; import frc.robot.Constants.RobotConstants; public class HoodIOSim implements HoodIO { - private static final AngularVelocityUnit RadiansPerSecond = null; private final DCMotorSim hoodSim; diff --git a/src/main/java/frc/robot/subsystems/launcher/Launcher.java b/src/main/java/frc/robot/subsystems/launcher/Launcher.java index a1f182e..9f98ea9 100644 --- a/src/main/java/frc/robot/subsystems/launcher/Launcher.java +++ b/src/main/java/frc/robot/subsystems/launcher/Launcher.java @@ -127,12 +127,19 @@ public void aimAtHub() { // Point turret to align velocity vectors var v0_flywheel = v0_total.minus(v_base); + + // Check if v0_flywheel has non-zero horizontal component + double v0_horizontal = Math.hypot(v0_flywheel.getX(), v0_flywheel.getY()); + if (v0_horizontal < 1e-6) { + // Flywheel velocity is too low or target unreachable, stop mechanisms + return; + } + Rotation2d turretSetpoint = new Rotation2d(v0_flywheel.getX(), v0_flywheel.getY()); turretIO.setPosition( turretSetpoint.minus(turretBasePose.toPose2d().getRotation()), RadiansPerSecond.of(robotRelative.omegaRadiansPerSecond).unaryMinus().times(2.0)); - Rotation2d hoodSetpoint = - new Rotation2d(Math.hypot(v0_flywheel.getX(), v0_flywheel.getY()), v0_flywheel.getZ()); + Rotation2d hoodSetpoint = new Rotation2d(v0_horizontal, v0_flywheel.getZ()); hoodIO.setPosition(hoodSetpoint, RadiansPerSecond.of(0)); // Get actual hood & turret position diff --git a/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java b/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java index 3f6d1b1..927f791 100644 --- a/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java +++ b/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java @@ -10,7 +10,6 @@ import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Distance; -import edu.wpi.first.units.measure.LinearVelocity; import frc.robot.Constants.MotorConstants.KrakenX60Constants; import frc.robot.Constants.MotorConstants.NEO550Constants; From b5978ac93436411a1dfef54aadaa61f19862c435 Mon Sep 17 00:00:00 2001 From: nlaverdure Date: Fri, 30 Jan 2026 10:13:08 -0500 Subject: [PATCH 06/13] hood discontinuous --- .../robot/subsystems/launcher/HoodIOSim.java | 32 +++++++++---------- .../subsystems/launcher/TurretIOSim.java | 31 +++++++++--------- 2 files changed, 31 insertions(+), 32 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/launcher/HoodIOSim.java b/src/main/java/frc/robot/subsystems/launcher/HoodIOSim.java index e8a8a1d..81c84f1 100644 --- a/src/main/java/frc/robot/subsystems/launcher/HoodIOSim.java +++ b/src/main/java/frc/robot/subsystems/launcher/HoodIOSim.java @@ -9,15 +9,16 @@ 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.Robot; import frc.robot.Constants.RobotConstants; public class HoodIOSim implements HoodIO { private final DCMotorSim hoodSim; - private boolean turnClosedLoop = false; - private PIDController turnController = new PIDController(hoodKpSim, 0.0, hoodKdSim); - private double turnAppliedVolts = 0.0; + private boolean closedLoop = false; + private PIDController positionController = new PIDController(hoodKpSim, 0.0, hoodKdSim); + private double appliedVolts = 0.0; private double feedforwardVolts = 0.0; public HoodIOSim() { @@ -25,46 +26,43 @@ public HoodIOSim() { new DCMotorSim( LinearSystemId.createDCMotorSystem(hoodGearbox, 0.004, hoodMotorReduction), hoodGearbox); - - // Enable wrapping for turn PID - turnController.enableContinuousInput(-Math.PI, Math.PI); } @Override public void updateInputs(HoodIOInputs inputs) { // Run closed-loop control - if (turnClosedLoop) { - turnAppliedVolts = - turnController.calculate(hoodSim.getAngularPositionRad()) + feedforwardVolts; + if (closedLoop) { + appliedVolts = + positionController.calculate(hoodSim.getAngularPositionRad()) + feedforwardVolts; } else { - turnController.reset(); + positionController.reset(); } // Update simulation state - hoodSim.setInputVoltage(MathUtil.clamp(turnAppliedVolts, -12.0, 12.0)); - hoodSim.update(0.02); + 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 = turnAppliedVolts; + inputs.appliedVolts = appliedVolts; inputs.currentAmps = Math.abs(hoodSim.getCurrentDrawAmps()); } @Override public void setOpenLoop(double output) { - turnClosedLoop = false; - turnAppliedVolts = output; + closedLoop = false; + appliedVolts = output; } @Override public void setPosition(Rotation2d rotation, AngularVelocity angularVelocity) { - turnClosedLoop = true; + closedLoop = true; this.feedforwardVolts = RobotConstants.kNominalVoltage * angularVelocity.in(RadiansPerSecond) / hoodMaxAngularVelocity.in(RadiansPerSecond); - turnController.setSetpoint(rotation.getRadians()); + positionController.setSetpoint(rotation.getRadians()); } } diff --git a/src/main/java/frc/robot/subsystems/launcher/TurretIOSim.java b/src/main/java/frc/robot/subsystems/launcher/TurretIOSim.java index 62c0605..a78d6f3 100644 --- a/src/main/java/frc/robot/subsystems/launcher/TurretIOSim.java +++ b/src/main/java/frc/robot/subsystems/launcher/TurretIOSim.java @@ -9,14 +9,15 @@ 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.Robot; import frc.robot.Constants.RobotConstants; public class TurretIOSim implements TurretIO { private final DCMotorSim turnSim; - private boolean turnClosedLoop = false; - private PIDController turnController = new PIDController(turnKpSim, 0.0, turnKdSim); - private double turnAppliedVolts = 0.0; + private boolean closedLoop = false; + private PIDController positionController = new PIDController(turnKpSim, 0.0, turnKdSim); + private double appliedVolts = 0.0; private double feedforwardVolts = 0.0; public TurretIOSim() { @@ -26,44 +27,44 @@ public TurretIOSim() { turnGearbox); // Enable wrapping for turn PID - turnController.enableContinuousInput(-Math.PI, Math.PI); + positionController.enableContinuousInput(-Math.PI, Math.PI); } @Override public void updateInputs(TurretIOInputs inputs) { // Run closed-loop control - if (turnClosedLoop) { - turnAppliedVolts = - turnController.calculate(turnSim.getAngularPositionRad()) + feedforwardVolts; + if (closedLoop) { + appliedVolts = + positionController.calculate(turnSim.getAngularPositionRad()) + feedforwardVolts; } else { - turnController.reset(); + positionController.reset(); } // Update simulation state - turnSim.setInputVoltage(MathUtil.clamp(turnAppliedVolts, -12.0, 12.0)); - turnSim.update(0.02); + turnSim.setInputVoltage(MathUtil.clamp(appliedVolts, -RobotConstants.kNominalVoltage, RobotConstants.kNominalVoltage)); + turnSim.update(Robot.defaultPeriodSecs); // Update turn inputs inputs.connected = true; inputs.position = new Rotation2d(turnSim.getAngularPositionRad()); inputs.velocityRadPerSec = turnSim.getAngularVelocityRadPerSec(); - inputs.appliedVolts = turnAppliedVolts; + inputs.appliedVolts = appliedVolts; inputs.currentAmps = Math.abs(turnSim.getCurrentDrawAmps()); } @Override public void setOpenLoop(double output) { - turnClosedLoop = false; - turnAppliedVolts = output; + closedLoop = false; + appliedVolts = output; } @Override public void setPosition(Rotation2d rotation, AngularVelocity angularVelocity) { - turnClosedLoop = true; + closedLoop = true; this.feedforwardVolts = RobotConstants.kNominalVoltage * angularVelocity.in(RadiansPerSecond) / turnMaxAngularVelocity.in(RadiansPerSecond); - turnController.setSetpoint(rotation.getRadians()); + positionController.setSetpoint(rotation.getRadians()); } } From 36d9230ebd0519d945ff68d32a9a2e1291a718b6 Mon Sep 17 00:00:00 2001 From: nlaverdure Date: Fri, 30 Jan 2026 10:29:32 -0500 Subject: [PATCH 07/13] variable renaming --- .../subsystems/launcher/FlywheelIOSim.java | 36 +++++----- .../launcher/FlywheelIOTalonFX.java | 2 +- .../robot/subsystems/launcher/HoodIOSim.java | 14 ++-- .../subsystems/launcher/HoodIOSpark.java | 17 +++-- .../launcher/LauncherConstants.java | 70 +++++++++---------- .../subsystems/launcher/TurretIOSim.java | 14 ++-- .../subsystems/launcher/TurretIOSpark.java | 13 ++-- 7 files changed, 80 insertions(+), 86 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/launcher/FlywheelIOSim.java b/src/main/java/frc/robot/subsystems/launcher/FlywheelIOSim.java index 4dfedcc..c41abfc 100644 --- a/src/main/java/frc/robot/subsystems/launcher/FlywheelIOSim.java +++ b/src/main/java/frc/robot/subsystems/launcher/FlywheelIOSim.java @@ -1,9 +1,7 @@ package frc.robot.subsystems.launcher; -import static edu.wpi.first.units.Units.RadiansPerSecond; -import static frc.robot.subsystems.launcher.LauncherConstants.FlywheelConstants.flywheelGearbox; -import static frc.robot.subsystems.launcher.LauncherConstants.FlywheelConstants.flywheelKpSim; -import static frc.robot.subsystems.launcher.LauncherConstants.FlywheelConstants.flywheelMotorReduction; +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; @@ -14,48 +12,46 @@ public class FlywheelIOSim implements FlywheelIO { private final DCMotorSim flywheelSim; - private boolean flywheelClosedLoop = false; - private PIDController flywheelController = new PIDController(flywheelKpSim, 0.0, 0.0); - private double flywheelAppliedVolts = 0.0; + 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(flywheelGearbox, 0.004, flywheelMotorReduction), - flywheelGearbox); + new DCMotorSim(LinearSystemId.createDCMotorSystem(gearbox, 0.004, motorReduction), gearbox); } @Override public void updateInputs(FlywheelIOInputs inputs) { // Run closed-loop control - if (flywheelClosedLoop) { - flywheelAppliedVolts = - flywheelController.calculate(flywheelSim.getAngularPositionRad()) + feedforwardVolts; + if (closedLoop) { + appliedVolts = + velocityController.calculate(flywheelSim.getAngularPositionRad()) + feedforwardVolts; } else { - flywheelController.reset(); + velocityController.reset(); } // Update simulation state - flywheelSim.setInputVoltage(MathUtil.clamp(flywheelAppliedVolts, -12.0, 12.0)); + flywheelSim.setInputVoltage(MathUtil.clamp(appliedVolts, -12.0, 12.0)); flywheelSim.update(0.02); // Update turn inputs inputs.connected = true; inputs.velocityRadPerSec = flywheelSim.getAngularVelocityRadPerSec(); - inputs.appliedVolts = flywheelAppliedVolts; + inputs.appliedVolts = appliedVolts; inputs.currentAmps = Math.abs(flywheelSim.getCurrentDrawAmps()); } @Override public void setOpenLoop(double output) { - flywheelClosedLoop = false; - flywheelAppliedVolts = output; + closedLoop = false; + appliedVolts = output; } @Override public void setVelocity(AngularVelocity angularVelocity) { - flywheelClosedLoop = true; - flywheelController.setSetpoint(angularVelocity.in(RadiansPerSecond)); + closedLoop = true; + velocityController.setSetpoint(angularVelocity.in(RadiansPerSecond)); } } diff --git a/src/main/java/frc/robot/subsystems/launcher/FlywheelIOTalonFX.java b/src/main/java/frc/robot/subsystems/launcher/FlywheelIOTalonFX.java index a57d4e2..aaa82fa 100644 --- a/src/main/java/frc/robot/subsystems/launcher/FlywheelIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/launcher/FlywheelIOTalonFX.java @@ -52,7 +52,7 @@ public class FlywheelIOTalonFX implements FlywheelIO { private final StatusSignal flywheelCurrent; public FlywheelIOTalonFX() { - flywheelTalon = new TalonFX(flywheelPort, kCANBus); + flywheelTalon = new TalonFX(port, kCANBus); // Configuration config = new TalonFXConfiguration(); config.MotorOutput.NeutralMode = NeutralModeValue.Brake; diff --git a/src/main/java/frc/robot/subsystems/launcher/HoodIOSim.java b/src/main/java/frc/robot/subsystems/launcher/HoodIOSim.java index 81c84f1..7e672fb 100644 --- a/src/main/java/frc/robot/subsystems/launcher/HoodIOSim.java +++ b/src/main/java/frc/robot/subsystems/launcher/HoodIOSim.java @@ -9,23 +9,21 @@ 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.Robot; 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(hoodKpSim, 0.0, hoodKdSim); + 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(hoodGearbox, 0.004, hoodMotorReduction), - hoodGearbox); + new DCMotorSim(LinearSystemId.createDCMotorSystem(gearbox, 0.004, motorReduction), gearbox); } @Override @@ -39,7 +37,9 @@ public void updateInputs(HoodIOInputs inputs) { } // Update simulation state - hoodSim.setInputVoltage(MathUtil.clamp(appliedVolts, -RobotConstants.kNominalVoltage, RobotConstants.kNominalVoltage)); + hoodSim.setInputVoltage( + MathUtil.clamp( + appliedVolts, -RobotConstants.kNominalVoltage, RobotConstants.kNominalVoltage)); hoodSim.update(Robot.defaultPeriodSecs); // Update turn inputs @@ -62,7 +62,7 @@ public void setPosition(Rotation2d rotation, AngularVelocity angularVelocity) { this.feedforwardVolts = RobotConstants.kNominalVoltage * angularVelocity.in(RadiansPerSecond) - / hoodMaxAngularVelocity.in(RadiansPerSecond); + / maxAngularVelocity.in(RadiansPerSecond); positionController.setSetpoint(rotation.getRadians()); } } diff --git a/src/main/java/frc/robot/subsystems/launcher/HoodIOSpark.java b/src/main/java/frc/robot/subsystems/launcher/HoodIOSpark.java index 951f468..4e23ed4 100644 --- a/src/main/java/frc/robot/subsystems/launcher/HoodIOSpark.java +++ b/src/main/java/frc/robot/subsystems/launcher/HoodIOSpark.java @@ -2,7 +2,7 @@ import static edu.wpi.first.units.Units.RadiansPerSecond; import static frc.robot.subsystems.launcher.LauncherConstants.HoodConstants.*; -import static frc.robot.subsystems.launcher.LauncherConstants.HoodConstants.hoodPort; +import static frc.robot.subsystems.launcher.LauncherConstants.HoodConstants.port; import static frc.robot.util.SparkUtil.*; import com.revrobotics.AbsoluteEncoder; @@ -34,7 +34,7 @@ public class HoodIOSpark implements HoodIO { new Debouncer(0.5, Debouncer.DebounceType.kFalling); public HoodIOSpark() { - hoodSpark = new SparkMax(hoodPort, MotorType.kBrushless); + hoodSpark = new SparkMax(port, MotorType.kBrushless); hoodEncoder = hoodSpark.getAbsoluteEncoder(); hoodController = hoodSpark.getClosedLoopController(); @@ -49,16 +49,16 @@ public HoodIOSpark() { hoodConfig .absoluteEncoder .inverted(false) - .positionConversionFactor(hoodEncoderPositionFactor) - .velocityConversionFactor(hoodEncoderVelocityFactor) + .positionConversionFactor(encoderPositionFactor) + .velocityConversionFactor(encoderVelocityFactor) .averageDepth(2); hoodConfig .closedLoop .feedbackSensor(FeedbackSensor.kAbsoluteEncoder) .positionWrappingEnabled(true) - .positionWrappingInputRange(hoodPIDMinInput, hoodPIDMaxInput) - .pid(hoodKp, 0.0, 0.0); + .positionWrappingInputRange(minInput, maxInput) + .pid(kPReal, 0.0, 0.0); hoodConfig .signals @@ -98,12 +98,11 @@ public void setOpenLoop(double output) { @Override public void setPosition(Rotation2d rotation, AngularVelocity angularVelocity) { - double setpoint = - MathUtil.inputModulus(rotation.getRadians(), hoodPIDMinInput, hoodPIDMaxInput); + double setpoint = MathUtil.inputModulus(rotation.getRadians(), minInput, maxInput); double feedforwardVolts = RobotConstants.kNominalVoltage * angularVelocity.in(RadiansPerSecond) - / hoodMaxAngularVelocity.in(RadiansPerSecond); + / maxAngularVelocity.in(RadiansPerSecond); hoodController.setSetpoint( setpoint, ControlType.kPosition, ClosedLoopSlot.kSlot0, feedforwardVolts); } diff --git a/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java b/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java index 927f791..7a83e99 100644 --- a/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java +++ b/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java @@ -33,24 +33,24 @@ public static final class TurretConstants { public static final Rotation2d rotationOffset = new Rotation2d(0.44); // Absolute encoder - public static final double turnEncoderPositionFactor = 2 * Math.PI; // Radians - public static final double turnEncoderVelocityFactor = (2 * Math.PI) / 60.0; // Rad/sec + public static final double encoderPositionFactor = 2 * Math.PI; // Radians + public static final double encoderVelocityFactor = (2 * Math.PI) / 60.0; // Rad/sec // Position controller - public static final double turnPIDMinInput = 0.0; - public static final double turnPIDMaxInput = 2 * Math.PI; - public static final double turnKp = 0.35; + public static final double minInput = 0.0; + public static final double maxInput = 2 * Math.PI; + public static final double kPReal = 0.35; // Motor controller public static final int port = 12; - public static final double turnMotorReduction = 5.0; - public static final AngularVelocity turnMaxAngularVelocity = - NEO550Constants.kFreeSpeed.div(turnMotorReduction); + public static final double motorReduction = 5.0; + public static final AngularVelocity maxAngularVelocity = + NEO550Constants.kFreeSpeed.div(motorReduction); // Simulation - public static final DCMotor turnGearbox = DCMotor.getNeo550(1); - public static final double turnKpSim = 0.8; - public static final double turnKdSim = 0.05; + public static final DCMotor gearbox = DCMotor.getNeo550(1); + public static final double kPSim = 0.8; + public static final double kDSim = 0.05; } public static final class FlywheelConstants { @@ -59,44 +59,44 @@ public static final class FlywheelConstants { // Velocity Controller public static final Slot0Configs flywheelGains = new Slot0Configs() - .withKP(1) - .withKI(0) - .withKD(1) - .withKS(0) - .withKV(0) - .withKA(0) + .withKP(1.0) + .withKI(0.0) + .withKD(1.0) + .withKS(0.0) + .withKV(0.0) + .withKA(0.0) .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); // Motor controller - public static final int flywheelPort = 2; - public static final double flywheelMotorReduction = 1.0; - public static final AngularVelocity flywheelMaxAngularVelocity = - KrakenX60Constants.kFreeSpeed.div(flywheelMotorReduction); + public static final int port = 2; + public static final double motorReduction = 1.0; + public static final AngularVelocity maxAngularVelocity = + KrakenX60Constants.kFreeSpeed.div(motorReduction); // Simulation - public static final double flywheelKpSim = 1; - public static final DCMotor flywheelGearbox = DCMotor.getKrakenX60(1); + public static final double kPSim = 1.0; + public static final DCMotor gearbox = DCMotor.getKrakenX60(1); } public static final class HoodConstants { // Absolute encoder - public static final double hoodEncoderPositionFactor = 2 * Math.PI; // Radians - public static final double hoodEncoderVelocityFactor = (2 * Math.PI) / 60.0; // Rad/sec + public static final double encoderPositionFactor = 2 * Math.PI; // Radians + public static final double encoderVelocityFactor = (2 * Math.PI) / 60.0; // Rad/sec // Position controller - public static final double hoodPIDMinInput = 0.0; - public static final double hoodPIDMaxInput = 2 * Math.PI; - public static final double hoodKp = 0.35; + public static final double minInput = 0.0; + public static final double maxInput = 2 * Math.PI; + public static final double kPReal = 0.35; // Motor controller - public static final int hoodPort = 1; - public static final double hoodMotorReduction = 275.0; - public static final AngularVelocity hoodMaxAngularVelocity = - NEO550Constants.kFreeSpeed.div(hoodMotorReduction); + public static final int port = 1; + public static final double motorReduction = 275.0; + public static final AngularVelocity maxAngularVelocity = + NEO550Constants.kFreeSpeed.div(motorReduction); // Simulation - public static final double hoodKpSim = 1; - public static final double hoodKdSim = 0; - public static final DCMotor hoodGearbox = DCMotor.getNeo550(1); + public static final double kPSim = 1.0; + public static final double kDSim = 0.0; + public static final DCMotor gearbox = DCMotor.getNeo550(1); } } diff --git a/src/main/java/frc/robot/subsystems/launcher/TurretIOSim.java b/src/main/java/frc/robot/subsystems/launcher/TurretIOSim.java index a78d6f3..d39e36c 100644 --- a/src/main/java/frc/robot/subsystems/launcher/TurretIOSim.java +++ b/src/main/java/frc/robot/subsystems/launcher/TurretIOSim.java @@ -9,22 +9,20 @@ 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.Robot; import frc.robot.Constants.RobotConstants; +import frc.robot.Robot; public class TurretIOSim implements TurretIO { private final DCMotorSim turnSim; private boolean closedLoop = false; - private PIDController positionController = new PIDController(turnKpSim, 0.0, turnKdSim); + private PIDController positionController = new PIDController(kPSim, 0.0, kDSim); private double appliedVolts = 0.0; private double feedforwardVolts = 0.0; public TurretIOSim() { turnSim = - new DCMotorSim( - LinearSystemId.createDCMotorSystem(turnGearbox, 0.004, turnMotorReduction), - turnGearbox); + new DCMotorSim(LinearSystemId.createDCMotorSystem(gearbox, 0.004, motorReduction), gearbox); // Enable wrapping for turn PID positionController.enableContinuousInput(-Math.PI, Math.PI); @@ -41,7 +39,9 @@ public void updateInputs(TurretIOInputs inputs) { } // Update simulation state - turnSim.setInputVoltage(MathUtil.clamp(appliedVolts, -RobotConstants.kNominalVoltage, RobotConstants.kNominalVoltage)); + turnSim.setInputVoltage( + MathUtil.clamp( + appliedVolts, -RobotConstants.kNominalVoltage, RobotConstants.kNominalVoltage)); turnSim.update(Robot.defaultPeriodSecs); // Update turn inputs @@ -64,7 +64,7 @@ public void setPosition(Rotation2d rotation, AngularVelocity angularVelocity) { this.feedforwardVolts = RobotConstants.kNominalVoltage * angularVelocity.in(RadiansPerSecond) - / turnMaxAngularVelocity.in(RadiansPerSecond); + / maxAngularVelocity.in(RadiansPerSecond); positionController.setSetpoint(rotation.getRadians()); } } diff --git a/src/main/java/frc/robot/subsystems/launcher/TurretIOSpark.java b/src/main/java/frc/robot/subsystems/launcher/TurretIOSpark.java index 8d6a748..cf131ba 100644 --- a/src/main/java/frc/robot/subsystems/launcher/TurretIOSpark.java +++ b/src/main/java/frc/robot/subsystems/launcher/TurretIOSpark.java @@ -48,16 +48,16 @@ public TurretIOSpark() { turnConfig .absoluteEncoder .inverted(false) - .positionConversionFactor(turnEncoderPositionFactor) - .velocityConversionFactor(turnEncoderVelocityFactor) + .positionConversionFactor(encoderPositionFactor) + .velocityConversionFactor(encoderVelocityFactor) .averageDepth(2); turnConfig .closedLoop .feedbackSensor(FeedbackSensor.kAbsoluteEncoder) .positionWrappingEnabled(true) - .positionWrappingInputRange(turnPIDMinInput, turnPIDMaxInput) - .pid(turnKp, 0.0, 0.0); + .positionWrappingInputRange(minInput, maxInput) + .pid(kPReal, 0.0, 0.0); turnConfig .signals @@ -101,12 +101,11 @@ public void setOpenLoop(double output) { @Override public void setPosition(Rotation2d rotation, AngularVelocity angularVelocity) { double setpoint = - MathUtil.inputModulus( - rotation.plus(rotationOffset).getRadians(), turnPIDMinInput, turnPIDMaxInput); + MathUtil.inputModulus(rotation.plus(rotationOffset).getRadians(), minInput, maxInput); double feedforwardVolts = RobotConstants.kNominalVoltage * angularVelocity.in(RadiansPerSecond) - / turnMaxAngularVelocity.in(RadiansPerSecond); + / maxAngularVelocity.in(RadiansPerSecond); turnController.setSetpoint( setpoint, ControlType.kPosition, ClosedLoopSlot.kSlot0, feedforwardVolts); } From 53db71fe32216abf874df5e776e7b9b50192ec4c Mon Sep 17 00:00:00 2001 From: nlaverdure Date: Fri, 30 Jan 2026 10:32:16 -0500 Subject: [PATCH 08/13] fix flywheel velocity controller; add feedforward --- .../java/frc/robot/subsystems/launcher/FlywheelIOSim.java | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/launcher/FlywheelIOSim.java b/src/main/java/frc/robot/subsystems/launcher/FlywheelIOSim.java index c41abfc..0f74c13 100644 --- a/src/main/java/frc/robot/subsystems/launcher/FlywheelIOSim.java +++ b/src/main/java/frc/robot/subsystems/launcher/FlywheelIOSim.java @@ -8,6 +8,7 @@ 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; public class FlywheelIOSim implements FlywheelIO { private final DCMotorSim flywheelSim; @@ -27,7 +28,8 @@ public void updateInputs(FlywheelIOInputs inputs) { // Run closed-loop control if (closedLoop) { appliedVolts = - velocityController.calculate(flywheelSim.getAngularPositionRad()) + feedforwardVolts; + velocityController.calculate(flywheelSim.getAngularVelocityRadPerSec()) + + feedforwardVolts; } else { velocityController.reset(); } @@ -52,6 +54,10 @@ public void setOpenLoop(double output) { @Override public void setVelocity(AngularVelocity angularVelocity) { closedLoop = true; + this.feedforwardVolts = + RobotConstants.kNominalVoltage + * angularVelocity.in(RadiansPerSecond) + / maxAngularVelocity.in(RadiansPerSecond); velocityController.setSetpoint(angularVelocity.in(RadiansPerSecond)); } } From e06973586ad35b53f9cf1c831a8ce3217110d3d4 Mon Sep 17 00:00:00 2001 From: nlaverdure Date: Fri, 30 Jan 2026 10:36:33 -0500 Subject: [PATCH 09/13] tune flywheel kP --- .../java/frc/robot/subsystems/launcher/LauncherConstants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java b/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java index 7a83e99..782409a 100644 --- a/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java +++ b/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java @@ -74,7 +74,7 @@ public static final class FlywheelConstants { KrakenX60Constants.kFreeSpeed.div(motorReduction); // Simulation - public static final double kPSim = 1.0; + public static final double kPSim = 0.1; public static final DCMotor gearbox = DCMotor.getKrakenX60(1); } From e6fd089af3d9916108c3dfe8a0ddb13bd22bef83 Mon Sep 17 00:00:00 2001 From: nlaverdure Date: Fri, 30 Jan 2026 10:45:16 -0500 Subject: [PATCH 10/13] make hood faster --- .../java/frc/robot/subsystems/launcher/LauncherConstants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java b/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java index 782409a..8239755 100644 --- a/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java +++ b/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java @@ -90,7 +90,7 @@ public static final class HoodConstants { // Motor controller public static final int port = 1; - public static final double motorReduction = 275.0; + public static final double motorReduction = 27.5; public static final AngularVelocity maxAngularVelocity = NEO550Constants.kFreeSpeed.div(motorReduction); From ef26da4360ef1b949d272b141981949ad94f74c9 Mon Sep 17 00:00:00 2001 From: nlaverdure Date: Fri, 30 Jan 2026 10:55:51 -0500 Subject: [PATCH 11/13] use existing constants --- .../java/frc/robot/subsystems/launcher/FlywheelIOSim.java | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/launcher/FlywheelIOSim.java b/src/main/java/frc/robot/subsystems/launcher/FlywheelIOSim.java index 0f74c13..1f60aa5 100644 --- a/src/main/java/frc/robot/subsystems/launcher/FlywheelIOSim.java +++ b/src/main/java/frc/robot/subsystems/launcher/FlywheelIOSim.java @@ -9,6 +9,7 @@ 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; @@ -35,8 +36,10 @@ public void updateInputs(FlywheelIOInputs inputs) { } // Update simulation state - flywheelSim.setInputVoltage(MathUtil.clamp(appliedVolts, -12.0, 12.0)); - flywheelSim.update(0.02); + flywheelSim.setInputVoltage( + MathUtil.clamp( + appliedVolts, -RobotConstants.kNominalVoltage, RobotConstants.kNominalVoltage)); + flywheelSim.update(Robot.defaultPeriodSecs); // Update turn inputs inputs.connected = true; From 89aba7f08a85b85c9281a2cb96bddc4330689933 Mon Sep 17 00:00:00 2001 From: nlaverdure Date: Fri, 30 Jan 2026 11:22:46 -0500 Subject: [PATCH 12/13] pass target as parameter --- src/main/java/frc/robot/Robot.java | 4 +- .../robot/subsystems/launcher/Launcher.java | 39 +++++++++---------- 2 files changed, 22 insertions(+), 21 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 494b7e1..6ac1eff 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -202,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. */ diff --git a/src/main/java/frc/robot/subsystems/launcher/Launcher.java b/src/main/java/frc/robot/subsystems/launcher/Launcher.java index 9f98ea9..c00e959 100644 --- a/src/main/java/frc/robot/subsystems/launcher/Launcher.java +++ b/src/main/java/frc/robot/subsystems/launcher/Launcher.java @@ -40,7 +40,8 @@ public class Launcher extends SubsystemBase { private final Alert flywheelDisconnectedAlert; private final Alert hoodDisconnectedAlert; - private Translation3d vectorTurretBaseToHub = new Translation3d(); + private Translation3d target = new Translation3d(); + private Translation3d vectorTurretBaseToTarget = new Translation3d(); private Pose3d turretBasePose = new Pose3d(); // private Translation3d v0_nominal = new Translation3d(); @@ -82,20 +83,10 @@ public void periodic() { flywheelDisconnectedAlert.set(!flywheelInputs.connected); hoodDisconnectedAlert.set(!hoodInputs.connected); - // Get vector from static target to turret - var hubPose = GameState.getMyHubPose(); - turretBasePose = new Pose3d(chassisPoseSupplier.get()).plus(chassisToTurretBase); - vectorTurretBaseToHub = hubPose.getTranslation().minus(turretBasePose.getTranslation()); - - updateBallisticsSim(fuelNominal); - updateBallisticsSim(fuelReplanned); - updateBallisticsSim(fuelActual); - - Logger.recordOutput( - "Launcher/" + nominalKey + "/FuelTrajectory", getBallTrajectory(fuelNominal)); - Logger.recordOutput( - "Launcher/" + replannedKey + "/FuelTrajectory", getBallTrajectory(fuelReplanned)); - Logger.recordOutput("Launcher/" + actualKey + "/FuelTrajectory", getBallTrajectory(fuelActual)); + // Update and plot ball trajectories + updateBallisticsSim(fuelNominal, nominalKey); + updateBallisticsSim(fuelReplanned, replannedKey); + updateBallisticsSim(fuelActual, actualKey); } public void stop() { @@ -104,9 +95,15 @@ public void stop() { hoodIO.setOpenLoop(0.0); } - public void aimAtHub() { + public void aim(Translation3d target) { + this.target = target; + + // Get vector from static target to turret + turretBasePose = new Pose3d(chassisPoseSupplier.get()).plus(chassisToTurretBase); + vectorTurretBaseToTarget = target.minus(turretBasePose.getTranslation()); + // Set flywheel speed assuming a motionless robot - var v0_nominal = getV0(vectorTurretBaseToHub, impactAngle, nominalKey); + var v0_nominal = getV0(vectorTurretBaseToTarget, impactAngle, nominalKey); AngularVelocity flywheelSetpoint = RadiansPerSecond.of(v0_nominal.getNorm() / wheelRadius.in(Meters)); flywheelIO.setVelocity(flywheelSetpoint); @@ -123,7 +120,7 @@ public void aimAtHub() { MetersPerSecond.of(flywheelInputs.velocityRadPerSec * wheelRadius.in(Meters)); // Replan shot using actual flywheel speed - var v0_total = getV0(vectorTurretBaseToHub, flywheelSpeed, replannedKey); + var v0_total = getV0(vectorTurretBaseToTarget, flywheelSpeed, replannedKey); // Point turret to align velocity vectors var v0_flywheel = v0_total.minus(v_base); @@ -154,7 +151,7 @@ public void aimAtHub() { hoodPosition.getSin()) .times(flywheelSpeed.in(MetersPerSecond)) .plus(v_base); - log(vectorTurretBaseToHub, v0_actual, actualKey); + log(vectorTurretBaseToTarget, v0_actual, actualKey); // Spawn simulated fuel fuelSpawnTimer += Robot.defaultPeriodSecs; @@ -309,7 +306,7 @@ private static class BallisticObject { } } - private void updateBallisticsSim(ArrayList traj) { + private void updateBallisticsSim(ArrayList traj, String key) { double dt = Robot.defaultPeriodSecs; double hubZ = GameState.getMyHubPose().getZ(); @@ -324,6 +321,8 @@ private void updateBallisticsSim(ArrayList traj) { // Remove when below target height and falling return o.position.getZ() < hubZ && o.velocity.getZ() < 0; }); + + Logger.recordOutput("Launcher/" + key + "/FuelTrajectory", getBallTrajectory(traj)); } public Translation3d[] getBallTrajectory(ArrayList traj) { From 9f12f373eec2cea730f9e2e51882b696c9af788d Mon Sep 17 00:00:00 2001 From: nlaverdure Date: Fri, 30 Jan 2026 11:56:53 -0500 Subject: [PATCH 13/13] tune hood --- .../frc/robot/subsystems/launcher/LauncherConstants.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java b/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java index 8239755..54b1ac5 100644 --- a/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java +++ b/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java @@ -90,13 +90,13 @@ public static final class HoodConstants { // Motor controller public static final int port = 1; - public static final double motorReduction = 27.5; + public static final double motorReduction = 2.75; public static final AngularVelocity maxAngularVelocity = NEO550Constants.kFreeSpeed.div(motorReduction); // Simulation - public static final double kPSim = 1.0; - public static final double kDSim = 0.0; + public static final double kPSim = 0.2; + public static final double kDSim = 0.05; public static final DCMotor gearbox = DCMotor.getNeo550(1); } }