001/************************ PROJECT ZIXI ************************/
002/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved.*/
003/* This work is licensed under the terms of the MIT license.  */
004/**************************************************************/
005package com.stuypulse.robot.util.swerve;
006
007import static org.wpilib.units.Units.MetersPerSecond;
008import static org.wpilib.units.Units.MetersPerSecondPerSecond;
009import static org.wpilib.units.Units.Seconds;
010
011import com.stuypulse.robot.constants.GlobalSettings;
012
013import org.wpilib.command3.button.CommandGamepad;
014import org.wpilib.math.filter.LinearFilter;
015import org.wpilib.math.filter.SlewRateLimiter;
016import org.wpilib.math.geometry.Translation2d;
017import org.wpilib.math.util.MathUtil;
018import org.wpilib.units.measure.LinearAcceleration;
019import org.wpilib.units.measure.LinearVelocity;
020
021/**
022 *
023 *
024 * <h2>DriveInputProcessor</h2>
025 *
026 * <p>Class for processing driver input for the drivetrain. It's intended to replace StuyLib's
027 * VStream and Filters and use pure WPILib and Java.
028 *
029 * <p>This will be used to be as close as the original StuyLib implementation as possible It will be
030 * used for
031 *
032 * <ul>
033 *   <li>Applying deadbands
034 *   <li>Applying power curves
035 *   <li>Limiting magnitude to 1
036 *   <li>Scaling to max velocity
037 *   <li>Applying rate limits
038 *   <li>Applying a low pass filter
039 * </ul>
040 *
041 * <p>To use within a command, create an instance of this class and call the {@link #update()}
042 * method in the start of the Command.
043 *
044 * <p>To get the processed speed, call the {@link #get()} method.
045 *
046 * @author Faizaan (https://github.com/Faizaan-J)
047 */
048public class DriveInputProcessor {
049    private final CommandGamepad controller;
050    private final double deadband;
051    private final double power;
052
053    /** The max velocity in meters per second/ */
054    private final LinearVelocity maxVelocity;
055
056    private final SlewRateLimiter xRateLimiter;
057    private final SlewRateLimiter yRateLimiter;
058
059    private final LinearFilter xLowPassFilter;
060    private final LinearFilter yLowPassFilter;
061
062    /** The speed vector after the full processing and filtering. */
063    private Translation2d processedSpeed;
064
065    /**
066     *
067     *
068     * <h4>Constructor for the DriveInputProcessor</h4>
069     *
070     * <p>Creates a new DriveInputProcessor with the specified parameters and processes the input
071     * periodically.
072     *
073     * @param controller The CommandXboxController to get driver input from
074     * @param deadband The deadband to apply to the input (0-1)
075     * @param power The power to apply to the input
076     * @param maxVelocity The maximum velocity to scale the input to
077     * @param maxAcceleration The maximum acceleration to apply to the input
078     * @param rc The time constant for the low pass filter (seconds)
079     */
080    public DriveInputProcessor(
081            CommandGamepad controller,
082            double deadband,
083            double power,
084            LinearVelocity maxVelocity,
085            LinearAcceleration maxAcceleration,
086            double rc) {
087        this.controller = controller;
088        this.deadband = deadband;
089        this.power = power;
090        this.maxVelocity = maxVelocity;
091
092        this.xRateLimiter = new SlewRateLimiter(maxAcceleration.in(MetersPerSecondPerSecond));
093        this.yRateLimiter = new SlewRateLimiter(maxAcceleration.in(MetersPerSecondPerSecond));
094        this.xLowPassFilter = LinearFilter.singlePoleIIR(rc, GlobalSettings.DT.in(Seconds));
095        this.yLowPassFilter = LinearFilter.singlePoleIIR(rc, GlobalSettings.DT.in(Seconds));
096
097        this.processedSpeed = Translation2d.ZERO;
098    }
099
100    /**
101     * Read the raw joystick axes and store them as a velocity vector in {@link #processedSpeed}
102     *
103     * @return This instance of the class
104     */
105    private DriveInputProcessor getDriverInputAsVelocity() {
106        this.processedSpeed = new Translation2d(-controller.getLeftY(), -controller.getLeftX());
107        return this;
108    }
109
110    /**
111     * Apply a deadband on the X and Y axes to the current {@link #processedSpeed} vector using
112     * {@link MathUtil#applyDeadband}
113     *
114     * @return This instance of the class
115     */
116    private DriveInputProcessor applyDeadband() {
117        double deadbandX = MathUtil.applyDeadband(this.processedSpeed.getX(), deadband);
118        double deadbandY = MathUtil.applyDeadband(this.processedSpeed.getY(), deadband);
119        this.processedSpeed = new Translation2d(deadbandX, deadbandY);
120        return this;
121    }
122
123    /**
124     * Limits the magnitude of the current {@link #processedSpeed} vector to 1
125     *
126     * @return This instance of the class
127     */
128    private DriveInputProcessor applyLimitMagnitudeToOne() {
129        double magnitude = this.processedSpeed.getNorm();
130
131        if (magnitude > 1.0) {
132            this.processedSpeed = this.processedSpeed.div(magnitude);
133        }
134
135        return this;
136    }
137
138    /**
139     * Apply a power curve to the current {@link #processedSpeed} vector. The vector's magnitude is
140     * raised according to the configured power while preserving direction.
141     *
142     * @return This instance of the class
143     */
144    private DriveInputProcessor applyPowerCurve() {
145        this.processedSpeed =
146                this.processedSpeed.times(Math.pow(this.processedSpeed.getNorm(), power - 1));
147        return this;
148    }
149
150    /**
151     * Scale the {@link #processedSpeed} vector to the robot's maximum velocity. Multiplies the
152     * current vector by maxVelocity (m/s).
153     *
154     * @return This instance of the class
155     */
156    private DriveInputProcessor applyScalingToMaxVelocity() {
157        this.processedSpeed = this.processedSpeed.times(maxVelocity.in(MetersPerSecond));
158        return this;
159    }
160
161    /**
162     * Applies a rate limit to {@link #processedSpeed} using a {@link SlewRateLimiter} for each
163     * axis. This limits the rate of change of the velocity vector to the max acceleration (m/s^2).
164     *
165     * @return This instance of the class
166     */
167    private DriveInputProcessor applyRateLimit() {
168        double limitedX = xRateLimiter.calculate(this.processedSpeed.getX());
169        double limitedY = yRateLimiter.calculate(this.processedSpeed.getY());
170        this.processedSpeed = new Translation2d(limitedX, limitedY);
171        return this;
172    }
173
174    /**
175     * Applies a low pass filter to {@link #processedSpeed} using a {@link LinearFilter} for each
176     * axis. This smooths out the velocity vector and reduces noise.
177     *
178     * @return This instance of the class
179     */
180    private DriveInputProcessor applyLowPassFilter() {
181        double filteredX = xLowPassFilter.calculate(this.processedSpeed.getX());
182        double filteredY = yLowPassFilter.calculate(this.processedSpeed.getY());
183        this.processedSpeed = new Translation2d(filteredX, filteredY);
184        return this;
185    }
186
187    /**
188     * Update method that processes all filters and updates the filtered speed. This should be
189     * called once per cycle in the Command using this processor, before retrieving the processed
190     * velocity vector via {@link #get()}.
191     */
192    public void update() {
193        getDriverInputAsVelocity()
194                .applyDeadband()
195                .applyPowerCurve()
196                .applyLimitMagnitudeToOne()
197                .applyScalingToMaxVelocity()
198                .applyRateLimit()
199                .applyLowPassFilter();
200    }
201
202    /**
203     * Get the processed speed.
204     *
205     * @return A Translation2d representing the processed speed
206     */
207    public Translation2d get() {
208        return processedSpeed;
209    }
210}