diff --git a/.claude/settings.local.json b/.claude/settings.local.json index e2fdd34..1a2be1f 100644 --- a/.claude/settings.local.json +++ b/.claude/settings.local.json @@ -30,7 +30,8 @@ "Bash(JAVA_HOME=\"/c/Users/Public/wpilib/2026/jdk\" ./gradlew compileJava:*)", "Bash(set \"JAVA_HOME=C:\\\\Users\\\\Public\\\\wpilib\\\\2026\\\\jdk\")", "Bash(\"C:\\\\Users\\\\Public\\\\wpilib\\\\2026\\\\jdk\\\\bin\\\\java\" -version)", - "Bash(git show:*)" + "Bash(git show:*)", + "Bash(set:*)" ] } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e7fb3a0..c37a927 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -276,9 +276,7 @@ private void configureBindings() { // Right trigger: enter SHOOTING state. Superstructure coordinates flywheel spin-up, // waits for adequate RPM, then runs spindexer and feeders. On release, returns to IDLE. - controlBoard - .getWantShoot() - .whileTrue(ShooterFactory.shootWithTrackingCommand(superstructure, turretManager)); + controlBoard.getWantShoot().whileTrue(ShooterFactory.setShootingCommand(superstructure)); // Zone-based actions: automatically retract hood when entering trench zones configureTrenchZoneTrigger(); diff --git a/src/main/java/frc/robot/controlboard/ControlBoard.java b/src/main/java/frc/robot/controlboard/ControlBoard.java index 6e72de8..75518cb 100644 --- a/src/main/java/frc/robot/controlboard/ControlBoard.java +++ b/src/main/java/frc/robot/controlboard/ControlBoard.java @@ -35,11 +35,6 @@ public double getRotation() { return driveControlBoard.getRotation(); } - @Override - public double getRotationY() { - return driveControlBoard.getRotationY(); - } - @Override public Trigger resetGyro() { return driveControlBoard.resetGyro(); @@ -50,11 +45,6 @@ public Trigger getWantToXWheels() { return buttonControlBoard.getWantToXWheels(); } - @Override - public Trigger getWantToAutoAlign() { - return buttonControlBoard.getWantToAutoAlign(); - } - @Override public Trigger getWantHubPathFind() { return driveControlBoard.getWantHubPathFind(); diff --git a/src/main/java/frc/robot/controlboard/GamepadButtonControlBoard.java b/src/main/java/frc/robot/controlboard/GamepadButtonControlBoard.java index dd67bd6..a03c14d 100644 --- a/src/main/java/frc/robot/controlboard/GamepadButtonControlBoard.java +++ b/src/main/java/frc/robot/controlboard/GamepadButtonControlBoard.java @@ -37,9 +37,4 @@ private GamepadButtonControlBoard() { public Trigger getWantToXWheels() { return controller.start().and(controller.back().negate()); } - - @Override - public Trigger getWantToAutoAlign() { - return controller.start(); - } } diff --git a/src/main/java/frc/robot/controlboard/GamepadDriveControlBoard.java b/src/main/java/frc/robot/controlboard/GamepadDriveControlBoard.java index ad719cf..a3ebfd2 100644 --- a/src/main/java/frc/robot/controlboard/GamepadDriveControlBoard.java +++ b/src/main/java/frc/robot/controlboard/GamepadDriveControlBoard.java @@ -45,12 +45,6 @@ public double getRotation() { * Math.signum(controller.getRightX()); } - @Override - public double getRotationY() { - return -(Math.pow(Math.abs(controller.getRightY()), 2.0)) - * Math.signum(controller.getRightY()); - } - @Override public Trigger resetGyro() { return controller.povRight().and(controller.start().negate()); diff --git a/src/main/java/frc/robot/controlboard/IButtonControlBoard.java b/src/main/java/frc/robot/controlboard/IButtonControlBoard.java index 42bbc9b..fdad33f 100644 --- a/src/main/java/frc/robot/controlboard/IButtonControlBoard.java +++ b/src/main/java/frc/robot/controlboard/IButtonControlBoard.java @@ -4,6 +4,4 @@ public interface IButtonControlBoard { Trigger getWantToXWheels(); - - Trigger getWantToAutoAlign(); } diff --git a/src/main/java/frc/robot/controlboard/IDriveControlBoard.java b/src/main/java/frc/robot/controlboard/IDriveControlBoard.java index a31e4d3..3bec127 100644 --- a/src/main/java/frc/robot/controlboard/IDriveControlBoard.java +++ b/src/main/java/frc/robot/controlboard/IDriveControlBoard.java @@ -9,8 +9,6 @@ public interface IDriveControlBoard { double getRotation(); - double getRotationY(); - Trigger resetGyro(); Trigger getWantHubPathFind(); diff --git a/src/main/java/frc/robot/factories/IntakeFactory.java b/src/main/java/frc/robot/factories/IntakeFactory.java index 97e5b64..5580e4e 100644 --- a/src/main/java/frc/robot/factories/IntakeFactory.java +++ b/src/main/java/frc/robot/factories/IntakeFactory.java @@ -28,31 +28,4 @@ public static Command setIntakingCommand(Superstructure superstructure) { () -> superstructure.setGoal(Goal.IDLE)) .withName("setIntakingCommand"); } - - /** - * Command to set the superstructure to OUTTAKING state. Runs continuously - use with - * whileTrue() bindings. Returns to IDLE when interrupted. - * - * @param superstructure The superstructure subsystem - * @return Command that maintains OUTTAKING state while running - */ - public static Command setOuttakingCommand(Superstructure superstructure) { - return superstructure - .startEnd( - () -> superstructure.setGoal(Goal.OUTTAKING), - () -> superstructure.setGoal(Goal.IDLE)) - .withName("setOuttakingCommand"); - } - - /** - * Command to set the superstructure to IDLE state immediately. - * - * @param superstructure The superstructure subsystem - * @return Instant command that sets IDLE goal - */ - public static Command setIdleCommand(Superstructure superstructure) { - return superstructure - .runOnce(() -> superstructure.setGoal(Goal.IDLE)) - .withName("setIdleCommand"); - } } diff --git a/src/main/java/frc/robot/factories/ShooterFactory.java b/src/main/java/frc/robot/factories/ShooterFactory.java index 7c55241..5f5b5c5 100644 --- a/src/main/java/frc/robot/factories/ShooterFactory.java +++ b/src/main/java/frc/robot/factories/ShooterFactory.java @@ -1,8 +1,6 @@ package frc.robot.factories; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; -import frc.robot.subsystems.shooter.TurretManager; import frc.robot.subsystems.superstructure.Superstructure; import frc.robot.subsystems.superstructure.Superstructure.Goal; @@ -28,36 +26,4 @@ public static Command setShootingCommand(Superstructure superstructure) { () -> superstructure.setGoal(Goal.IDLE)) .withName("setShootingCommand"); } - - /** - * Command to shoot while turrets track the hub. Returns to IDLE when interrupted. - * - * @param superstructure The superstructure subsystem - * @param turretManager The turret manager subsystem - * @return Command that shoots while tracking the hub - */ - public static Command shootWithTrackingCommand( - Superstructure superstructure, TurretManager turretManager) { - return superstructure - .startEnd( - () -> superstructure.setGoal(Goal.SHOOTING), - () -> superstructure.setGoal(Goal.IDLE)) - .withName("shootWithTrackingCommand"); - } - - /** - * Command to wait for turrets to aim and then shoot. Waits until turrets are aligned before - * starting to shoot. Use for autonomous sequences where precise aiming is required. - * - * @param superstructure The superstructure subsystem - * @param turretManager The turret manager subsystem - * @return Command that waits for aim then shoots - */ - public static Command aimAndShootCommand( - Superstructure superstructure, TurretManager turretManager) { - return Commands.sequence( - Commands.waitUntil(turretManager::areBothAtTarget), - setShootingCommand(superstructure)) - .withName("aimThenShootCommand"); - } } diff --git a/src/main/java/frc/robot/lib/commands/ChezyRepeatCommand.java b/src/main/java/frc/robot/lib/commands/ChezyRepeatCommand.java deleted file mode 100644 index 2717c07..0000000 --- a/src/main/java/frc/robot/lib/commands/ChezyRepeatCommand.java +++ /dev/null @@ -1,84 +0,0 @@ -package frc.robot.lib.commands; - -import static edu.wpi.first.util.ErrorMessages.requireNonNullParam; - -import edu.wpi.first.util.sendable.SendableBuilder; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.CommandScheduler; - -public class ChezyRepeatCommand extends Command { - private final Command m_command; - private boolean m_ended; - private final int kMaxLoops = 3; - - /** - * Creates a new RepeatCommand. Will run another command repeatedly, restarting it whenever it - * ends, until this command is interrupted. - * - * @param command the command to run repeatedly - */ - @SuppressWarnings("this-escape") - public ChezyRepeatCommand(Command command) { - m_command = requireNonNullParam(command, "command", "RepeatCommand"); - CommandScheduler.getInstance().registerComposedCommands(command); - addRequirements(command.getRequirements()); - setName("Repeat(" + command.getName() + ")"); - } - - @Override - public void initialize() { - m_ended = false; - m_command.initialize(); - } - - @Override - public void execute() { - int loops = 0; - while (loops < kMaxLoops) { - if (m_ended) { - m_ended = false; - m_command.initialize(); - } - m_command.execute(); - if (m_command.isFinished()) { - // restart command - m_command.end(false); - m_ended = true; - } else { - return; - } - loops++; - } - } - - @Override - public boolean isFinished() { - return false; - } - - @Override - public void end(boolean interrupted) { - // Make sure we didn't already call end() (which would happen if the command finished in the - // last call to our execute()) - if (!m_ended) { - m_command.end(interrupted); - m_ended = true; - } - } - - @Override - public boolean runsWhenDisabled() { - return m_command.runsWhenDisabled(); - } - - @Override - public InterruptionBehavior getInterruptionBehavior() { - return m_command.getInterruptionBehavior(); - } - - @Override - public void initSendable(SendableBuilder builder) { - super.initSendable(builder); - builder.addStringProperty("command", m_command::getName, null); - } -} diff --git a/src/main/java/frc/robot/lib/subsystems/CanCoderIOHardware.java b/src/main/java/frc/robot/lib/subsystems/CanCoderIOHardware.java index 8d9f615..f2e2966 100644 --- a/src/main/java/frc/robot/lib/subsystems/CanCoderIOHardware.java +++ b/src/main/java/frc/robot/lib/subsystems/CanCoderIOHardware.java @@ -8,7 +8,6 @@ import com.ctre.phoenix6.hardware.CANcoder; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; -import frc.robot.lib.util.CANStatusLogger; import frc.robot.lib.util.CTREUtil; public class CanCoderIOHardware implements CanCoderIO { @@ -32,13 +31,6 @@ public CanCoderIOHardware(CanCoderConfig config) { signals = new BaseStatusSignal[] {positionSignal, velocitySignal}; BaseStatusSignal.setUpdateFrequencyForAll(100.0, signals); - - CANStatusLogger.getInstance() - .registerCANcoder( - "CANcoder_ID" + config.CANID.getDeviceNumber(), - canCoder, - config.CANID.getDeviceNumber(), - config.CANID.getBus()); } @Override diff --git a/src/main/java/frc/robot/lib/util/CANBusStatusLogger.java b/src/main/java/frc/robot/lib/util/CANBusStatusLogger.java deleted file mode 100644 index 3bd5f3a..0000000 --- a/src/main/java/frc/robot/lib/util/CANBusStatusLogger.java +++ /dev/null @@ -1,19 +0,0 @@ -package frc.robot.lib.util; - -import com.ctre.phoenix6.CANBus; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import org.littletonrobotics.junction.Logger; - -public class CANBusStatusLogger { - private final CANBus bus; - - public CANBusStatusLogger(String name) { - bus = new CANBus(name); - } - - public void logStatus() { - var status = bus.getStatus(); - Logger.recordOutput("CANBusStatus/" + bus.getName(), status.Status); - SmartDashboard.putString("CANBusStatus/" + bus.getName(), status.Status.getName()); - } -} diff --git a/src/main/java/frc/robot/lib/util/CANStatusLogger.java b/src/main/java/frc/robot/lib/util/CANStatusLogger.java index 274af43..8e06c37 100644 --- a/src/main/java/frc/robot/lib/util/CANStatusLogger.java +++ b/src/main/java/frc/robot/lib/util/CANStatusLogger.java @@ -1,39 +1,10 @@ package frc.robot.lib.util; -import com.ctre.phoenix6.BaseStatusSignal; -import com.ctre.phoenix6.StatusCode; -import com.ctre.phoenix6.StatusSignal; -import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.TalonFX; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.lib.drivers.CANDeviceId; -import java.util.ArrayList; -import java.util.List; -import org.littletonrobotics.junction.Logger; public class CANStatusLogger { private static CANStatusLogger instance; - private final List devices = new ArrayList<>(); - - private static class DeviceStatusInfo { - private final String name; - private final TalonFX talon; - private final int deviceId; - private final String canBus; - private final StatusSignal supplyVoltage; - - public DeviceStatusInfo(String name, TalonFX talon, int deviceId, String canBus) { - this.name = name; - this.talon = talon; - this.deviceId = deviceId; - this.canBus = canBus; - this.supplyVoltage = talon.getSupplyVoltage(); - - if (this.supplyVoltage != null) { - this.supplyVoltage.setUpdateFrequency(100); - } - } - } private CANStatusLogger() {} @@ -45,59 +16,10 @@ public static synchronized CANStatusLogger getInstance() { } public void registerTalonFX(String name, TalonFX talon, int deviceId, String canBus) { - devices.add(new DeviceStatusInfo(name, talon, deviceId, canBus)); + // Registration placeholder for future CAN status monitoring } public void registerTalonFX(String name, TalonFX talon, CANDeviceId deviceId) { registerTalonFX(name, talon, deviceId.getDeviceNumber(), deviceId.getBus()); } - - public void registerCANcoder(String name, CANcoder cancoder, int deviceId, String canBus) { - // This method is kept for compatibility but doesn't need to track CANcoders - } - - private BaseStatusSignal[] signals; - - private void initializeSignalArray() { - int validSignalCount = 0; - for (DeviceStatusInfo device : devices) { - if (device.supplyVoltage != null) { - validSignalCount++; - } - } - - if (signals == null || signals.length != validSignalCount) { - signals = new BaseStatusSignal[validSignalCount]; - int index = 0; - for (DeviceStatusInfo device : devices) { - if (device.supplyVoltage != null) { - signals[index++] = device.supplyVoltage; - } - } - } - } - - public void updateCanStatus() { - if (signals == null) { - initializeSignalArray(); - } - try { - BaseStatusSignal.refreshAll(signals); - } catch (Exception e) { - } - - // Check each device status - for (int i = 0; i < devices.size(); i++) { - DeviceStatusInfo device = devices.get(i); - String deviceName = device.name + "ID" + device.deviceId; - boolean isConnected = false; - - if (device.talon != null && device.supplyVoltage != null) { - isConnected = (device.supplyVoltage.getStatus() == StatusCode.OK); - } - - Logger.recordOutput("CANstatus/" + deviceName, isConnected); - SmartDashboard.putBoolean("CAN/" + device.canBus + "/" + deviceName, isConnected); - } - } } diff --git a/src/main/java/frc/robot/lib/util/CheesyTrigger.java b/src/main/java/frc/robot/lib/util/CheesyTrigger.java deleted file mode 100644 index cb8432b..0000000 --- a/src/main/java/frc/robot/lib/util/CheesyTrigger.java +++ /dev/null @@ -1,79 +0,0 @@ -package frc.robot.lib.util; - -import static edu.wpi.first.util.ErrorMessages.requireNonNullParam; - -import edu.wpi.first.wpilibj.event.EventLoop; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.CommandScheduler; -import edu.wpi.first.wpilibj2.command.button.Trigger; -import java.util.function.BooleanSupplier; - -public class CheesyTrigger extends Trigger { - // Do this because Trigger has a bunch of private stuff. - protected final BooleanSupplier m_condition; - protected final EventLoop m_loop; - protected boolean shouldBeRunning = false; - - public CheesyTrigger(EventLoop loop, BooleanSupplier condition) { - super(loop, condition); - m_loop = requireNonNullParam(loop, "loop", "Trigger"); - m_condition = requireNonNullParam(condition, "condition", "Trigger"); - } - - public CheesyTrigger(BooleanSupplier condition) { - this(CommandScheduler.getInstance().getDefaultButtonLoop(), condition); - } - - /** Functional interface for the body of a trigger binding. */ - @FunctionalInterface - protected interface BindingBody { - /** - * Executes the body of the binding. - * - * @param previous The previous state of the condition. - * @param current The current state of the condition. - */ - void run(boolean previous, boolean current); - } - - /** - * Adds a binding to the EventLoop. - * - * @param body The body of the binding to add. - */ - protected void addBinding(CheesyTrigger.BindingBody body) { - m_loop.bind( - new Runnable() { - private boolean m_previous = m_condition.getAsBoolean(); - - @Override - public void run() { - boolean current = m_condition.getAsBoolean(); - - body.run(m_previous, current); - - m_previous = current; - } - }); - } - - // Similar to whileTrue, but always start the command (that is keep trying). - public Trigger whileTrueAlwaysRunning(Command command) { - requireNonNullParam(command, "command", "whileTrueAlwaysStart"); - addBinding( - (previous, current) -> { - if (!previous && current) { - CommandScheduler.getInstance().schedule(command); - shouldBeRunning = true; - } else if (previous && !current) { - command.cancel(); - shouldBeRunning = false; - } - if (current && shouldBeRunning && !command.isScheduled()) { - // Try again! - CommandScheduler.getInstance().schedule(command); - } - }); - return this; - } -} diff --git a/src/main/java/frc/robot/lib/util/CurrentSpikeDetector.java b/src/main/java/frc/robot/lib/util/CurrentSpikeDetector.java deleted file mode 100644 index 86bd1de..0000000 --- a/src/main/java/frc/robot/lib/util/CurrentSpikeDetector.java +++ /dev/null @@ -1,45 +0,0 @@ -package frc.robot.lib.util; - -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj2.command.button.Trigger; -import java.util.function.BooleanSupplier; - -public class CurrentSpikeDetector implements BooleanSupplier { - private double currentThresholdAmps; - private double timeThresholdSeconds; - private Timer currentOverThresholdTimer; - private Trigger cachedTrigger; - private boolean lastValue = false; - - public CurrentSpikeDetector(double currentThresholdAmps, double timeThresholdSeconds) { - this.currentThresholdAmps = currentThresholdAmps; - this.timeThresholdSeconds = timeThresholdSeconds; - this.currentOverThresholdTimer = new Timer(); - } - - public boolean update(double current) { - if (current > currentThresholdAmps) { - currentOverThresholdTimer.start(); - - boolean newValue = currentOverThresholdTimer.hasElapsed(timeThresholdSeconds); - lastValue = newValue; - return newValue; - } else { - currentOverThresholdTimer.stop(); - currentOverThresholdTimer.reset(); - lastValue = false; - return false; - } - } - - public boolean getAsBoolean() { - return lastValue; - } - - public Trigger asTrigger() { - if (cachedTrigger == null) { - cachedTrigger = new Trigger(this); - } - return cachedTrigger; - } -} diff --git a/src/main/java/frc/robot/lib/util/CustomProfiledPIDController.java b/src/main/java/frc/robot/lib/util/CustomProfiledPIDController.java deleted file mode 100644 index 90c28d3..0000000 --- a/src/main/java/frc/robot/lib/util/CustomProfiledPIDController.java +++ /dev/null @@ -1,105 +0,0 @@ -package frc.robot.lib.util; - -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.trajectory.TrapezoidProfile; - -/** A custom profiled PID controller that supports specifying an end velocity. */ -public class CustomProfiledPIDController { - private final PIDController m_controller; - private TrapezoidProfile.Constraints m_constraints; - private TrapezoidProfile m_profile; - - private TrapezoidProfile.State m_goal = new TrapezoidProfile.State(); - private TrapezoidProfile.State m_setpoint = new TrapezoidProfile.State(); - - private double m_kV = 0.0; - - public CustomProfiledPIDController( - double Kp, - double Ki, - double Kd, - TrapezoidProfile.Constraints constraints, - double period) { - m_controller = new PIDController(Kp, Ki, Kd, period); - m_constraints = constraints; - - m_profile = new TrapezoidProfile(constraints); - } - - public CustomProfiledPIDController( - double Kp, - double Ki, - double Kd, - double Kv, - TrapezoidProfile.Constraints constraints, - double period) { - this(Kp, Ki, Kd, constraints, period); - m_kV = Kv; - } - - public void setVelocityFF(double kV) { - m_kV = kV; - } - - public void setConstraints(TrapezoidProfile.Constraints constraints) { - m_constraints = constraints; - m_profile = new TrapezoidProfile(m_constraints); - } - - public TrapezoidProfile.Constraints getConstraints() { - return m_constraints; - } - - public void setGoal(double goalPosition, double goalVelocity) { - m_goal = new TrapezoidProfile.State(goalPosition, goalVelocity); - } - - public void setGoal(double goalPosition) { - setGoal(goalPosition, 0.0); - } - - public void setGoal(TrapezoidProfile.State goal) { - m_goal = goal; - } - - public TrapezoidProfile.State getGoal() { - return m_goal; - } - - public TrapezoidProfile.State getSetpoint() { - return m_setpoint; - } - - public void reset(double measuredPosition, double measuredVelocity) { - m_controller.reset(); - m_setpoint = new TrapezoidProfile.State(measuredPosition, measuredVelocity); - } - - public void reset(double measuredPosition) { - reset(measuredPosition, 0.0); - } - - public double calculate(double measuredPosition, double measuredVelocity) { - m_setpoint = m_profile.calculate(m_controller.getPeriod(), m_setpoint, m_goal); - - double positionOutput = m_controller.calculate(measuredPosition, m_setpoint.position); - - double velocityFF = m_kV * m_setpoint.velocity; - - return positionOutput + velocityFF; - } - - public double calculate( - double measuredPosition, double measuredVelocity, TrapezoidProfile.State goal) { - setGoal(goal); - return calculate(measuredPosition, measuredVelocity); - } - - public boolean atGoal(double positionTolerance) { - return Math.abs(m_goal.position - m_setpoint.position) < positionTolerance; - } - - public PIDController getPIDController() { - return m_controller; - } -} diff --git a/src/main/java/frc/robot/lib/util/ExponentialMovingAverage.java b/src/main/java/frc/robot/lib/util/ExponentialMovingAverage.java deleted file mode 100644 index a61b16f..0000000 --- a/src/main/java/frc/robot/lib/util/ExponentialMovingAverage.java +++ /dev/null @@ -1,26 +0,0 @@ -package frc.robot.lib.util; - -import java.util.OptionalDouble; - -public class ExponentialMovingAverage { - private final double alpha; - private OptionalDouble oldValue = OptionalDouble.empty(); - - public ExponentialMovingAverage(double alpha) { - this.alpha = alpha; - } - - public double calculate(double value) { - if (oldValue.isEmpty()) { - oldValue = OptionalDouble.of(value); - return value; - } - double newValue = oldValue.getAsDouble() + alpha * (value - oldValue.getAsDouble()); - oldValue = OptionalDouble.of(newValue); - return newValue; - } - - public void reset() { - oldValue = OptionalDouble.empty(); - } -} diff --git a/src/main/java/frc/robot/lib/util/ExponentialMovingAveragePose.java b/src/main/java/frc/robot/lib/util/ExponentialMovingAveragePose.java deleted file mode 100644 index 0a210a4..0000000 --- a/src/main/java/frc/robot/lib/util/ExponentialMovingAveragePose.java +++ /dev/null @@ -1,29 +0,0 @@ -package frc.robot.lib.util; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; - -public class ExponentialMovingAveragePose { - private final ExponentialMovingAverage filterX; - private final ExponentialMovingAverage filterY; - private final ExponentialMovingAverage filterTheta; - - public ExponentialMovingAveragePose(double alpha) { - filterX = new ExponentialMovingAverage(alpha); - filterY = new ExponentialMovingAverage(alpha); - filterTheta = new ExponentialMovingAverage(alpha); - } - - public void reset() { - filterX.reset(); - filterY.reset(); - filterTheta.reset(); - } - - public Pose2d calculate(Pose2d value) { - double newX = filterX.calculate(value.getX()); - double newY = filterY.calculate(value.getY()); - double newTheta = filterTheta.calculate(value.getRotation().getRadians()); - return new Pose2d(newX, newY, new Rotation2d(newTheta)); - } -}