This base class can be used to represent single-input single-output control (SISO) algorithms 13 | * (commonly PID and feedforward). 14 | * 15 | *
For any controller, digital filters can be applied to the incoming setpoints and measurements, 16 | * or the outgoing outputs. This allows for the easy application of filters often involved with 17 | * control theory, like motion profile filters for setpoints and low-pass filters for noisy 18 | * measurements. *These filters are already provided in the StuyLib filters library.* 19 | * 20 | *
Different control schemes that share the same setpoint and measurement can also be concisely 21 | * composed together if they all implement this class. 22 | * 23 | * @author Myles Pasetsky (myles.pasetsky@gmail.com) 24 | */ 25 | public abstract class Controller { 26 | 27 | /** The most recent setpoint of the controller */ 28 | private double mSetpoint; 29 | 30 | /** The most recent measurement of the controller */ 31 | private double mMeasurement; 32 | 33 | /** The most recent output of the controller */ 34 | private double mOutput; 35 | 36 | /** The filter that is applied to the supplied setpoints */ 37 | private IFilter mSetpointFilter; 38 | 39 | /** The filter that is applied to the supplied measurements */ 40 | private IFilter mMeasurementFilter; 41 | 42 | /** The filter that is applied to the calculated outputs */ 43 | private IFilter mOutputFilter; 44 | 45 | /** Default initialization of a controller */ 46 | public Controller() { 47 | mSetpoint = 0.0; 48 | mMeasurement = 0.0; 49 | mOutput = 0.0; 50 | 51 | mSetpointFilter = x -> x; 52 | mMeasurementFilter = x -> x; 53 | mOutputFilter = x -> x; 54 | } 55 | 56 | /** 57 | * Set the setpoint filter of the controller 58 | * 59 | * @param setpointFilter setpoint filters 60 | * @return reference to the controller 61 | */ 62 | public final Controller setSetpointFilter(IFilter... setpointFilter) { 63 | mSetpointFilter = IFilter.create(setpointFilter); 64 | return this; 65 | } 66 | 67 | /** 68 | * Set the measurement filter of the controller 69 | * 70 | * @param measurementFilter measurement filters 71 | * @return reference to the controller 72 | */ 73 | public final Controller setMeasurementFilter(IFilter... measurementFilter) { 74 | mMeasurementFilter = IFilter.create(measurementFilter); 75 | return this; 76 | } 77 | 78 | /** 79 | * Set the output filter of the controller 80 | * 81 | * @param outputFilter output filters 82 | * @return reference to the controller 83 | */ 84 | public final Controller setOutputFilter(IFilter... outputFilter) { 85 | mOutputFilter = IFilter.create(outputFilter); 86 | return this; 87 | } 88 | 89 | /** @return the most recent setpoint of the controller */ 90 | public final double getSetpoint() { 91 | return mSetpoint; 92 | } 93 | 94 | /** @return the most recent measurement of the controller */ 95 | public final double getMeasurement() { 96 | return mMeasurement; 97 | } 98 | 99 | /** @return the most recent output of the controller */ 100 | public final double getOutput() { 101 | return mOutput; 102 | } 103 | 104 | /** @return the most recent error of the controller */ 105 | public final double getError() { 106 | return getSetpoint() - getMeasurement(); 107 | } 108 | 109 | /** 110 | * Determines if the controller is finished based on an acceptable error. 111 | * 112 | * @param acceptableError acceptable error for the controller 113 | * @return whether or not the controller is done 114 | */ 115 | public final boolean isDone(double acceptableError) { 116 | return Math.abs(getError()) < acceptableError; 117 | } 118 | 119 | /** 120 | * Combines this controller into a group with other controllers that share the same setpoint and 121 | * measurement. 122 | * 123 | * @param other the other controllers 124 | * @return the group of controllers that 125 | */ 126 | public final ControllerGroup add(Controller... other) { 127 | return new ControllerGroup(this, other); 128 | } 129 | 130 | /** 131 | * Updates the state of the controller. 132 | * 133 | *
Applies filters to setpoint and measurement, calculates output with filtered values, 134 | * filters and returns output 135 | * 136 | * @param setpoint setpoint of the variable being controlled 137 | * @param measurement measurement of the variable being controlled 138 | * @return the final output 139 | */ 140 | public final double update(double setpoint, double measurement) { 141 | mSetpoint = mSetpointFilter.get(setpoint); 142 | mMeasurement = mMeasurementFilter.get(measurement); 143 | 144 | return mOutput = mOutputFilter.get(calculate(mSetpoint, mMeasurement)); 145 | } 146 | 147 | /** 148 | * Calculates the output of the controller given a setpoint and measurement. 149 | * 150 | * @param setpoint setpoint 151 | * @param measurement measurement 152 | * @return calculated output 153 | */ 154 | protected abstract double calculate(double setpoint, double measurement); 155 | } 156 | -------------------------------------------------------------------------------- /src/com/stuypulse/stuylib/control/ControllerGroup.java: -------------------------------------------------------------------------------- 1 | /* Copyright (c) 2025 StuyPulse Robotics. All rights reserved. */ 2 | /* This work is licensed under the terms of the MIT license */ 3 | /* found in the root directory of this project. */ 4 | 5 | package com.stuypulse.stuylib.control; 6 | 7 | /** 8 | * Controllers can be grouped together in a "controller group" if they have the same setpoint and 9 | * measurement. 10 | * 11 | *
This allows different controller implementations that are controlling the same variable to be 12 | * concisely composed together. 13 | * 14 | * @author Myles Pasetsky (myles.pasetsky@gmail.com) 15 | */ 16 | public class ControllerGroup extends Controller { 17 | 18 | /** Controller part of the group */ 19 | private final Controller mController; 20 | 21 | /** Controllers part of the group */ 22 | private final Controller[] mControllers; 23 | 24 | /** Create a controller group */ 25 | public ControllerGroup(Controller controller, Controller... controllers) { 26 | mController = controller; 27 | mControllers = controllers; 28 | } 29 | 30 | /** 31 | * Updates the internal controllers with the setpoint and measurement and returns their combined 32 | * output. 33 | * 34 | * @param setpoint setpoint 35 | * @param measurement measurement 36 | * @return summed output of the interal controllers 37 | */ 38 | @Override 39 | protected double calculate(double setpoint, double measurement) { 40 | double output = mController.update(setpoint, measurement); 41 | 42 | for (Controller controller : mControllers) { 43 | output += controller.update(setpoint, measurement); 44 | } 45 | 46 | return output; 47 | } 48 | } 49 | -------------------------------------------------------------------------------- /src/com/stuypulse/stuylib/control/angle/AngleController.java: -------------------------------------------------------------------------------- 1 | /* Copyright (c) 2025 StuyPulse Robotics. All rights reserved. */ 2 | /* This work is licensed under the terms of the MIT license */ 3 | /* found in the root directory of this project. */ 4 | 5 | package com.stuypulse.stuylib.control.angle; 6 | 7 | import com.stuypulse.stuylib.math.Angle; 8 | import com.stuypulse.stuylib.streams.angles.filters.AFilter; 9 | import com.stuypulse.stuylib.streams.numbers.filters.IFilter; 10 | 11 | /** 12 | * Base class of controller classes of continuous systems. This means that both the setpoint and 13 | * measurement are angles, as opposed to just numbers. 14 | * 15 | *
Other than this, works the same as the Controller class. 16 | * 17 | * @see com.stuypulse.stuylib.math.Angle 18 | * @see com.stuypulse.stuylib.control.Controller 19 | * @author Myles Pasetsky (myles.pasetsky@gmail.com) 20 | */ 21 | public abstract class AngleController { 22 | 23 | /** The most recent setpoint of the controller */ 24 | private Angle mSetpoint; 25 | 26 | /** The most recent measurement of the controller */ 27 | private Angle mMeasurement; 28 | 29 | /** The most recent output of the controller */ 30 | private double mOutput; 31 | 32 | /** The filter that is applied to the supplied setpoints */ 33 | private AFilter mSetpointFilter; 34 | 35 | /** The filter that is applied to the supplied measurements */ 36 | private AFilter mMeasurementFilter; 37 | 38 | /** The filter that is applied to the calculated outputs */ 39 | private IFilter mOutputFilter; 40 | 41 | /** Default initialization of an angle controller */ 42 | public AngleController() { 43 | mSetpoint = Angle.kZero; 44 | mMeasurement = Angle.kZero; 45 | mOutput = 0.0; 46 | 47 | mSetpointFilter = x -> x; 48 | mMeasurementFilter = x -> x; 49 | mOutputFilter = x -> x; 50 | } 51 | 52 | /** 53 | * Set the setpoint filter of the controller 54 | * 55 | * @param setpointFilter angular setpoint filters 56 | * @return reference to the controller 57 | */ 58 | public final AngleController setSetpointFilter(AFilter... setpointFilter) { 59 | mSetpointFilter = AFilter.create(setpointFilter); 60 | return this; 61 | } 62 | 63 | /** 64 | * Set the measurement filter of the controller 65 | * 66 | * @param measurementFilter angular measurement filters 67 | * @return reference to the controller 68 | */ 69 | public final AngleController setMeasurementFilter(AFilter... measurementFilter) { 70 | mMeasurementFilter = AFilter.create(measurementFilter); 71 | return this; 72 | } 73 | 74 | /** 75 | * Set the output filter of the controller 76 | * 77 | * @param outputFilter output filters 78 | * @return reference to the controller 79 | */ 80 | public final AngleController setOutputFilter(IFilter... outputFilter) { 81 | mOutputFilter = IFilter.create(outputFilter); 82 | return this; 83 | } 84 | 85 | /** @return the most recent setpoint of the controller */ 86 | public final Angle getSetpoint() { 87 | return mSetpoint; 88 | } 89 | 90 | /** @return the most recent measurement of the controller */ 91 | public final Angle getMeasurement() { 92 | return mMeasurement; 93 | } 94 | 95 | /** @return the most recent output of the controller */ 96 | public final double getOutput() { 97 | return mOutput; 98 | } 99 | 100 | /** @return the most recent error of the controller */ 101 | public final Angle getError() { 102 | return getSetpoint().sub(getMeasurement()); 103 | } 104 | 105 | /** 106 | * Determines if the controller is finished based on an acceptable radian error. 107 | * 108 | * @param acceptableError acceptable error for the controller 109 | * @return whether or not the controller is done 110 | */ 111 | public final boolean isDoneRadians(double acceptableError) { 112 | return Math.abs(getError().toRadians()) < acceptableError; 113 | } 114 | 115 | /** 116 | * Determines if the controller is finished based on an acceptable degree error. 117 | * 118 | * @param acceptableError acceptable error for the controller 119 | * @return whether or not the controller is done 120 | */ 121 | public final boolean isDoneDegrees(double acceptableError) { 122 | return Math.abs(getError().toDegrees()) < acceptableError; 123 | } 124 | 125 | /** 126 | * Combines this controller into a group with other controllers that share the same setpoint and 127 | * measurement. 128 | * 129 | * @param other the other controllers 130 | * @return the group of controllers that 131 | */ 132 | public final AngleControllerGroup add(AngleController... other) { 133 | return new AngleControllerGroup(this, other); 134 | } 135 | 136 | /** 137 | * Updates the state of the controller. 138 | * 139 | *
Applies filters to setpoint and measurement, calculates output with filtered values, 140 | * filters and returns output 141 | * 142 | * @param setpoint setpoint of the variable being controlled 143 | * @param measurement measurement of the variable being controlled 144 | * @return the final output 145 | */ 146 | public final double update(Angle setpoint, Angle measurement) { 147 | mSetpoint = mSetpointFilter.get(setpoint); 148 | mMeasurement = mMeasurementFilter.get(measurement); 149 | 150 | return mOutput = mOutputFilter.get(calculate(mSetpoint, mMeasurement)); 151 | } 152 | 153 | /** 154 | * Calculates the output of the controller given a setpoint and measurement. 155 | * 156 | * @param setpoint setpoint 157 | * @param measurement measurement 158 | * @return calculated output 159 | */ 160 | protected abstract double calculate(Angle setpoint, Angle measurement); 161 | } 162 | -------------------------------------------------------------------------------- /src/com/stuypulse/stuylib/control/angle/AngleControllerGroup.java: -------------------------------------------------------------------------------- 1 | /* Copyright (c) 2025 StuyPulse Robotics. All rights reserved. */ 2 | /* This work is licensed under the terms of the MIT license */ 3 | /* found in the root directory of this project. */ 4 | 5 | package com.stuypulse.stuylib.control.angle; 6 | 7 | import com.stuypulse.stuylib.math.Angle; 8 | 9 | /** 10 | * Angle controllers can be grouped together in a "controller group" if they have the same setpoint 11 | * and measurement. 12 | * 13 | *
This allows different controller implementations that are controlling the same variable to be 14 | * concisely composed together. 15 | * 16 | * @author Myles Pasetsky (myles.pasetsky@gmail.com) 17 | */ 18 | public class AngleControllerGroup extends AngleController { 19 | 20 | /** Controller part of the group */ 21 | private final AngleController mController; 22 | 23 | /** Controllers part of the group */ 24 | private final AngleController[] mControllers; 25 | 26 | /** Create a controller group */ 27 | public AngleControllerGroup(AngleController controller, AngleController... controllers) { 28 | mController = controller; 29 | mControllers = controllers; 30 | } 31 | 32 | /** 33 | * Updates the internal controllers with the setpoint and measurement and returns their combined 34 | * output. 35 | * 36 | * @param setpoint setpoint 37 | * @param measurement measurement 38 | * @return summed output of the interal controllers 39 | */ 40 | @Override 41 | protected double calculate(Angle setpoint, Angle measurement) { 42 | double output = mController.update(setpoint, measurement); 43 | 44 | for (AngleController controller : mControllers) { 45 | output += controller.update(setpoint, measurement); 46 | } 47 | 48 | return output; 49 | } 50 | } 51 | -------------------------------------------------------------------------------- /src/com/stuypulse/stuylib/control/angle/feedback/AnglePIDController.java: -------------------------------------------------------------------------------- 1 | /* Copyright (c) 2025 StuyPulse Robotics. All rights reserved. */ 2 | /* This work is licensed under the terms of the MIT license */ 3 | /* found in the root directory of this project. */ 4 | 5 | package com.stuypulse.stuylib.control.angle.feedback; 6 | 7 | import com.stuypulse.stuylib.control.angle.AngleController; 8 | import com.stuypulse.stuylib.math.Angle; 9 | import com.stuypulse.stuylib.math.SLMath; 10 | import com.stuypulse.stuylib.network.SmartNumber; 11 | import com.stuypulse.stuylib.streams.numbers.filters.IFilter; 12 | import com.stuypulse.stuylib.streams.numbers.filters.IFilterGroup; 13 | import com.stuypulse.stuylib.util.StopWatch; 14 | 15 | /** 16 | * This PID controller is built by extending the AngleController class. It has a dynamic rate, so it 17 | * can detect how much time has passed between each update. It is made to be easy to use and simple 18 | * to understand while still being accurate. 19 | * 20 | *
The angle PID controller class is ALWAYS in units of *RADIANS* 21 | * 22 | * @author Sam (sam.belliveau@gmail.com) 23 | */ 24 | public class AnglePIDController extends AngleController { 25 | 26 | /** 27 | * Amount of time in between .update() calls that is aloud before the controller resets the 28 | * system 29 | */ 30 | private static final double kMaxTimeBeforeReset = 0.5; 31 | 32 | // Internal timer used by Controller 33 | private StopWatch mTimer; 34 | 35 | // Constants used by the PID controller 36 | private Number mP; 37 | private Number mI; 38 | private Number mD; 39 | 40 | // The Integral of the errors and filter for the I Component 41 | private double mIntegral; 42 | private IFilter mIFilter; 43 | 44 | // The Derivative of the error and the filter for the D Component 45 | private Angle mLastError; 46 | private IFilter mDFilter; 47 | 48 | /** 49 | * @param p The Proportional Multiplier 50 | * @param i The Integral Multiplier 51 | * @param d The Derivative Multiplier 52 | */ 53 | public AnglePIDController(Number p, Number i, Number d) { 54 | mTimer = new StopWatch(); 55 | 56 | mLastError = Angle.kZero; 57 | setIntegratorFilter(0, 0); 58 | setDerivativeFilter(x -> x); 59 | setPID(p, i, d); 60 | reset(); 61 | } 62 | 63 | /** Creates a blank PIDController that doesn't move */ 64 | public AnglePIDController() { 65 | this(-1, -1, -1); 66 | } 67 | 68 | /** 69 | * Resets the integrator in the PIDController. This automatically gets called if the gap between 70 | * update() commands is too large 71 | */ 72 | public void reset() { 73 | mIntegral = 0; 74 | } 75 | 76 | @Override 77 | protected double calculate(Angle setpoint, Angle measurement) { 78 | // Calculate error & time step 79 | Angle error = setpoint.sub(measurement); 80 | double dt = mTimer.reset(); 81 | 82 | // Calculate P Component 83 | double p_out = error.toRadians() * getP(); 84 | 85 | // Calculate I Component 86 | mIntegral += error.toRadians() * dt; 87 | mIntegral = mIFilter.get(mIntegral); 88 | double i_out = mIntegral * getI(); 89 | 90 | // Calculate D Component 91 | double derivative = mDFilter.get(error.velocityRadians(mLastError, dt)); 92 | mLastError = error; 93 | double d_out = derivative * getD(); 94 | 95 | // Check if time passed exceeds reset limit 96 | if (dt < kMaxTimeBeforeReset) { 97 | // Return the calculated result 98 | return p_out + i_out + d_out; 99 | } else { 100 | // If time in system is broken, then reset and return p part 101 | // This is because the P part is not relative to time 102 | reset(); 103 | return p_out; 104 | } 105 | } 106 | 107 | /** @return the P value being used by the PID controller. */ 108 | public double getP() { 109 | return Math.max(mP.doubleValue(), 0.0); 110 | } 111 | 112 | /** @return the P value being used by the PID controller. */ 113 | public double getI() { 114 | return Math.max(mI.doubleValue(), 0.0); 115 | } 116 | 117 | /** @return the P value being used by the PID controller. */ 118 | public double getD() { 119 | return Math.max(mD.doubleValue(), 0.0); 120 | } 121 | 122 | /** 123 | * @param p new p value used by the PID controller. 124 | * @return reference to PIDController (so you can chain the commands together) 125 | */ 126 | public AnglePIDController setP(Number p) { 127 | mP = SmartNumber.setNumber(mP, p); 128 | return this; 129 | } 130 | 131 | /** 132 | * @param i new i value used by the PID controller. 133 | * @return reference to PIDController (so you can chain the commands together) 134 | */ 135 | public AnglePIDController setI(Number i) { 136 | mI = SmartNumber.setNumber(mI, i); 137 | return this; 138 | } 139 | 140 | /** 141 | * @param d new d value used by the PID controller. 142 | * @return reference to PIDController (so you can chain the commands together) 143 | */ 144 | public AnglePIDController setD(Number d) { 145 | mD = SmartNumber.setNumber(mD, d); 146 | return this; 147 | } 148 | 149 | /** 150 | * @param p new p value used by the PID controller. 151 | * @param i new i value used by the PID controller. 152 | * @param d new d value used by the PID controller. 153 | * @return reference to PIDController (so you can chain the commands together) 154 | */ 155 | public AnglePIDController setPID(Number p, Number i, Number d) { 156 | return setP(p).setI(i).setD(d); 157 | } 158 | 159 | /** 160 | * @param pidValues PIDController that stores the PID values 161 | * @return reference to PIDController (so you can chain the commands together) 162 | */ 163 | public AnglePIDController setPID(AnglePIDController pidValues) { 164 | return setPID(pidValues.getP(), pidValues.getI(), pidValues.getD()); 165 | } 166 | 167 | /** 168 | * Add constraints to the integral of the PID Controller 169 | * 170 | * @param range the range of error in which the integral is allowed to accumulate (0 will 171 | * disable) 172 | * @param limit the max / min the integral is allowed to accumulate to (0 will disables) 173 | * @return reference to PIDController (so you can chain the commands together) 174 | */ 175 | public AnglePIDController setIntegratorFilter(Number range, Number limit) { 176 | mIFilter = 177 | new IFilterGroup( 178 | x -> range.doubleValue() <= 0 || isDoneRadians(range.doubleValue()) ? x : 0, 179 | x -> limit.doubleValue() <= 0 ? x : SLMath.clamp(x, limit.doubleValue())); 180 | return this; 181 | } 182 | 183 | /** 184 | * Add a filter to the error velocity / derivative of the PID controller. 185 | * 186 | * @param derivativeFilter the filter to apply to derivative 187 | * @return reference to PIDController (so you can chain the commands together) 188 | */ 189 | public AnglePIDController setDerivativeFilter(IFilter... derivativeFilter) { 190 | mDFilter = IFilter.create(derivativeFilter); 191 | return this; 192 | } 193 | } 194 | -------------------------------------------------------------------------------- /src/com/stuypulse/stuylib/control/angle/feedforward/AngleArmFeedforward.java: -------------------------------------------------------------------------------- 1 | /* Copyright (c) 2025 StuyPulse Robotics. All rights reserved. */ 2 | /* This work is licensed under the terms of the MIT license */ 3 | /* found in the root directory of this project. */ 4 | 5 | package com.stuypulse.stuylib.control.angle.feedforward; 6 | 7 | import com.stuypulse.stuylib.control.angle.AngleController; 8 | import com.stuypulse.stuylib.math.Angle; 9 | 10 | /** 11 | * A feedforward term to account for gravity for motorized arms that can move continuously (if not 12 | * use `ArmFeedforward`) 13 | * 14 | *
The motor feedforward used in the context of an arm will not account for gravity that is 15 | * acting on the arm. 16 | * 17 | *
Can be paired with MotorFeedforward or other controllers with .add 18 | * 19 | * @author Myles Pasetsky (myles.pasetsky@gmail.com) 20 | */ 21 | public class AngleArmFeedforward extends AngleController { 22 | 23 | /** voltage to hold arm horizontal */ 24 | private final Number kG; 25 | 26 | /** 27 | * Create arm feedforward 28 | * 29 | * @param kG term to hold arm vertical against gravity (volts) 30 | */ 31 | public AngleArmFeedforward(Number kG) { 32 | this.kG = kG; 33 | } 34 | 35 | /** 36 | * Calculates voltage to hold arm at the setpoint angle 37 | * 38 | * @param setpoint setpoint 39 | * @param measurement measurement 40 | * @return kG * cos(setpoint) 41 | */ 42 | @Override 43 | protected double calculate(Angle setpoint, Angle measurement) { 44 | return kG.doubleValue() * setpoint.cos(); 45 | } 46 | } 47 | -------------------------------------------------------------------------------- /src/com/stuypulse/stuylib/control/angle/feedforward/AnglePositionFeedforwardController.java: -------------------------------------------------------------------------------- 1 | /* Copyright (c) 2025 StuyPulse Robotics. All rights reserved. */ 2 | /* This work is licensed under the terms of the MIT license */ 3 | /* found in the root directory of this project. */ 4 | 5 | package com.stuypulse.stuylib.control.angle.feedforward; 6 | 7 | import com.stuypulse.stuylib.control.angle.AngleController; 8 | import com.stuypulse.stuylib.control.feedforward.MotorFeedforward; 9 | import com.stuypulse.stuylib.math.Angle; 10 | import com.stuypulse.stuylib.util.AngleVelocity; 11 | 12 | /** 13 | * A positional feedforward controller for angular systems. 14 | * 15 | * @see com.stuypulse.stuylib.control.feedforward.PositionFeedforwardController 16 | * @author Myles Pasetsky (myles.pasetsky@gmail.com) 17 | */ 18 | public class AnglePositionFeedforwardController extends AngleController { 19 | 20 | /** the feedforward model */ 21 | private final MotorFeedforward mFeedforward; 22 | 23 | /** find the derivative of angular setpoints */ 24 | private final AngleVelocity mDerivative; 25 | 26 | /** 27 | * Create an angle position feedforward controller 28 | * 29 | * @param feedforward model 30 | */ 31 | public AnglePositionFeedforwardController(MotorFeedforward feedforward) { 32 | mFeedforward = feedforward; 33 | mDerivative = new AngleVelocity(); 34 | } 35 | 36 | /** 37 | * Calculates a motor output by feeding the derivative of a positional setpoint to a feedforward 38 | * model 39 | * 40 | * @param setpoint angular positional setpoint 41 | * @param measurement angular position measurement, which is not used by the feedforward model 42 | * @return motor output from feedforward model 43 | */ 44 | @Override 45 | protected double calculate(Angle setpoint, Angle measurement) { 46 | return mFeedforward.calculate(mDerivative.get(setpoint)); 47 | } 48 | } 49 | -------------------------------------------------------------------------------- /src/com/stuypulse/stuylib/control/feedback/PIDController.java: -------------------------------------------------------------------------------- 1 | /* Copyright (c) 2025 StuyPulse Robotics. All rights reserved. */ 2 | /* This work is licensed under the terms of the MIT license */ 3 | /* found in the root directory of this project. */ 4 | 5 | package com.stuypulse.stuylib.control.feedback; 6 | 7 | import com.stuypulse.stuylib.control.Controller; 8 | import com.stuypulse.stuylib.math.SLMath; 9 | import com.stuypulse.stuylib.network.SmartNumber; 10 | import com.stuypulse.stuylib.streams.numbers.filters.IFilter; 11 | import com.stuypulse.stuylib.streams.numbers.filters.IFilterGroup; 12 | import com.stuypulse.stuylib.util.StopWatch; 13 | 14 | /** 15 | * The PID algorithm is a feedback control algorithm meant for calculating an output based on an 16 | * error between a measurement and a setpoint. 17 | * 18 | *
PID takes into account the error itself, the rate of change of error, and built-up error over 19 | * time to create a stable controller. 20 | * 21 | *
Because it's a feedback algorithm, it will only react when the system is already behind (there 22 | * must be error for there to be any output). *However, a feedback contrller can be easily combined 23 | * with a feedforward model to reduce the delay.* 24 | * 25 | *
This PID controller is built by extending the Controller class. It has a dynamic rate, so it 26 | * can detect how much time has passed between each update. It is made to be easy to use and simple 27 | * to understand while still being accurate. 28 | * 29 | * @author Sam (sam.belliveau@gmail.com) 30 | */ 31 | public class PIDController extends Controller { 32 | 33 | /** 34 | * Amount of time in between .update() calls that is aloud before the controller resets the 35 | * system 36 | */ 37 | private static final double kMaxTimeBeforeReset = 0.5; 38 | 39 | // Internal timer used by Controller 40 | private StopWatch mTimer; 41 | 42 | // Constants used by the PID controller 43 | private Number mP; 44 | private Number mI; 45 | private Number mD; 46 | 47 | // The Integral of the errors and filter for the I Component 48 | private double mIntegral; 49 | private IFilter mIFilter; 50 | 51 | // The Derivative of the error and the filter for the D Component 52 | private double mLastError; 53 | private IFilter mDFilter; 54 | 55 | /** 56 | * @param p The Proportional Multiplier 57 | * @param i The Integral Multiplier 58 | * @param d The Derivative Multiplier 59 | */ 60 | public PIDController(Number p, Number i, Number d) { 61 | mTimer = new StopWatch(); 62 | 63 | setIntegratorFilter(0, 0); 64 | setDerivativeFilter(x -> x); 65 | setPID(p, i, d); 66 | reset(); 67 | } 68 | 69 | /** Creates a blank PIDController that doesn't move */ 70 | public PIDController() { 71 | this(-1, -1, -1); 72 | } 73 | 74 | /** 75 | * Resets the integrator in the PIDController. This automatically gets called if the gap between 76 | * update() commands is too large 77 | */ 78 | public void reset() { 79 | mIntegral = 0; 80 | } 81 | 82 | @Override 83 | protected double calculate(double setpoint, double measurement) { 84 | // Calculate error & time step 85 | double error = setpoint - measurement; 86 | double dt = mTimer.reset(); 87 | 88 | // Calculate P Component 89 | double p_out = error * getP(); 90 | 91 | // Calculate I Component 92 | mIntegral += error * dt; 93 | mIntegral = mIFilter.get(mIntegral); 94 | double i_out = mIntegral * getI(); 95 | 96 | // Calculate D Component 97 | double derivative = mDFilter.get((error - mLastError) / dt); 98 | mLastError = error; 99 | double d_out = derivative * getD(); 100 | 101 | // Check if time passed exceeds reset limit 102 | if (dt < kMaxTimeBeforeReset) { 103 | // Return the calculated result 104 | return p_out + i_out + d_out; 105 | } else { 106 | // If time in system is broken, then reset and return p part 107 | // This is because the P part is not relative to time 108 | reset(); 109 | return p_out; 110 | } 111 | } 112 | 113 | /** @return the P value being used by the PID controller. */ 114 | public double getP() { 115 | return Math.max(mP.doubleValue(), 0.0); 116 | } 117 | 118 | /** @return the P value being used by the PID controller. */ 119 | public double getI() { 120 | return Math.max(mI.doubleValue(), 0.0); 121 | } 122 | 123 | /** @return the P value being used by the PID controller. */ 124 | public double getD() { 125 | return Math.max(mD.doubleValue(), 0.0); 126 | } 127 | 128 | /** 129 | * @param p new p value used by the PID controller. 130 | * @return reference to PIDController (so you can chain the commands together) 131 | */ 132 | public PIDController setP(Number p) { 133 | mP = SmartNumber.setNumber(mP, p); 134 | return this; 135 | } 136 | 137 | /** 138 | * @param i new i value used by the PID controller. 139 | * @return reference to PIDController (so you can chain the commands together) 140 | */ 141 | public PIDController setI(Number i) { 142 | mI = SmartNumber.setNumber(mI, i); 143 | return this; 144 | } 145 | 146 | /** 147 | * @param d new d value used by the PID controller. 148 | * @return reference to PIDController (so you can chain the commands together) 149 | */ 150 | public PIDController setD(Number d) { 151 | mD = SmartNumber.setNumber(mD, d); 152 | return this; 153 | } 154 | 155 | /** 156 | * @param p new p value used by the PID controller. 157 | * @param i new i value used by the PID controller. 158 | * @param d new d value used by the PID controller. 159 | * @return reference to PIDController (so you can chain the commands together) 160 | */ 161 | public PIDController setPID(Number p, Number i, Number d) { 162 | return setP(p).setI(i).setD(d); 163 | } 164 | 165 | /** 166 | * @param pidValues PIDController that stores the PID values 167 | * @return reference to PIDController (so you can chain the commands together) 168 | */ 169 | public PIDController setPID(PIDController pidValues) { 170 | return setPID(pidValues.getP(), pidValues.getI(), pidValues.getD()); 171 | } 172 | 173 | /** 174 | * Add constraints to the integral of the PID Controller 175 | * 176 | * @param range the range of error in which the integral is allowed to accumulate (0 will 177 | * disable) 178 | * @param limit the max / min the integral is allowed to accumulate to (0 will disables) 179 | * @return reference to PIDController (so you can chain the commands together) 180 | */ 181 | public PIDController setIntegratorFilter(Number range, Number limit) { 182 | mIFilter = 183 | new IFilterGroup( 184 | x -> range.doubleValue() <= 0 || isDone(range.doubleValue()) ? x : 0, 185 | x -> limit.doubleValue() <= 0 ? x : SLMath.clamp(x, limit.doubleValue())); 186 | return this; 187 | } 188 | 189 | /** 190 | * Add a filter to the error velocity / derivative of the PID controller. 191 | * 192 | * @param derivativeFilter the filter to apply to derivative 193 | * @return reference to PIDController (so you can chain the commands together) 194 | */ 195 | public PIDController setDerivativeFilter(IFilter... derivativeFilter) { 196 | mDFilter = IFilter.create(derivativeFilter); 197 | return this; 198 | } 199 | } 200 | -------------------------------------------------------------------------------- /src/com/stuypulse/stuylib/control/feedforward/ArmFeedforward.java: -------------------------------------------------------------------------------- 1 | /* Copyright (c) 2025 StuyPulse Robotics. All rights reserved. */ 2 | /* This work is licensed under the terms of the MIT license */ 3 | /* found in the root directory of this project. */ 4 | 5 | package com.stuypulse.stuylib.control.feedforward; 6 | 7 | import com.stuypulse.stuylib.control.Controller; 8 | import com.stuypulse.stuylib.streams.numbers.filters.IFilter; 9 | 10 | /** 11 | * A feedforward term to account for gravity for motorized arms. 12 | * 13 | *
The motor feedforward used in the context of an arm will not account for gravity that is 14 | * acting on the arm. 15 | * 16 | *
Can be paired with MotorFeedforward or other controllers with .add 17 | * 18 | * @author Myles Pasetsky (myles.pasetsky@gmail.com) 19 | */ 20 | public class ArmFeedforward extends Controller { 21 | 22 | /** voltage to hold arm horizontal */ 23 | private final Number kG; 24 | 25 | /** 26 | * function to configure units of cosine (or even use sin if angles are measured differently) 27 | */ 28 | private final IFilter cosine; 29 | 30 | /** 31 | * Create arm feedforward 32 | * 33 | * @param kG term to hold arm vertical against gravity (volts) 34 | */ 35 | public ArmFeedforward(Number kG) { 36 | this(kG, x -> Math.cos(Math.toRadians(x))); 37 | } 38 | 39 | /** 40 | * Create arm feedforward 41 | * 42 | * @param kG term to hold arm vertical against gravity (volts) 43 | * @param cosine function to calculate cosine of setpoint 44 | */ 45 | public ArmFeedforward(Number kG, IFilter cosine) { 46 | this.kG = kG; 47 | this.cosine = cosine; 48 | } 49 | 50 | /** 51 | * Calculates voltage to hold arm at the setpoint angle 52 | * 53 | * @param setpoint setpoint 54 | * @param measurement measurement 55 | * @return kG * cos(setpoint) 56 | */ 57 | @Override 58 | protected double calculate(double setpoint, double measurement) { 59 | return kG.doubleValue() * cosine.get(setpoint); 60 | } 61 | } 62 | -------------------------------------------------------------------------------- /src/com/stuypulse/stuylib/control/feedforward/ElevatorFeedforward.java: -------------------------------------------------------------------------------- 1 | /* Copyright (c) 2025 StuyPulse Robotics. All rights reserved. */ 2 | /* This work is licensed under the terms of the MIT license */ 3 | /* found in the root directory of this project. */ 4 | 5 | package com.stuypulse.stuylib.control.feedforward; 6 | 7 | import com.stuypulse.stuylib.control.Controller; 8 | 9 | /** 10 | * A feedforward term to account for gravity for motorized lifts. 11 | * 12 | *
The motor feedforward used in the context of a lift will not account for gravity that is 13 | * acting on the lift. 14 | * 15 | *
Can be paired with MotorFeedforward or other controllers with .add 16 | * 17 | * @author Myles Pasetsky (myles.pasetsky@gmail.com) 18 | */ 19 | public class ElevatorFeedforward extends Controller { 20 | 21 | /** voltage to hold arm in place against gravity */ 22 | private final Number kG; 23 | 24 | /** 25 | * Create ElevatorFeedforward 26 | * 27 | * @param kG voltage to hold lift 28 | */ 29 | public ElevatorFeedforward(Number kG) { 30 | this.kG = kG; 31 | } 32 | 33 | /** 34 | * Calculate voltage to hold elevator at setpoint height 35 | * 36 | * @param setpoint setpoint 37 | * @param measurement measurement 38 | * @return kG 39 | */ 40 | @Override 41 | protected double calculate(double setpoint, double measurement) { 42 | return kG.doubleValue(); 43 | } 44 | } 45 | -------------------------------------------------------------------------------- /src/com/stuypulse/stuylib/control/feedforward/MotorFeedforward.java: -------------------------------------------------------------------------------- 1 | /* Copyright (c) 2025 StuyPulse Robotics. All rights reserved. */ 2 | /* This work is licensed under the terms of the MIT license */ 3 | /* found in the root directory of this project. */ 4 | 5 | package com.stuypulse.stuylib.control.feedforward; 6 | 7 | import com.stuypulse.stuylib.control.Controller; 8 | import com.stuypulse.stuylib.control.angle.AngleController; 9 | import com.stuypulse.stuylib.control.angle.feedforward.AnglePositionFeedforwardController; 10 | import com.stuypulse.stuylib.streams.numbers.filters.Derivative; 11 | 12 | /** 13 | * A motor feedforward model, which can be used to calculate an output voltage given a desired 14 | * velocity of a motor. 15 | * 16 | *
A feedforward model often uses the dynamics of a system to calculate a voltage for a movement 17 | * that is specified by a desired velocity. 18 | * 19 | *
Feedforward models often take into account acceleration as well, but this is implicitly 20 | * calculated from desired velocities so that only sensible movements can be specified. 21 | * 22 | *
A motor feedforward *model* can be converted into *controllers* through 3 decorator methods: 23 | * `.velocity()`, `.position()`, and `.angle()`. `.velocity()` returns a velocity feedforward 24 | * controller, while `.position()` returns a position feedforward controller, and `.angle()` returns 25 | * an angle feedforward controller. These methods differ on what the controller's setpoint unit is. 26 | * 27 | *
For example, the velocity feedforward controller takes in velocity setpoints, which are 28 | * directly passed to the model. On the other hand, a position feedforward controller takes in 29 | * positional setpoints, which are then differentiated to get velocity and then fed to the 30 | * feedforward controller. An angle feedforward controller is a positional controller that is forced 31 | * to use angular units, so angular velocity is calculated and then fed to the feedforward model. 32 | * 33 | * @author Myles Pasetsky (myles.pasetsky@gmail.com) 34 | */ 35 | public class MotorFeedforward { 36 | 37 | /** The derivative filter that will handle calculating acceleration */ 38 | private final Derivative mDerivative; 39 | 40 | /** The constants for the feedforward equation */ 41 | private final Number kS; 42 | 43 | private final Number kV; 44 | private final Number kA; 45 | 46 | /** 47 | * Create a feedforward model for a motor 48 | * 49 | * @param kS volts, describes portion of voltage to overcome static friction 50 | * @param kV volts * seconds / distance, describes voltage needed to hold constant velocity 51 | * @param kA volts * seconds^2 / distance, describes voltage needed to move at an acceleration 52 | */ 53 | public MotorFeedforward(Number kS, Number kV, Number kA) { 54 | mDerivative = new Derivative(); 55 | 56 | this.kS = kS; 57 | this.kV = kV; 58 | this.kA = kA; 59 | } 60 | 61 | /** 62 | * Creates a controller that uses this feedforward model to calculate a motor output given 63 | * velocity setpoints. 64 | * 65 | * @return a velocity controller for this feedforward model 66 | */ 67 | public final Controller velocity() { 68 | return new VelocityFeedforwardController(this); 69 | } 70 | 71 | /** 72 | * Creates a controller that uses this feedforward model to calculate a motor output given 73 | * positional setpoints. 74 | * 75 | *
NOTE: The derivative of the position setpoints is calculated and then plugged into this 76 | * model. 77 | * 78 | * @return the position controller for this feedforward model 79 | */ 80 | public final Controller position() { 81 | return new PositionFeedforwardController(this); 82 | } 83 | 84 | /** 85 | * Creates a controller that uses this feedforward model to calculate a motor output given angle 86 | * setpoints. 87 | * 88 | *
NOTE: the angular velocity of the angle setpoints is calculated and then plugged into this 89 | * model. 90 | * 91 | * @return the angle controller for this feedforward model 92 | */ 93 | public final AngleController angle() { 94 | return new AnglePositionFeedforwardController(this); 95 | } 96 | 97 | /** 98 | * Calculates a motor output given a desired velocity 99 | * 100 | *
Implicitly determines acceleration by taking the derivative of velocity. 101 | * 102 | * @param velocity desired velocity 103 | * @return motor output, often in volts 104 | */ 105 | public final double calculate(double velocity) { 106 | double acceleration = mDerivative.get(velocity); 107 | 108 | return kS.doubleValue() * Math.signum(velocity) 109 | + kV.doubleValue() * velocity 110 | + kA.doubleValue() * acceleration; 111 | } 112 | } 113 | -------------------------------------------------------------------------------- /src/com/stuypulse/stuylib/control/feedforward/PositionFeedforwardController.java: -------------------------------------------------------------------------------- 1 | /* Copyright (c) 2025 StuyPulse Robotics. All rights reserved. */ 2 | /* This work is licensed under the terms of the MIT license */ 3 | /* found in the root directory of this project. */ 4 | 5 | package com.stuypulse.stuylib.control.feedforward; 6 | 7 | import com.stuypulse.stuylib.control.Controller; 8 | import com.stuypulse.stuylib.streams.numbers.filters.Derivative; 9 | 10 | /** 11 | * A positional controller that uses a feedforward model to calculate motor outputs given positional 12 | * setpoints. 13 | * 14 | *
The feedforward model takes in velocity setpoints, so the derivative of the positional 15 | * setpoints must be calculated. This is done implicitly by this controller though. 16 | * 17 | * @author Myles Pasetsky (myles.pasetsky@gmail.com) 18 | */ 19 | public class PositionFeedforwardController extends Controller { 20 | 21 | /** Filter to implicitly calculate velocity from position setpoints */ 22 | private final Derivative mVelocity; 23 | 24 | /** Internal feedforward model */ 25 | private final MotorFeedforward mFeedforward; 26 | 27 | /** 28 | * Create a position controller with a feedforward model 29 | * 30 | * @param feedforward feedforward model 31 | */ 32 | public PositionFeedforwardController(MotorFeedforward feedforward) { 33 | mVelocity = new Derivative(); 34 | mFeedforward = feedforward; 35 | } 36 | 37 | /** 38 | * Calculates a motor output by feeding the derivative of a positional setpoint to a feedforward 39 | * model 40 | * 41 | * @param setpoint positional setpoint 42 | * @param measurement position measurement, which is not used by the feedforward model 43 | * @return motor output from feedforward model 44 | */ 45 | @Override 46 | protected double calculate(double setpoint, double measurement) { 47 | double velocity = mVelocity.get(setpoint); 48 | return mFeedforward.calculate(velocity); 49 | } 50 | } 51 | -------------------------------------------------------------------------------- /src/com/stuypulse/stuylib/control/feedforward/VelocityFeedforwardController.java: -------------------------------------------------------------------------------- 1 | /* Copyright (c) 2025 StuyPulse Robotics. All rights reserved. */ 2 | /* This work is licensed under the terms of the MIT license */ 3 | /* found in the root directory of this project. */ 4 | 5 | package com.stuypulse.stuylib.control.feedforward; 6 | 7 | import com.stuypulse.stuylib.control.Controller; 8 | 9 | /** 10 | * A velocity controller that uses a feedforward model to calculate motor outputs given velocity 11 | * setpoints. 12 | * 13 | * @author Myles Pasetsky (myles.pasetsky@gmail.com) 14 | */ 15 | public class VelocityFeedforwardController extends Controller { 16 | 17 | /** Internal feedforward model */ 18 | private final MotorFeedforward mFeedforward; 19 | 20 | /** 21 | * Create a velocity controller with a feedforward model 22 | * 23 | * @param feedforward the model 24 | */ 25 | public VelocityFeedforwardController(MotorFeedforward feedforward) { 26 | mFeedforward = feedforward; 27 | } 28 | 29 | /** 30 | * Calculates a motor output by feeding a velocity setpoint to a feedforward model. 31 | * 32 | * @param setpoint velocity setpoint of the controller 33 | * @param measurement measurement, which is not used in calculating an output 34 | * @return motor output from feedforward model 35 | */ 36 | @Override 37 | protected double calculate(double setpoint, double measurement) { 38 | return mFeedforward.calculate(setpoint); 39 | } 40 | } 41 | -------------------------------------------------------------------------------- /src/com/stuypulse/stuylib/control/readme.md: -------------------------------------------------------------------------------- 1 | # StuyLib Control Library 2 | 3 | A rudimentary control theory library designed to create controllers using feedback, feedforward, and signal processing algorithms. 4 | -------------------------------------------------------------------------------- /src/com/stuypulse/stuylib/input/WPIGamepad.java: -------------------------------------------------------------------------------- 1 | /* Copyright (c) 2025 StuyPulse Robotics. All rights reserved. */ 2 | /* This work is licensed under the terms of the MIT license */ 3 | /* found in the root directory of this project. */ 4 | 5 | package com.stuypulse.stuylib.input; 6 | 7 | import edu.wpi.first.wpilibj.Joystick; 8 | import edu.wpi.first.wpilibj2.command.button.Trigger; 9 | 10 | /** 11 | * WPI Gamepad extends Gamepad and adds functions that makes interacting with the underlying 12 | * Joystick class easy. The WPI joystick class is kept in a separate class to make it easy to update 13 | * if WPI updates. 14 | * 15 | *
If you would like to get a button id that is not defined, or an axis id that is not defined, 16 | * use {@link #getRawButton(int)} or {@link #getRawAxis(int)}. If you want it to return a button for 17 | * an unimplemented button, type {@link #getButton(int)}. 18 | * 19 | *
To initialize this class, pass in a Joystick or an int set to the port number. This will be 20 | * the Joystick that the Gamepad class will interact with. 21 | * 22 | *
If you do not initialize with a Joystick, everything will still work except for - {@link 23 | * #getRawButton(int)} - {@link #getRawAxis(int)} - {@link #getButton(int)} which will return either 24 | * false or 0.0 25 | * 26 | *
The difference between the implementations of the Gamepad class is how it interacts with the 27 | * underlying Joystick class. 28 | * 29 | * @author Sam (sam.belliveau@gmail.com) 30 | */ 31 | public class WPIGamepad extends Gamepad { 32 | 33 | /** Underlying Joystick Class */ 34 | private Joystick mJoy; 35 | 36 | /*******************/ 37 | /*** CONSTRUCTOR ***/ 38 | /*******************/ 39 | 40 | /** @param joystick WPI Joystick that will be stored in this class */ 41 | public WPIGamepad(Joystick joystick) { 42 | this.mJoy = joystick; 43 | } 44 | 45 | /** @param port The port that the gamepad is plugged into */ 46 | public WPIGamepad(int port) { 47 | this(new Joystick(port)); 48 | } 49 | 50 | /**********************/ 51 | /*** JOYSTICK STUFF ***/ 52 | /**********************/ 53 | 54 | @Override 55 | public String getGamepadName() { 56 | return "WPIGamepad"; 57 | } 58 | 59 | /** @return if Gamepad has a Joystick */ 60 | public final boolean hasJoystick() { 61 | return getJoystick() != null; 62 | } 63 | 64 | /** @return Underlying joystick */ 65 | public final Joystick getJoystick() { 66 | return this.mJoy; 67 | } 68 | 69 | /** 70 | * @param button Joystick button id 71 | * @return The value of the button 72 | */ 73 | public final boolean getRawButton(int button) { 74 | if (!hasJoystick()) { 75 | return false; 76 | } 77 | return getJoystick().getRawButton(button); 78 | } 79 | 80 | /** 81 | * @param button Joystick button id 82 | * @return Trigger that activates with {@link #getRawButton(int)} 83 | */ 84 | public final Trigger getButton(int button) { 85 | return new Trigger(() -> getRawButton(button)); 86 | } 87 | 88 | /** 89 | * @param axis Joystick axis id 90 | * @return The value of the axis 91 | */ 92 | public final double getRawAxis(int axis) { 93 | if (!hasJoystick()) { 94 | return 0.0; 95 | } 96 | return getJoystick().getRawAxis(axis); 97 | } 98 | 99 | /** @param intensity amount to make the gamepad rumble */ 100 | public final void setRumble(double intensity) { 101 | getJoystick().setRumble(Joystick.RumbleType.kLeftRumble, intensity); 102 | getJoystick().setRumble(Joystick.RumbleType.kRightRumble, intensity); 103 | } 104 | } 105 | -------------------------------------------------------------------------------- /src/com/stuypulse/stuylib/input/gamepads/AutoGamepad.java: -------------------------------------------------------------------------------- 1 | /* Copyright (c) 2025 StuyPulse Robotics. All rights reserved. */ 2 | /* This work is licensed under the terms of the MIT license */ 3 | /* found in the root directory of this project. */ 4 | 5 | package com.stuypulse.stuylib.input.gamepads; 6 | 7 | import com.stuypulse.stuylib.input.Gamepad; 8 | import com.stuypulse.stuylib.math.SLMath; 9 | import com.stuypulse.stuylib.util.StopWatch; 10 | 11 | import edu.wpi.first.wpilibj.GenericHID.HIDType; 12 | import edu.wpi.first.wpilibj.Joystick; 13 | 14 | /** 15 | * This class was created as a saftey measure to prevent issues that arise when the wrong gamepad is 16 | * plugged in. After the robot smashed into a wall because we plugged in a PS4 controller when the 17 | * code expected a Logitech controller, it was deemed that we needed this class. This class will 18 | * ONLY RETURN SAFE VALUES, it will never return negatives or unusual values. This cannot be said 19 | * about other gamepad classes. 20 | * 21 | *
The type of controllers supported are {@link Logitech.DMode}, {@link Logitech.XMode}, {@link 22 | * PS4}, and {@link Xbox}. If none of these gamepads are detected, the code will default to a 23 | * gamepad that does not do anything. 24 | * 25 | * @author Sam (sam.belliveau@gmail.com) 26 | */ 27 | public final class AutoGamepad extends Gamepad { 28 | 29 | // The amount of time that the gamepad will go before checking the type 30 | private static final double MIN_CONTROLLER_CHECK = 4; 31 | 32 | // All of the Different Gamepad classes that AutoGamepad Supports 33 | // Logitech XMode is not here as it is the same as an Xbox Controller 34 | private final int port; 35 | private final Joystick mJoystick; 36 | private final Gamepad mLogitech; 37 | private final Gamepad mPS4; 38 | private final Gamepad mXbox; 39 | private final Gamepad mNull; 40 | 41 | // The gamepad that will be used, and Stopwatch to track updating it 42 | private StopWatch mTimer; 43 | private Gamepad mCurrent; 44 | 45 | /** @param port the port that the gamepad should read from */ 46 | public AutoGamepad(int port) { 47 | this.port = port; 48 | mJoystick = new Joystick(this.port); 49 | mLogitech = new Logitech.DMode(mJoystick); 50 | mPS4 = new PS4(mJoystick); 51 | mXbox = new Xbox(port); 52 | mNull = new Gamepad(); 53 | 54 | mTimer = new StopWatch(); 55 | mCurrent = mNull; 56 | } 57 | 58 | /** @return the correct type of gamepad coming from driverstation directly */ 59 | private Gamepad forceDetectGamepad() { 60 | // Check if joystick is connected 61 | if (!mJoystick.isConnected()) return mNull; 62 | 63 | // Get Type of Joystick 64 | HIDType type = mJoystick.getType(); 65 | 66 | // Return the right gamepad based on the gamepad type 67 | if (type == null) return mNull; 68 | else { 69 | switch (type) { 70 | case kXInputGamepad: 71 | return mXbox; 72 | case kHIDJoystick: 73 | return mLogitech; 74 | case kHIDGamepad: 75 | return mPS4; 76 | default: 77 | return mNull; 78 | } 79 | } 80 | } 81 | 82 | /** @return the internal gamepad class that this gamepad will be reading from */ 83 | public Gamepad getDetectedGamepad() { 84 | if (MIN_CONTROLLER_CHECK < mTimer.getTime()) { 85 | mTimer.reset(); 86 | mCurrent = forceDetectGamepad(); 87 | } 88 | 89 | return mCurrent; 90 | } 91 | 92 | /***************************/ 93 | /*** Gamepad Passthrough ***/ 94 | /***************************/ 95 | 96 | // Name // 97 | @Override 98 | public String getGamepadName() { 99 | return "AutoGamepad (" + getDetectedGamepad().getGamepadName() + ")"; 100 | } 101 | 102 | // Left Stick // 103 | public double getLeftX() { 104 | return SLMath.clamp(getDetectedGamepad().getLeftX(), -1.0, 1.0); 105 | } 106 | 107 | public double getLeftY() { 108 | return SLMath.clamp(getDetectedGamepad().getLeftY(), -1.0, 1.0); 109 | } 110 | 111 | // Right Stick // 112 | public double getRightX() { 113 | return SLMath.clamp(getDetectedGamepad().getRightX(), -1.0, 1.0); 114 | } 115 | 116 | public double getRightY() { 117 | return SLMath.clamp(getDetectedGamepad().getRightY(), -1.0, 1.0); 118 | } 119 | 120 | // D-Pad // 121 | public boolean getRawDPadUp() { 122 | return getDetectedGamepad().getRawDPadUp(); 123 | } 124 | 125 | public boolean getRawDPadDown() { 126 | return getDetectedGamepad().getRawDPadDown(); 127 | } 128 | 129 | public boolean getRawDPadLeft() { 130 | return getDetectedGamepad().getRawDPadLeft(); 131 | } 132 | 133 | public boolean getRawDPadRight() { 134 | return getDetectedGamepad().getRawDPadRight(); 135 | } 136 | 137 | // Bumpers // 138 | public boolean getRawLeftBumper() { 139 | return getDetectedGamepad().getRawLeftBumper(); 140 | } 141 | 142 | public boolean getRawRightBumper() { 143 | return getDetectedGamepad().getRawRightBumper(); 144 | } 145 | 146 | // Triggers // 147 | public double getLeftTrigger() { 148 | return SLMath.clamp(getDetectedGamepad().getLeftTrigger(), 0.0, 1.0); 149 | } 150 | 151 | public double getRightTrigger() { 152 | return SLMath.clamp(getDetectedGamepad().getRightTrigger(), 0.0, 1.0); 153 | } 154 | 155 | // Face Buttons // 156 | public boolean getRawLeftButton() { 157 | return getDetectedGamepad().getRawLeftButton(); 158 | } 159 | 160 | public boolean getRawBottomButton() { 161 | return getDetectedGamepad().getRawBottomButton(); 162 | } 163 | 164 | public boolean getRawRightButton() { 165 | return getDetectedGamepad().getRawRightButton(); 166 | } 167 | 168 | public boolean getRawTopButton() { 169 | return getDetectedGamepad().getRawTopButton(); 170 | } 171 | 172 | // Left Menu / Right Menu // 173 | public boolean getRawLeftMenuButton() { 174 | return getDetectedGamepad().getRawLeftMenuButton(); 175 | } 176 | 177 | public boolean getRawRightMenuButton() { 178 | return getDetectedGamepad().getRawRightMenuButton(); 179 | } 180 | 181 | // Analog Stick Buttons // 182 | public boolean getRawLeftStickButton() { 183 | return getDetectedGamepad().getRawLeftStickButton(); 184 | } 185 | 186 | public boolean getRawRightStickButton() { 187 | return getDetectedGamepad().getRawRightStickButton(); 188 | } 189 | 190 | // Rumble // 191 | public void setRumble(double intensity) { 192 | getDetectedGamepad().setRumble(intensity); 193 | } 194 | } 195 | -------------------------------------------------------------------------------- /src/com/stuypulse/stuylib/input/gamepads/Logitech.java: -------------------------------------------------------------------------------- 1 | /* Copyright (c) 2025 StuyPulse Robotics. All rights reserved. */ 2 | /* This work is licensed under the terms of the MIT license */ 3 | /* found in the root directory of this project. */ 4 | 5 | package com.stuypulse.stuylib.input.gamepads; 6 | 7 | import com.stuypulse.stuylib.input.WPIGamepad; 8 | 9 | import edu.wpi.first.wpilibj.Joystick; 10 | import edu.wpi.first.wpilibj.XboxController; 11 | 12 | /** 13 | * Implementation of Logitech Controller and its 2 Modes for the Gamepad Class 14 | * 15 | * @author Sam (sam.belliveau@gmail.com) 16 | */ 17 | public class Logitech extends WPIGamepad { 18 | 19 | // Constructor // 20 | protected Logitech(int port) { 21 | super(port); 22 | } 23 | 24 | protected Logitech(Joystick joystick) { 25 | super(joystick); 26 | } 27 | 28 | // D-Pad // 29 | @Override 30 | public boolean getRawDPadUp() { 31 | return getJoystick().getPOV() == 0; 32 | } 33 | 34 | @Override 35 | public boolean getRawDPadDown() { 36 | return getJoystick().getPOV() == 180; 37 | } 38 | 39 | @Override 40 | public boolean getRawDPadLeft() { 41 | return getJoystick().getPOV() == 270; 42 | } 43 | 44 | @Override 45 | public boolean getRawDPadRight() { 46 | return getJoystick().getPOV() == 90; 47 | } 48 | 49 | /** 50 | * There is a switch on the back of the controller this is for when the switch is in the D 51 | * position 52 | */ 53 | public static class DMode extends Logitech { 54 | 55 | // Constructor // 56 | public DMode(int port) { 57 | super(port); 58 | } 59 | 60 | public DMode(Joystick joystick) { 61 | super(joystick); 62 | } 63 | 64 | // Name // 65 | @Override 66 | public String getGamepadName() { 67 | return "Logitech DMode"; 68 | } 69 | 70 | // Left Stick // 71 | @Override 72 | public double getLeftX() { 73 | return getRawAxis(0); 74 | } 75 | 76 | @Override 77 | public double getLeftY() { 78 | return -getRawAxis(1); 79 | } 80 | 81 | // Right Stick // 82 | @Override 83 | public double getRightX() { 84 | return getRawAxis(2); 85 | } 86 | 87 | @Override 88 | public double getRightY() { 89 | return -getRawAxis(3); 90 | } 91 | 92 | // Bumpers // 93 | @Override 94 | public boolean getRawLeftBumper() { 95 | return getRawButton(5); 96 | } 97 | 98 | @Override 99 | public boolean getRawRightBumper() { 100 | return getRawButton(6); 101 | } 102 | 103 | // Triggers // 104 | @Override 105 | public double getLeftTrigger() { 106 | return getRawButton(7) ? 1.0 : 0.0; 107 | } 108 | 109 | @Override 110 | public double getRightTrigger() { 111 | return getRawButton(8) ? 1.0 : 0.0; 112 | } 113 | 114 | // Face Buttons // 115 | @Override 116 | public boolean getRawLeftButton() { 117 | return getRawButton(1); 118 | } 119 | 120 | @Override 121 | public boolean getRawBottomButton() { 122 | return getRawButton(2); 123 | } 124 | 125 | @Override 126 | public boolean getRawRightButton() { 127 | return getRawButton(3); 128 | } 129 | 130 | @Override 131 | public boolean getRawTopButton() { 132 | return getRawButton(4); 133 | } 134 | 135 | // Left Menu / Right Menu // 136 | @Override 137 | public boolean getRawLeftMenuButton() { 138 | return getRawButton(9); 139 | } 140 | 141 | @Override 142 | public boolean getRawRightMenuButton() { 143 | return getRawButton(10); 144 | } 145 | 146 | // Analog Stick Buttons // 147 | @Override 148 | public boolean getRawLeftStickButton() { 149 | return getRawButton(11); 150 | } 151 | 152 | @Override 153 | public boolean getRawRightStickButton() { 154 | return getRawButton(12); 155 | } 156 | } 157 | 158 | /** 159 | * There is a switch on the back of the controller this is for when the switch is in the X 160 | * position. 161 | * 162 | *
Because of the use of XInput, this is equivilant to using {@link Xbox}. However if you are 163 | * using a logitech controller on XMode, use this class for readability. 164 | */ 165 | public static class XMode extends Xbox { 166 | 167 | // Constructor // 168 | public XMode(int port) { 169 | super(port); 170 | } 171 | 172 | public XMode(XboxController joystick) { 173 | super(joystick); 174 | } 175 | 176 | // Name // 177 | @Override 178 | public String getGamepadName() { 179 | return "Logitech XMode"; 180 | } 181 | } 182 | } 183 | -------------------------------------------------------------------------------- /src/com/stuypulse/stuylib/input/gamepads/PS4.java: -------------------------------------------------------------------------------- 1 | /* Copyright (c) 2025 StuyPulse Robotics. All rights reserved. */ 2 | /* This work is licensed under the terms of the MIT license */ 3 | /* found in the root directory of this project. */ 4 | 5 | package com.stuypulse.stuylib.input.gamepads; 6 | 7 | import com.stuypulse.stuylib.input.WPIGamepad; 8 | 9 | import edu.wpi.first.wpilibj.Joystick; 10 | 11 | /** 12 | * Implementation of the PS4 for the {@link com.stuypulse.stuylib.input.Gamepad} Class 13 | * 14 | * @author Sam (sam.belliveau@gmail.com) 15 | */ 16 | public class PS4 extends WPIGamepad { 17 | 18 | // Constructor // 19 | public PS4(int port) { 20 | super(port); 21 | } 22 | 23 | public PS4(Joystick joystick) { 24 | super(joystick); 25 | } 26 | 27 | // Name // 28 | @Override 29 | public String getGamepadName() { 30 | return "Playstation Controller"; 31 | } 32 | 33 | // Left Stick // 34 | @Override 35 | public double getLeftX() { 36 | return getRawAxis(0); 37 | } 38 | 39 | @Override 40 | public double getLeftY() { 41 | return -getRawAxis(1); 42 | } 43 | 44 | // Right Stick // 45 | @Override 46 | public double getRightX() { 47 | return getRawAxis(2); 48 | } 49 | 50 | @Override 51 | public double getRightY() { 52 | return -getRawAxis(5); 53 | } 54 | 55 | // D-Pad // 56 | @Override 57 | public boolean getRawDPadUp() { 58 | return getJoystick().getPOV() == 0; 59 | } 60 | 61 | @Override 62 | public boolean getRawDPadDown() { 63 | return getJoystick().getPOV() == 180; 64 | } 65 | 66 | @Override 67 | public boolean getRawDPadLeft() { 68 | return getJoystick().getPOV() == 270; 69 | } 70 | 71 | @Override 72 | public boolean getRawDPadRight() { 73 | return getJoystick().getPOV() == 90; 74 | } 75 | 76 | // Bumpers // 77 | @Override 78 | public boolean getRawLeftBumper() { 79 | return getRawButton(5); 80 | } 81 | 82 | @Override 83 | public boolean getRawRightBumper() { 84 | return getRawButton(6); 85 | } 86 | 87 | // Triggers // 88 | @Override 89 | public double getLeftTrigger() { 90 | return (getRawAxis(3) + 1.0) / 2.0; 91 | } 92 | 93 | @Override 94 | public double getRightTrigger() { 95 | return (getRawAxis(4) + 1.0) / 2.0; 96 | } 97 | 98 | // Face Buttons // 99 | @Override 100 | public boolean getRawLeftButton() { 101 | return getRawButton(1); 102 | } 103 | 104 | @Override 105 | public boolean getRawBottomButton() { 106 | return getRawButton(2); 107 | } 108 | 109 | @Override 110 | public boolean getRawRightButton() { 111 | return getRawButton(3); 112 | } 113 | 114 | @Override 115 | public boolean getRawTopButton() { 116 | return getRawButton(4); 117 | } 118 | 119 | // Left Menu / Right Menu // 120 | @Override 121 | public boolean getRawLeftMenuButton() { 122 | return getRawButton(9); 123 | } 124 | 125 | @Override 126 | public boolean getRawRightMenuButton() { 127 | return getRawButton(10); 128 | } 129 | 130 | // Analog Stick Buttons // 131 | @Override 132 | public boolean getRawLeftStickButton() { 133 | return getRawButton(11); 134 | } 135 | 136 | @Override 137 | public boolean getRawRightStickButton() { 138 | return getRawButton(12); 139 | } 140 | } 141 | -------------------------------------------------------------------------------- /src/com/stuypulse/stuylib/input/gamepads/Xbox.java: -------------------------------------------------------------------------------- 1 | /* Copyright (c) 2025 StuyPulse Robotics. All rights reserved. */ 2 | /* This work is licensed under the terms of the MIT license */ 3 | /* found in the root directory of this project. */ 4 | 5 | package com.stuypulse.stuylib.input.gamepads; 6 | 7 | import com.stuypulse.stuylib.input.Gamepad; 8 | 9 | import edu.wpi.first.wpilibj.GenericHID.RumbleType; 10 | import edu.wpi.first.wpilibj.XboxController; 11 | 12 | /** 13 | * A wrapper for the XboxController class to work with the {@link Gamepad} interface. 14 | * 15 | * @author Sam (sam.belliveau@gmail.com) 16 | */ 17 | public class Xbox extends Gamepad { 18 | 19 | private XboxController mJoystick; 20 | 21 | private final boolean flipped; 22 | 23 | // Constructor // 24 | public Xbox(XboxController joystick) { 25 | this(joystick, true); 26 | } 27 | 28 | protected Xbox(XboxController joystick, boolean flipped) { 29 | mJoystick = joystick; 30 | 31 | this.flipped = flipped; 32 | } 33 | 34 | public Xbox(int port) { 35 | this(new XboxController(port)); 36 | } 37 | 38 | // Get the underlying XboxClass 39 | public XboxController getJoystick() { 40 | return mJoystick; 41 | } 42 | 43 | // Name // 44 | @Override 45 | public String getGamepadName() { 46 | return "Xbox Controller"; 47 | } 48 | 49 | // Left Stick // 50 | @Override 51 | public double getLeftX() { 52 | return getJoystick().getLeftX(); 53 | } 54 | 55 | @Override 56 | public double getLeftY() { 57 | return (flipped ? -1 : +1) * getJoystick().getLeftY(); 58 | } 59 | 60 | // Right Stick // 61 | @Override 62 | public double getRightX() { 63 | return getJoystick().getRightX(); 64 | } 65 | 66 | @Override 67 | public double getRightY() { 68 | return (flipped ? -1 : +1) * getJoystick().getRightY(); 69 | } 70 | 71 | // D-Pad // 72 | @Override 73 | public boolean getRawDPadUp() { 74 | return getJoystick().getPOV() == 0; 75 | } 76 | 77 | @Override 78 | public boolean getRawDPadDown() { 79 | return getJoystick().getPOV() == 180; 80 | } 81 | 82 | @Override 83 | public boolean getRawDPadLeft() { 84 | return getJoystick().getPOV() == 270; 85 | } 86 | 87 | @Override 88 | public boolean getRawDPadRight() { 89 | return getJoystick().getPOV() == 90; 90 | } 91 | 92 | // Bumpers // 93 | @Override 94 | public boolean getRawLeftBumper() { 95 | return getJoystick().getLeftBumper(); 96 | } 97 | 98 | @Override 99 | public boolean getRawRightBumper() { 100 | return getJoystick().getRightBumper(); 101 | } 102 | 103 | // Triggers // 104 | @Override 105 | public double getLeftTrigger() { 106 | return getJoystick().getLeftTriggerAxis(); 107 | } 108 | 109 | @Override 110 | public double getRightTrigger() { 111 | return getJoystick().getRightTriggerAxis(); 112 | } 113 | 114 | // Face Buttons // 115 | @Override 116 | public boolean getRawLeftButton() { 117 | return getJoystick().getXButton(); 118 | } 119 | 120 | @Override 121 | public boolean getRawBottomButton() { 122 | return getJoystick().getAButton(); 123 | } 124 | 125 | @Override 126 | public boolean getRawRightButton() { 127 | return getJoystick().getBButton(); 128 | } 129 | 130 | @Override 131 | public boolean getRawTopButton() { 132 | return getJoystick().getYButton(); 133 | } 134 | 135 | // Left Menu / Right Menu // 136 | @Override 137 | public boolean getRawLeftMenuButton() { 138 | return getJoystick().getBackButton(); 139 | } 140 | 141 | @Override 142 | public boolean getRawRightMenuButton() { 143 | return getJoystick().getStartButton(); 144 | } 145 | 146 | // Analog Stick Buttons // 147 | @Override 148 | public boolean getRawLeftStickButton() { 149 | return getJoystick().getLeftStickButton(); 150 | } 151 | 152 | @Override 153 | public boolean getRawRightStickButton() { 154 | return getJoystick().getRightStickButton(); 155 | } 156 | 157 | // Rumble 158 | @Override 159 | public void setRumble(double intensity) { 160 | mJoystick.setRumble(RumbleType.kLeftRumble, intensity); 161 | mJoystick.setRumble(RumbleType.kRightRumble, intensity); 162 | } 163 | 164 | public Xbox flipped() { 165 | return new Xbox(mJoystick, !flipped); 166 | } 167 | } 168 | -------------------------------------------------------------------------------- /src/com/stuypulse/stuylib/input/gamepads/readme.md: -------------------------------------------------------------------------------- 1 | # StuyLib Gamepad Implementations 2 | 3 | WIP... -------------------------------------------------------------------------------- /src/com/stuypulse/stuylib/input/readme.md: -------------------------------------------------------------------------------- 1 | # StuyLib Gamepad Library 2 | 3 | WIP... -------------------------------------------------------------------------------- /src/com/stuypulse/stuylib/math/SLMath.java: -------------------------------------------------------------------------------- 1 | /* Copyright (c) 2025 StuyPulse Robotics. All rights reserved. */ 2 | /* This work is licensed under the terms of the MIT license */ 3 | /* found in the root directory of this project. */ 4 | 5 | package com.stuypulse.stuylib.math; 6 | 7 | /** 8 | * SLMath (StuyLib Math) is a class containing many algorithms that are useful for developing robot 9 | * code. Algorithms include limit, deadband, raising to powers while keeping the sign, and some 10 | * other new algorithms we came up with. 11 | * 12 | * @author Sam (sam.belliveau@gmail.com) 13 | */ 14 | public final class SLMath { 15 | 16 | // Prevent the class from being extended at all 17 | private SLMath() {} 18 | 19 | /**************/ 20 | /*** LIMITS ***/ 21 | /**************/ 22 | 23 | /** 24 | * clamp input from max to min 25 | * 26 | * @param x input 27 | * @param min min value for x 28 | * @param max max value for x 29 | * @return clamp input 30 | */ 31 | public static double clamp(double x, double min, double max) { 32 | if (min < max) { 33 | if (x > max) return max; 34 | if (x < min) return min; 35 | } else { 36 | if (x > min) return min; 37 | if (x < max) return max; 38 | } 39 | return x; 40 | } 41 | 42 | /** 43 | * clamp input from max to -max 44 | * 45 | * @param x input 46 | * @param max max and min value 47 | * @return clamped input 48 | */ 49 | public static double clamp(double x, double max) { 50 | return clamp(x, -max, max); 51 | } 52 | 53 | /** 54 | * clamp input from -1 to 1 55 | * 56 | * @param x input 57 | * @return clamped input 58 | */ 59 | public static double clamp(double x) { 60 | return clamp(x, 1.0); 61 | } 62 | 63 | /**************************/ 64 | /*** DEADBAND ALGORITHM ***/ 65 | /**************************/ 66 | 67 | /** 68 | * Dead bands x value with window being the dead band. all values for this are [-1.0...1.0] 69 | * 70 | * @param x value 71 | * @param window deadband window 72 | * @return deadbanded value 73 | */ 74 | public static double deadband(double x, double window) { 75 | window = Math.abs(window); 76 | 77 | if (Math.abs(x) < window) { 78 | return 0.0; 79 | } else { 80 | return (x - Math.copySign(window, x)) / (1.0 - window); 81 | } 82 | } 83 | 84 | /*****************************************/ 85 | /*** RAISE TO POWER WHILE KEEPING SIGN ***/ 86 | /*****************************************/ 87 | 88 | /** 89 | * spow (signless pow), raises a number to a power without affecting the sign of the number 90 | * 91 | * @param x input 92 | * @param power power to raise x to 93 | * @return input ^ power 94 | */ 95 | public static double spow(double x, double power) { 96 | return Math.pow(Math.abs(x), power) * Math.signum(x); 97 | } 98 | } 99 | -------------------------------------------------------------------------------- /src/com/stuypulse/stuylib/math/Vector2D.java: -------------------------------------------------------------------------------- 1 | /* Copyright (c) 2025 StuyPulse Robotics. All rights reserved. */ 2 | /* This work is licensed under the terms of the MIT license */ 3 | /* found in the root directory of this project. */ 4 | 5 | package com.stuypulse.stuylib.math; 6 | 7 | import edu.wpi.first.math.geometry.Translation2d; 8 | 9 | /** 10 | * A Vector2D class that stores x and y position data. It is made to work with the StuyLib Angle 11 | * class and be easy to use. It is a standard Vector2D class with all of the functions that you 12 | * would expect. 13 | * 14 | * @author Sam (sam.belliveau@gmail.com) 15 | */ 16 | public final class Vector2D { 17 | 18 | // Vector Constnants 19 | public static final Vector2D kOrigin = new Vector2D(0, 0); 20 | public static final Vector2D kI = new Vector2D(1, 0); 21 | public static final Vector2D kJ = new Vector2D(0, 1); 22 | 23 | /****************************/ 24 | /*** Class Implementation ***/ 25 | /****************************/ 26 | 27 | /** The x position of the Vector2D */ 28 | public final double x; 29 | 30 | /** The y position of the Vector2D */ 31 | public final double y; 32 | 33 | /** 34 | * @param x the x axis of the vector 35 | * @param y the y axis of the vector 36 | */ 37 | public Vector2D(double x, double y) { 38 | this.x = x; 39 | this.y = y; 40 | } 41 | 42 | /** 43 | * @param axis Array of size 2 where the first element will be defined as x, and the second will 44 | * be defined as y. 45 | */ 46 | public Vector2D(double[] axis) { 47 | if (axis.length != 2) { 48 | throw new IllegalArgumentException("axis must be of size 2"); 49 | } 50 | 51 | this.x = axis[0]; 52 | this.y = axis[1]; 53 | } 54 | 55 | /** @param translation Translation2d to copy this vector into */ 56 | public Vector2D(Translation2d translation) { 57 | this.x = translation.getX(); 58 | this.y = translation.getY(); 59 | } 60 | 61 | /** @return double array of size 2 defined as {x, y} */ 62 | public double[] getArray() { 63 | return new double[] {x, y}; 64 | } 65 | 66 | /** 67 | * Return the StuyLib Vector2D class in the form as WPILib's Translation2d. 68 | * 69 | *
This function is here in order to make interoperability with WPILib easier so that manual
70 | * conversion isn't needed as much.
71 | *
72 | * @return Translation2d class with the same value as the Vector2d
73 | */
74 | public Translation2d getTranslation2d() {
75 | return new Translation2d(x, y);
76 | }
77 |
78 | /**
79 | * @param other other Vector2D
80 | * @return the distance between the two Vector2Ds
81 | */
82 | public double distance(Vector2D other) {
83 | return Math.hypot(other.x - this.x, other.y - this.y);
84 | }
85 |
86 | /** @return distance from 0, 0 */
87 | public double distance() {
88 | return Math.hypot(this.x, this.y);
89 | }
90 |
91 | /** @return magnitude of the vector (same as distance from 0, 0) */
92 | public double magnitude() {
93 | return this.distance();
94 | }
95 |
96 | /** @return the angle of the Vector2D around 0, 0 */
97 | public Angle getAngle() {
98 | return Angle.fromVector(this);
99 | }
100 |
101 | /**
102 | * @param angle angle to rotate by
103 | * @param origin point to rotate around
104 | * @return result of rotation
105 | */
106 | public Vector2D rotate(Angle angle, Vector2D origin) {
107 | return new Vector2D(
108 | origin.x + (this.x - origin.x) * angle.cos() - (this.y - origin.y) * angle.sin(),
109 | origin.y + (this.y - origin.y) * angle.cos() + (this.x - origin.x) * angle.sin());
110 | }
111 |
112 | /**
113 | * @param angle angle to rotate by
114 | * @return result of rotation
115 | */
116 | public Vector2D rotate(Angle angle) {
117 | return new Vector2D(
118 | this.x * angle.cos() - this.y * angle.sin(),
119 | this.y * angle.cos() + this.x * angle.sin());
120 | }
121 |
122 | /**
123 | * @param other Vector2D to be added by
124 | * @return sum of the two Vector2Ds
125 | */
126 | public Vector2D add(Vector2D other) {
127 | return new Vector2D(this.x + other.x, this.y + other.y);
128 | }
129 |
130 | /**
131 | * @param other Vector2D to be subtracted from
132 | * @return difference between the two Vector2Ds
133 | */
134 | public Vector2D sub(Vector2D other) {
135 | return new Vector2D(this.x - other.x, this.y - other.y);
136 | }
137 |
138 | /**
139 | * @param other Vector2D to be multiplied by
140 | * @return product of the two Vector2Ds
141 | */
142 | public Vector2D mul(Vector2D other) {
143 | return new Vector2D(this.x * other.x, this.y * other.y);
144 | }
145 |
146 | /**
147 | * @param other Vector2D to be divided by
148 | * @return division of the two Vector2Ds
149 | */
150 | public Vector2D div(Vector2D other) {
151 | return new Vector2D(this.x / other.x, this.y / other.y);
152 | }
153 |
154 | /**
155 | * @param multiplier amount to multiply the x and y components by
156 | * @return result of multiplying the x and y components by the multiplier
157 | */
158 | public Vector2D mul(double multiplier) {
159 | return new Vector2D(this.x * multiplier, this.y * multiplier);
160 | }
161 |
162 | /**
163 | * @param divisor amount to divide the x and y components by
164 | * @return result of dividing the x and y components by the divisor
165 | */
166 | public Vector2D div(double divisor) {
167 | return new Vector2D(this.x / divisor, this.y / divisor);
168 | }
169 |
170 | /**
171 | * @param power power to raise magnitude of vector to
172 | * @return result of raising the magnitude of the vector to the power
173 | */
174 | public Vector2D pow(double power) {
175 | return this.mul(Math.pow(magnitude(), power - 1));
176 | }
177 |
178 | /**
179 | * @param other Vector2D to perform dot product with
180 | * @return result of performing the dot product with the other Vector2D
181 | */
182 | public double dot(Vector2D other) {
183 | return this.x * other.x + this.y * other.y;
184 | }
185 |
186 | /**
187 | * @param other Vector3D to perform cross product with
188 | * @return result of performing the cross product with the other Vector2D
189 | */
190 | public double cross(Vector2D other) {
191 | return this.x * other.y - this.y * other.x;
192 | }
193 |
194 | /** @return result of normalizing the Vector2D so that the magnitude is 1.0 */
195 | public Vector2D normalize() {
196 | final double magnitude = this.distance();
197 | if (magnitude <= 1e-9) {
198 | return Vector2D.kI;
199 | } else {
200 | return this.div(magnitude);
201 | }
202 | }
203 |
204 | /**
205 | * limit the magnitude of a vector to a maximum
206 | *
207 | * @param maxMagnitude max magitude of resulting vector
208 | * @return vector with limited magnitude
209 | */
210 | public Vector2D clamp(double maxMagnitude) {
211 | if (maxMagnitude <= 0.0) return Vector2D.kOrigin;
212 |
213 | final double magnitude = this.distance();
214 |
215 | if (maxMagnitude <= magnitude) {
216 | return mul(maxMagnitude / magnitude);
217 | }
218 |
219 | return this;
220 | }
221 |
222 | /** @return result of negating the x and y components */
223 | public Vector2D negative() {
224 | return new Vector2D(-this.x, -this.y);
225 | }
226 |
227 | /**
228 | * @param other object to compare to
229 | * @return both objects are Vector2Ds and they equal eachother
230 | */
231 | @Override
232 | public boolean equals(Object other) {
233 | if (this == other) {
234 | return true;
235 | }
236 |
237 | if (other instanceof Vector2D) {
238 | Vector2D o = (Vector2D) other;
239 | return this.x == o.x && this.y == o.y;
240 | }
241 |
242 | return false;
243 | }
244 | }
245 |
--------------------------------------------------------------------------------
/src/com/stuypulse/stuylib/math/readme.md:
--------------------------------------------------------------------------------
1 | # StuyLib Math Library
2 |
3 | ## What is the point of these classes
4 |
5 | ### [SLMath](https://github.com/StuyPulse/StuyLib/blob/main/src/com/stuypulse/stuylib/math/SLMath.java)
6 |
7 | SLMath is just a class with some static math functions that are useful in robotics.
8 |
9 | For example, when squaring a number, `SLMath.square(x)` will keep the sign of the number. *this is used when filtering controller inputs* `SLMath.pow(x, p)` will also keep the sign of x no matter what the value of x or p. This might not seem useful normally, but is nice to have in FRC.
10 |
11 | Other functions like `SLMath.limit` or `SLMath.deadband` are super helpful in FRC.
12 |
13 | ### [Vector2D](https://github.com/StuyPulse/StuyLib/blob/main/src/com/stuypulse/stuylib/math/Vector2D.java)
14 |
15 | This is just a standard Vector2D class that has every feature you would need, and also works with our other classes in Stuylib. It also works nicely with our [Angle](https://github.com/StuyPulse/StuyLib/blob/main/src/com/stuypulse/stuylib/math/Angle.java) class, which is a nice bonus.
16 |
17 | ### [Angle](https://github.com/StuyPulse/StuyLib/blob/main/src/com/stuypulse/stuylib/math/Vector2D.java)
18 |
19 | This is an angle class that removes the confusion about degrees / radians, and works nicely with our [Vector2D](https://github.com/StuyPulse/StuyLib/blob/main/src/com/stuypulse/stuylib/math/Vector2D.java). When doing math with the angle class, you do not need to worry about the value of the angle being outside the range of `-pi < x < pi`.
20 |
21 | ## Summary
22 |
23 | This Math folder in stuylib contains some helpful utilities that make programming FRC code a little bit easier.
--------------------------------------------------------------------------------
/src/com/stuypulse/stuylib/network/SmartBoolean.java:
--------------------------------------------------------------------------------
1 | /* Copyright (c) 2025 StuyPulse Robotics. All rights reserved. */
2 | /* This work is licensed under the terms of the MIT license */
3 | /* found in the root directory of this project. */
4 |
5 | package com.stuypulse.stuylib.network;
6 |
7 | import com.stuypulse.stuylib.streams.booleans.BStream;
8 |
9 | import edu.wpi.first.networktables.NetworkTablesJNI;
10 | import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
11 |
12 | /**
13 | * {@link SmartBoolean} works as a wrapper for values on {@link SmartDashboard}. The idea for this
14 | * class was to make getting values on {@link SmartDashboard} easier by making them variables that
15 | * you know were initialized.
16 | *
17 | * @author Sam (sam.belliveau@gmail.com)
18 | */
19 | public final class SmartBoolean implements BStream {
20 |
21 | /** The ID / Name for the value on {@link SmartDashboard}. */
22 | private final int mHandle;
23 |
24 | /** The default value that the {@link SmartDashboard} value was set too. */
25 | private final boolean mDefaultValue;
26 |
27 | /**
28 | * Creates a {@link SmartBoolean} with the element name and a default value. The value on {@link
29 | * SmartDashboard} will be reset to the default value on initialization.
30 | *
31 | * @param id the name of the boolean on {@link SmartDashboard}
32 | * @param value the default / initialization value for the value
33 | */
34 | public SmartBoolean(String id, boolean value) {
35 | mHandle =
36 | NetworkTablesJNI.getEntry(
37 | NetworkTablesJNI.getDefaultInstance(), "SmartDashboard/" + id);
38 | mDefaultValue = value;
39 | reset();
40 | }
41 |
42 | /** @return the value of the boolean from {@link SmartDashboard} */
43 | public boolean get() {
44 | return NetworkTablesJNI.getBoolean(mHandle, mDefaultValue);
45 | }
46 |
47 | /** @return the default value of the boolean */
48 | public boolean getDefault() {
49 | return mDefaultValue;
50 | }
51 |
52 | /** @param value what the value on {@link SmartDashboard} will be set to */
53 | public void set(boolean value) {
54 | NetworkTablesJNI.setBoolean(mHandle, 0, value);
55 | }
56 |
57 | /** Resets the value on {@link SmartDashboard} to the default value */
58 | public void reset() {
59 | set(getDefault());
60 | }
61 | }
62 |
--------------------------------------------------------------------------------
/src/com/stuypulse/stuylib/network/SmartNumber.java:
--------------------------------------------------------------------------------
1 | /* Copyright (c) 2025 StuyPulse Robotics. All rights reserved. */
2 | /* This work is licensed under the terms of the MIT license */
3 | /* found in the root directory of this project. */
4 |
5 | package com.stuypulse.stuylib.network;
6 |
7 | import com.stuypulse.stuylib.streams.numbers.IStream;
8 |
9 | import edu.wpi.first.networktables.NetworkTablesJNI;
10 | import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
11 |
12 | /**
13 | * SmartNumber works as a wrapper for values on {@link SmartDashboard}. The idea for this class was
14 | * to make getting values on {@link SmartDashboard} easier by making them variables that you know
15 | * were initialized.
16 | *
17 | * @author Sam (sam.belliveau@gmail.com)
18 | */
19 | public final class SmartNumber extends Number implements IStream {
20 |
21 | private static final long serialVersionUID = 1L;
22 |
23 | /** The ID / Name for the value on {@link SmartDashboard}. */
24 | private final int mHandle;
25 |
26 | /** The default value that the {@link SmartDashboard} value was set too. */
27 | private final double mDefaultValue;
28 |
29 | /**
30 | * Creates a SmartNumber with the element name and a default value. The value on {@link
31 | * SmartDashboard} will be reset to the default value on initialization.
32 | *
33 | * @param id the name of the number on SmartDashboard
34 | * @param value the default / initialization value for the value
35 | */
36 | public SmartNumber(String id, double value) {
37 | mHandle =
38 | NetworkTablesJNI.getEntry(
39 | NetworkTablesJNI.getDefaultInstance(), "SmartDashboard/" + id);
40 | mDefaultValue = value;
41 | reset();
42 | }
43 |
44 | /** @return the value of the number from SmartDashboard */
45 | public double get() {
46 | return NetworkTablesJNI.getDouble(mHandle, mDefaultValue);
47 | }
48 |
49 | /** @return the default value of the number */
50 | public double getDefault() {
51 | return mDefaultValue;
52 | }
53 |
54 | /** @param value what the value on {@link SmartDashboard} will be set to */
55 | public void set(Number value) {
56 | NetworkTablesJNI.setDouble(mHandle, 0, value.doubleValue());
57 | }
58 |
59 | /** Resets the value on {@link SmartDashboard} to the default value */
60 | public void reset() {
61 | set(getDefault());
62 | }
63 |
64 | /** @return the value of the number from {@link SmartDashboard} (casted to a double) */
65 | public double doubleValue() {
66 | return (double) this.get();
67 | }
68 |
69 | /** @return the value of the number from {@link SmartDashboard} (casted to a float) */
70 | public float floatValue() {
71 | return (float) this.get();
72 | }
73 |
74 | /** @return the value of the number from {@link SmartDashboard} (casted to a int) */
75 | public int intValue() {
76 | return (int) this.get();
77 | }
78 |
79 | /** @return the value of the number from {@link SmartDashboard} (casted to a long) */
80 | public long longValue() {
81 | return (long) this.get();
82 | }
83 |
84 | /**
85 | * This function checks to see if {@code oldNumber} is a {@link SmartNumber}, if it is, then it
86 | * sets the {@code oldNumber} to {@code newNumber} and returns the {@code oldNumber}. Otherwise,
87 | * it just returns {@code newNumber}.
88 | *
89 | * @param oldNumber number that is being replaced
90 | * @param newNumber number representing the value of the new number
91 | * @return if {@code oldNumber} is not a {@link SmartNumber}, then this function will return
92 | * {@code newNumber}. If {@code oldNumber} is a {@link SmartNumber}, then this function will
93 | * return {@code oldNumber} with its value set to {@code newNumber}
94 | */
95 | public static Number setNumber(Number oldNumber, Number newNumber) {
96 | if (oldNumber instanceof SmartNumber) {
97 | SmartNumber oldSmartNumber = (SmartNumber) oldNumber;
98 | oldSmartNumber.set(newNumber.doubleValue());
99 | return oldSmartNumber;
100 | } else {
101 | return newNumber;
102 | }
103 | }
104 | }
105 |
--------------------------------------------------------------------------------
/src/com/stuypulse/stuylib/network/SmartString.java:
--------------------------------------------------------------------------------
1 | /* Copyright (c) 2025 StuyPulse Robotics. All rights reserved. */
2 | /* This work is licensed under the terms of the MIT license */
3 | /* found in the root directory of this project. */
4 |
5 | package com.stuypulse.stuylib.network;
6 |
7 | import edu.wpi.first.networktables.NetworkTablesJNI;
8 | import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
9 |
10 | import java.util.function.Supplier;
11 |
12 | /**
13 | * SmartString works as a wrapper for values on {@link SmartDashboard}. The idea for this class was
14 | * to make getting values on {@link SmartDashboard} easier by making them variables that you know
15 | * were initialized.
16 | *
17 | * @author Sam (sam.belliveau@gmail.com)
18 | */
19 | public final class SmartString implements Supplier This could be used to automatically filter controller inputs
14 | *
15 | * @author Sam (sam.belliveau@gmail.com)
16 | */
17 | public class FilteredAStream implements AStream {
18 |
19 | private AStream mStream; // Stream used
20 | private AFilter mStreamFilter; // StreamFilter used
21 |
22 | /**
23 | * Makes filtered stream from stream and stream filter
24 | *
25 | * @param stream input stream
26 | * @param filter stream filter
27 | */
28 | public FilteredAStream(AStream stream, AFilter... filter) {
29 | mStream = stream;
30 | mStreamFilter = AFilter.create(filter);
31 | }
32 |
33 | /**
34 | * Get next value from filtered stream
35 | *
36 | * @return next value
37 | */
38 | public Angle get() {
39 | return mStreamFilter.get(mStream.get());
40 | }
41 | }
42 |
--------------------------------------------------------------------------------
/src/com/stuypulse/stuylib/streams/angles/PollingAStream.java:
--------------------------------------------------------------------------------
1 | /* Copyright (c) 2025 StuyPulse Robotics. All rights reserved. */
2 | /* This work is licensed under the terms of the MIT license */
3 | /* found in the root directory of this project. */
4 |
5 | package com.stuypulse.stuylib.streams.angles;
6 |
7 | import com.stuypulse.stuylib.math.Angle;
8 |
9 | import edu.wpi.first.wpilibj.Notifier;
10 |
11 | /**
12 | * A PollingAStream calls {@link AStream#get()} every x milliseconds instead of when the user calls
13 | * get
14 | *
15 | * @author Sam (sam.belliveau@gmail.com)
16 | */
17 | public class PollingAStream implements AStream, AutoCloseable {
18 |
19 | private Notifier mPoller;
20 | private volatile Angle mResult;
21 |
22 | /**
23 | * Creates a PollingAStream from an AStream and a time value
24 | *
25 | * @param stream astream to poll from
26 | * @param dt Number of calls per second
27 | */
28 | public PollingAStream(AStream stream, double dt) {
29 | if (dt <= 0) {
30 | throw new IllegalArgumentException("dt must be greater than 0");
31 | }
32 |
33 | mResult = Angle.kNull;
34 | mPoller = new Notifier(() -> mResult = stream.get());
35 | mPoller.startPeriodic(dt);
36 | }
37 |
38 | public Angle get() {
39 | return mResult;
40 | }
41 |
42 | protected void finalize() {
43 | close();
44 | }
45 |
46 | public void close() {
47 | mPoller.close();
48 | mResult = Angle.kNull;
49 | }
50 | }
51 |
--------------------------------------------------------------------------------
/src/com/stuypulse/stuylib/streams/angles/filters/AFilter.java:
--------------------------------------------------------------------------------
1 | /* Copyright (c) 2025 StuyPulse Robotics. All rights reserved. */
2 | /* This work is licensed under the terms of the MIT license */
3 | /* found in the root directory of this project. */
4 |
5 | package com.stuypulse.stuylib.streams.angles.filters;
6 |
7 | import com.stuypulse.stuylib.math.Angle;
8 |
9 | /**
10 | * This is the Filter interface class that gives a definition for how to implement a filter.
11 | *
12 | * All that a filter does is take in the next double in a series and gives you the filtered
13 | * value.
14 | *
15 | * @author Sam (sam.belliveau@gmail.com)
16 | */
17 | public interface AFilter {
18 |
19 | /**
20 | * Create an AFilter from a list of AFilters. This will create an AFilter with the least
21 | * possible overhead for the number of filters provided.
22 | *
23 | * @param filters list of AFilters to apply
24 | * @return an AFilter that uses every filter given
25 | */
26 | public static AFilter create(AFilter... filters) {
27 | switch (filters.length) {
28 | case 0:
29 | return x -> x;
30 | case 1:
31 | return filters[0];
32 | case 2:
33 | return x -> filters[1].get(filters[0].get(x));
34 | case 3:
35 | return x -> filters[2].get(filters[1].get(filters[0].get(x)));
36 | case 4:
37 | return x -> filters[3].get(filters[2].get(filters[1].get(filters[0].get(x))));
38 | default:
39 | return new AFilterGroup(filters);
40 | }
41 | }
42 |
43 | /**
44 | * Get next value in Filter based on the next value given
45 | *
46 | * @param next next input value in the stream
47 | * @return the output value of the filter
48 | */
49 | public Angle get(Angle next);
50 |
51 | /**
52 | * Combine an AFilter with another AFilter
53 | *
54 | * @param next filter to be evaluated after this one
55 | * @return the combined filter
56 | */
57 | public default AFilter then(AFilter next) {
58 | return x -> next.get(get(x));
59 | }
60 |
61 | /**
62 | * Combine two AFilters by adding their results together
63 | *
64 | * @param other other AFilter to add to this one
65 | * @return the resulting AFilter after the sum
66 | */
67 | public default AFilter add(AFilter other) {
68 | return x -> get(x).add(other.get(x));
69 | }
70 |
71 | /**
72 | * Combine two AFilters by subtracting their results together
73 | *
74 | * @param other other AFilter to subtract from this one
75 | * @return the resulting AFilter after the subtraction
76 | */
77 | public default AFilter sub(AFilter other) {
78 | return x -> get(x).sub(other.get(x));
79 | }
80 |
81 | /**
82 | * Invert an AFilter by subtracting the input from the result of the AFilter.
83 | *
84 | * Inverting a LowPassFilter gives you a HighPassFilter and vise versa.
85 | *
86 | * Inverting something twice gives you the original value.
87 | *
88 | * @return the inverted filter
89 | */
90 | public default AFilter invert() {
91 | return x -> x.sub(get(x));
92 | }
93 | }
94 |
--------------------------------------------------------------------------------
/src/com/stuypulse/stuylib/streams/angles/filters/AFilterGroup.java:
--------------------------------------------------------------------------------
1 | /* Copyright (c) 2025 StuyPulse Robotics. All rights reserved. */
2 | /* This work is licensed under the terms of the MIT license */
3 | /* found in the root directory of this project. */
4 |
5 | package com.stuypulse.stuylib.streams.angles.filters;
6 |
7 | import com.stuypulse.stuylib.math.Angle;
8 |
9 | /**
10 | * A class that lets you combine multiple stream filters into one stream filter
11 | *
12 | * @author Sam (sam.belliveau@gmail.com)
13 | */
14 | public class AFilterGroup implements AFilter {
15 |
16 | // Array of all the filters
17 | private final AFilter[] mFilters;
18 |
19 | /**
20 | * Make AFilterGroup out of an array of Filters
21 | *
22 | * @param filters list of filters to combine
23 | */
24 | public AFilterGroup(AFilter... filters) {
25 | mFilters = filters;
26 | }
27 |
28 | public Angle get(Angle next) {
29 | // Put next through each of the filters
30 | for (AFilter filter : mFilters) {
31 | next = filter.get(next);
32 | }
33 |
34 | // Return filtered value
35 | return next;
36 | }
37 | }
38 |
--------------------------------------------------------------------------------
/src/com/stuypulse/stuylib/streams/angles/filters/AHighPassFilter.java:
--------------------------------------------------------------------------------
1 | /* Copyright (c) 2025 StuyPulse Robotics. All rights reserved. */
2 | /* This work is licensed under the terms of the MIT license */
3 | /* found in the root directory of this project. */
4 |
5 | package com.stuypulse.stuylib.streams.angles.filters;
6 |
7 | import com.stuypulse.stuylib.math.Angle;
8 |
9 | /**
10 | * Implementation for of a real time IIR HighPassFilter
11 | *
12 | * @author Sam (sam.belliveau@gmail.com)
13 | */
14 | public class AHighPassFilter implements AFilter {
15 |
16 | private ALowPassFilter mInverse;
17 |
18 | /** @param rc time constant for high pass filter */
19 | public AHighPassFilter(Number rc) {
20 | mInverse = new ALowPassFilter(rc);
21 | }
22 |
23 | public Angle get(Angle next) {
24 | return next.sub(mInverse.get(next));
25 | }
26 | }
27 |
--------------------------------------------------------------------------------
/src/com/stuypulse/stuylib/streams/angles/filters/ALowPassFilter.java:
--------------------------------------------------------------------------------
1 | /* Copyright (c) 2025 StuyPulse Robotics. All rights reserved. */
2 | /* This work is licensed under the terms of the MIT license */
3 | /* found in the root directory of this project. */
4 |
5 | package com.stuypulse.stuylib.streams.angles.filters;
6 |
7 | import com.stuypulse.stuylib.math.Angle;
8 | import com.stuypulse.stuylib.util.StopWatch;
9 |
10 | /**
11 | * Implementation of a real time IIR LowPassFilter
12 | *
13 | * @author Sam (sam.belliveau@gmail.com)
14 | */
15 | public class ALowPassFilter implements AFilter {
16 |
17 | // Used to calculate time in between calls
18 | private final StopWatch mTimer;
19 |
20 | // Used to calculate next value based on previous value and time
21 | private final Number mRC;
22 | private Angle mLastValue;
23 |
24 | /**
25 | * @param rc Time Constant. The time constant is the amount of time in seconds that it takes to
26 | * get 63.2% of the way to the target value. 63.2% is (1 - (1 / e)).
27 | */
28 | public ALowPassFilter(Number rc) {
29 | mTimer = new StopWatch();
30 | mLastValue = Angle.kZero;
31 | mRC = rc;
32 | }
33 |
34 | public Angle get(Angle next) {
35 | // If RC will cause errors, disable filter
36 | if (mRC.doubleValue() <= 0.0) {
37 | return mLastValue = next;
38 | }
39 |
40 | // Get a constant, which is determined based on dt and the mRC constant
41 | double a = Math.exp(-mTimer.reset() / mRC.doubleValue());
42 |
43 | // Based on the value of a (which is determined by dt), the next value
44 | // could either change a lot, or not by much. (smaller dt = smaller change)
45 | return mLastValue = mLastValue.add(next.sub(mLastValue).mul(1.0 - a));
46 | }
47 | }
48 |
--------------------------------------------------------------------------------
/src/com/stuypulse/stuylib/streams/angles/filters/AMotionProfile.java:
--------------------------------------------------------------------------------
1 | /* Copyright (c) 2025 StuyPulse Robotics. All rights reserved. */
2 | /* This work is licensed under the terms of the MIT license */
3 | /* found in the root directory of this project. */
4 |
5 | package com.stuypulse.stuylib.streams.angles.filters;
6 |
7 | import com.stuypulse.stuylib.math.Angle;
8 | import com.stuypulse.stuylib.math.SLMath;
9 | import com.stuypulse.stuylib.util.StopWatch;
10 |
11 | /**
12 | * A filter, that when applied to the input of a motor, will profile it. Similar to the way in which
13 | * motion profiling can limit the amount of acceleration and jerk in an S-Curve, this can do that to
14 | * real time input. Because this will add a delay, it is recommended that the accelLimit is as high
15 | * as possible. Aside from the accelLimit, this is identical to a SlewRateLimiter or TimedRateLimit.
16 | *
17 | * @author Sam (sam.belliveau@gmail.com)
18 | */
19 | public class AMotionProfile implements AFilter {
20 |
21 | // Default number of times to apply filter (helps accuracy)
22 | private static final int kDefaultSteps = 64;
23 |
24 | // Stopwatch to Track dt
25 | private StopWatch mTimer;
26 |
27 | // Limits for each of the derivatives
28 | private Number mVelLimit;
29 | private Number mAccelLimit;
30 |
31 | // The last output / acceleration
32 | private Angle mOutput;
33 | private double mAccel;
34 |
35 | // Number of times to apply filter (helps accuracy)
36 | private final int mSteps;
37 |
38 | /**
39 | * @param velLimit maximum change in velocity per second (u/s)
40 | * @param accelLimit maximum change in acceleration per second (u/s/s)
41 | * @param steps number of times to apply filter (improves accuracy)
42 | */
43 | public AMotionProfile(Number velLimit, Number accelLimit, int steps) {
44 | mTimer = new StopWatch();
45 |
46 | mVelLimit = velLimit;
47 | mAccelLimit = accelLimit;
48 |
49 | mOutput = Angle.kZero;
50 | mAccel = 0;
51 |
52 | mSteps = steps;
53 | }
54 |
55 | /**
56 | * @param velLimit maximum change in velocity per second (u/s)
57 | * @param accelLimit maximum change in acceleration per second (u/s/s)
58 | */
59 | public AMotionProfile(Number velLimit, Number accelLimit) {
60 | this(velLimit, accelLimit, kDefaultSteps);
61 | }
62 |
63 | public Angle get(Angle target) {
64 | double dt = mTimer.reset() / mSteps;
65 |
66 | for (int i = 0; i < mSteps; ++i) {
67 | // if there is a accel limit, limit the amount the acceleration can change
68 | if (0 < mAccelLimit.doubleValue()) {
69 | // amount of windup in system (how long it would take to slow down)
70 | double windup = Math.abs(mAccel) / mAccelLimit.doubleValue();
71 |
72 | // If the windup is too small, just use normal algorithm to limit acceleration
73 | if (windup < dt) {
74 | // Calculate acceleration needed to reach target
75 | double accel = target.velocityRadians(mOutput, dt) - mAccel;
76 |
77 | // Try to reach it while abiding by accellimit
78 | mAccel += SLMath.clamp(accel, dt * mAccelLimit.doubleValue());
79 | } else {
80 | // the position it would end up if it attempted to come to a full stop
81 | Angle future =
82 | mOutput.addRadians(
83 | 0.5 * mAccel * (dt + windup)); // where the robot will end up
84 |
85 | // Calculate acceleration needed to come to stop at target throughout windup
86 | double accel = target.velocityRadians(future, windup);
87 |
88 | // Try to reach it while abiding by accelLimit
89 | mAccel += SLMath.clamp(accel, dt * mAccelLimit.doubleValue());
90 | }
91 |
92 | } else {
93 | // make the acceleration the difference between target and current
94 | mAccel = target.velocityRadians(mOutput, dt);
95 | }
96 |
97 | // if there is an acceleration limit, limit the acceleration
98 | if (0 < mVelLimit.doubleValue()) {
99 | mAccel = SLMath.clamp(mAccel, mVelLimit.doubleValue());
100 | }
101 |
102 | // adjust output by calculated acceleration
103 | mOutput = mOutput.addRadians(dt * mAccel);
104 | }
105 |
106 | return mOutput;
107 | }
108 | }
109 |
--------------------------------------------------------------------------------
/src/com/stuypulse/stuylib/streams/angles/filters/ARateLimit.java:
--------------------------------------------------------------------------------
1 | /* Copyright (c) 2025 StuyPulse Robotics. All rights reserved. */
2 | /* This work is licensed under the terms of the MIT license */
3 | /* found in the root directory of this project. */
4 |
5 | package com.stuypulse.stuylib.streams.angles.filters;
6 |
7 | import com.stuypulse.stuylib.math.Angle;
8 | import com.stuypulse.stuylib.math.SLMath;
9 | import com.stuypulse.stuylib.util.StopWatch;
10 |
11 | /**
12 | * This class lets you rate limit a stream of inputs
13 | *
14 | * Instead of being based on the rate that update is called, the value you give it is based on
15 | * how much it should be able to change in one second.
16 | *
17 | * @author Sam (sam.belliveau@gmail.com)
18 | */
19 | public class ARateLimit implements AFilter {
20 |
21 | // Used to get the time since the last get call
22 | private StopWatch mTimer;
23 |
24 | // Used to limit the change from the last value
25 | private Angle mLastValue;
26 | private Number mRateLimit;
27 |
28 | /** @param rateLimit The max speed in rad/s. */
29 | public ARateLimit(Number rateLimit) {
30 | if (rateLimit.doubleValue() <= 0) {
31 | throw new IllegalArgumentException("rateLimit must be a positive number");
32 | }
33 |
34 | mTimer = new StopWatch();
35 | mRateLimit = rateLimit;
36 | mLastValue = Angle.kZero;
37 | }
38 |
39 | public Angle get(Angle next) {
40 | final double limit = mRateLimit.doubleValue() * mTimer.reset();
41 | return mLastValue =
42 | mLastValue.addRadians(SLMath.clamp(next.sub(mLastValue).toRadians(), limit));
43 | }
44 | }
45 |
--------------------------------------------------------------------------------
/src/com/stuypulse/stuylib/streams/booleans/BStream.java:
--------------------------------------------------------------------------------
1 | /* Copyright (c) 2025 StuyPulse Robotics. All rights reserved. */
2 | /* This work is licensed under the terms of the MIT license */
3 | /* found in the root directory of this project. */
4 |
5 | package com.stuypulse.stuylib.streams.booleans;
6 |
7 | import com.stuypulse.stuylib.streams.booleans.filters.BFilter;
8 |
9 | import edu.wpi.first.wpilibj.DigitalInput;
10 | import edu.wpi.first.wpilibj2.command.button.Trigger;
11 |
12 | import java.util.function.BooleanSupplier;
13 |
14 | /**
15 | * A BStream is similar to an IStream, but instead of a stream of doubles, it represents a stream of
16 | * booleans.
17 | *
18 | * @author Sam (sam.belliveau@gmail.com)
19 | */
20 | public interface BStream extends BooleanSupplier {
21 |
22 | /**
23 | * Create a BStream from another BStream. This is helpful if you want to use some of the
24 | * decorator functions with a lambda.
25 | *
26 | * @param stream stream to create BStream from
27 | * @return the resulting BStream
28 | */
29 | public static BStream create(BStream stream) {
30 | return stream;
31 | }
32 |
33 | /**
34 | * Create a BStream from a digital source. This can helpful for processing a digital stream,
35 | * like negating the value when used for switches.
36 | *
37 | * @param input digital input object
38 | * @return the resulting BStream
39 | */
40 | public static BStream create(DigitalInput input) {
41 | return input::get;
42 | }
43 |
44 | /** @return next value in the stream */
45 | public boolean get();
46 |
47 | /** @return get BStream as a Boolean */
48 | public default boolean getAsBoolean() {
49 | return get();
50 | }
51 |
52 | /**
53 | * Create a new FilteredBStream from the current stream
54 | *
55 | * @param filters the filters you want to apply to the BStream
56 | * @return The FilteredBStream
57 | */
58 | public default FilteredBStream filtered(BFilter... filters) {
59 | return new FilteredBStream(this, filters);
60 | }
61 |
62 | /**
63 | * Create a new PollingBStream from the current stream
64 | *
65 | * @param dt the time inbetween each poll of the BStream
66 | * @return The PollingBStream
67 | */
68 | public default PollingBStream polling(double dt) {
69 | return new PollingBStream(this, dt);
70 | }
71 |
72 | /**
73 | * Combine two BStreams by and'ing their results together
74 | *
75 | * @param other other BStream to and with this one
76 | * @return the resulting BStream after the and
77 | */
78 | public default BStream and(BStream other) {
79 | return () -> get() & other.get();
80 | }
81 |
82 | /**
83 | * Combine two BStreams by or'ing their results together
84 | *
85 | * @param other other BStream to or with this one
86 | * @return the resulting BStream after the or
87 | */
88 | public default BStream or(BStream other) {
89 | return () -> get() | other.get();
90 | }
91 |
92 | /**
93 | * Combine two BStreams by xor'ing their results together
94 | *
95 | * @param other other BStream to xor with this one
96 | * @return the resulting BStream after the xor
97 | */
98 | public default BStream xor(BStream other) {
99 | return () -> get() ^ other.get();
100 | }
101 |
102 | /**
103 | * Create a BStream that returns the opposite result as the original
104 | *
105 | * @return the resulting BStream after the not operation
106 | */
107 | public default BStream not() {
108 | return () -> !get();
109 | }
110 |
111 | /** @return a WPILib Trigger that is pressed when this class is true */
112 | public default Trigger toButton() {
113 | return new Trigger(this);
114 | }
115 | }
116 |
--------------------------------------------------------------------------------
/src/com/stuypulse/stuylib/streams/booleans/FilteredBStream.java:
--------------------------------------------------------------------------------
1 | /* Copyright (c) 2025 StuyPulse Robotics. All rights reserved. */
2 | /* This work is licensed under the terms of the MIT license */
3 | /* found in the root directory of this project. */
4 |
5 | package com.stuypulse.stuylib.streams.booleans;
6 |
7 | import com.stuypulse.stuylib.streams.booleans.filters.BFilter;
8 |
9 | /**
10 | * A FilteredBStream is similar to a FilteredIStream.
11 | *
12 | * It works like a BStream, but every time you call .get(), it runs the value through the filters
13 | * you provided it.
14 | *
15 | * @author Sam (sam.belliveau@gmail.com)
16 | */
17 | public class FilteredBStream implements BStream {
18 |
19 | private BStream mStream; // Stream used
20 | private BFilter mStreamFilter; // StreamFilter used
21 |
22 | /**
23 | * Makes filtered stream from stream and stream filter
24 | *
25 | * @param stream input stream
26 | * @param filter stream filter
27 | */
28 | public FilteredBStream(BStream stream, BFilter... filter) {
29 | mStream = stream;
30 | mStreamFilter = BFilter.create(filter);
31 | }
32 |
33 | /**
34 | * Get next value from filtered stream
35 | *
36 | * @return next value
37 | */
38 | public boolean get() {
39 | return mStreamFilter.get(mStream.get());
40 | }
41 | }
42 |
--------------------------------------------------------------------------------
/src/com/stuypulse/stuylib/streams/booleans/PollingBStream.java:
--------------------------------------------------------------------------------
1 | /* Copyright (c) 2025 StuyPulse Robotics. All rights reserved. */
2 | /* This work is licensed under the terms of the MIT license */
3 | /* found in the root directory of this project. */
4 |
5 | package com.stuypulse.stuylib.streams.booleans;
6 |
7 | import edu.wpi.first.wpilibj.Notifier;
8 |
9 | /**
10 | * A PollingBStream is a BStream but its .get() method is called for you at a certain rate. This is
11 | * really helpful when you want to read from a sensor using a debouncer, but you dont always call
12 | * .get() in your periodic loop. This will do that for you, and give you the most recent result. It
13 | * is not recommended to filter a PollingBStream as you should ideally filter before you poll.
14 | *
15 | * @author Sam (sam.belliveau@gmail.com)
16 | */
17 | public class PollingBStream implements BStream {
18 |
19 | private Notifier mPoller;
20 | private volatile boolean mResult;
21 |
22 | /**
23 | * Creates a PollingBStream from an BStream and a time value
24 | *
25 | * @param stream BStream to poll from
26 | * @param dt time inbetween each poll
27 | */
28 | public PollingBStream(BStream stream, double dt) {
29 | if (dt <= 0) {
30 | throw new IllegalArgumentException("dt must be greater than 0");
31 | }
32 |
33 | mResult = false;
34 | mPoller = new Notifier(() -> mResult = stream.get());
35 | mPoller.startPeriodic(dt);
36 | }
37 |
38 | public boolean get() {
39 | return mResult;
40 | }
41 |
42 | protected void finalize() {
43 | close();
44 | }
45 |
46 | public void close() {
47 | mPoller.close();
48 | mResult = false;
49 | }
50 | }
51 |
--------------------------------------------------------------------------------
/src/com/stuypulse/stuylib/streams/booleans/filters/BButton.java:
--------------------------------------------------------------------------------
1 | /* Copyright (c) 2025 StuyPulse Robotics. All rights reserved. */
2 | /* This work is licensed under the terms of the MIT license */
3 | /* found in the root directory of this project. */
4 |
5 | package com.stuypulse.stuylib.streams.booleans.filters;
6 |
7 | /**
8 | * A simple boolean filter that returns true when a boolean stream changes depending on the type.
9 | *
10 | * 1. Both - will return true if the BStream changes at all
11 | *
12 | * 2. Pressed - will return true if the BStream changes from false to true
13 | *
14 | * 3. Released - will return true if the BStream changes from true to false
15 | *
16 | * @author Sam (sam.belliveau@gmail.com)
17 | */
18 | public interface BButton extends BFilter {
19 |
20 | public static class Both implements BButton {
21 | private boolean mLastValue;
22 |
23 | public Both() {
24 | mLastValue = false;
25 | }
26 |
27 | public boolean get(boolean next) {
28 | if (next == mLastValue) {
29 | return false;
30 | } else {
31 | mLastValue = next;
32 | return true;
33 | }
34 | }
35 | }
36 |
37 | public static class Pressed implements BButton {
38 | private boolean mLastValue;
39 |
40 | public Pressed() {
41 | mLastValue = false;
42 | }
43 |
44 | public boolean get(boolean next) {
45 | if (next == mLastValue) {
46 | return false;
47 | } else {
48 | return (mLastValue = next) == true;
49 | }
50 | }
51 | }
52 |
53 | public static class Released implements BButton {
54 | private boolean mLastValue;
55 |
56 | public Released() {
57 | mLastValue = false;
58 | }
59 |
60 | public boolean get(boolean next) {
61 | if (next == mLastValue) {
62 | return false;
63 | } else {
64 | return (mLastValue = next) == false;
65 | }
66 | }
67 | }
68 | }
69 |
--------------------------------------------------------------------------------
/src/com/stuypulse/stuylib/streams/booleans/filters/BButtonRC.java:
--------------------------------------------------------------------------------
1 | /* Copyright (c) 2025 StuyPulse Robotics. All rights reserved. */
2 | /* This work is licensed under the terms of the MIT license */
3 | /* found in the root directory of this project. */
4 |
5 | package com.stuypulse.stuylib.streams.booleans.filters;
6 |
7 | import com.stuypulse.stuylib.streams.numbers.filters.HighPassFilter;
8 |
9 | /**
10 | * A simple boolean filter that returns true when a boolean stream changes depending on the type.
11 | * This type of button filter will hold true for a certain decay length before returning to false.
12 | *
13 | * 1. Both - will return true if the BStream changes at all
14 | *
15 | * 2. Pressed - will return true if the BStream changes from false to true
16 | *
17 | * 3. Released - will return true if the BStream changes from true to false
18 | *
19 | * @author Sam (sam.belliveau@gmail.com)
20 | */
21 | public interface BButtonRC extends BFilter {
22 |
23 | public static final double kTrue = 1.0;
24 | public static final double kFalse = 0.0;
25 |
26 | public static final double kPressedLimit = 1.0 / Math.E;
27 | public static final double kReleasedLimit = -kPressedLimit;
28 |
29 | public static class Both implements BButtonRC {
30 | private HighPassFilter mFilter;
31 |
32 | /** @param decay time in seconds to remain true after the BStream is pressed or released */
33 | public Both(Number decay) {
34 | mFilter = new HighPassFilter(decay);
35 | }
36 |
37 | public boolean get(boolean next) {
38 | double value = mFilter.get(next ? kTrue : kFalse);
39 | return value > kPressedLimit || value < kReleasedLimit;
40 | }
41 | }
42 |
43 | public static class Pressed implements BButtonRC {
44 | private HighPassFilter mFilter;
45 |
46 | /** @param decay time in seconds to remain true after the BStream has been pressed */
47 | public Pressed(Number decay) {
48 | mFilter = new HighPassFilter(decay);
49 | }
50 |
51 | public boolean get(boolean next) {
52 | return mFilter.get(next ? kTrue : kFalse) > kPressedLimit;
53 | }
54 | }
55 |
56 | public static class Released implements BButtonRC {
57 | private HighPassFilter mFilter;
58 |
59 | /** @param decay time in seconds to remain true after the BStream has been released */
60 | public Released(Number decay) {
61 | mFilter = new HighPassFilter(decay);
62 | }
63 |
64 | public boolean get(boolean next) {
65 | return mFilter.get(next ? kTrue : kFalse) < kReleasedLimit;
66 | }
67 | }
68 | }
69 |
--------------------------------------------------------------------------------
/src/com/stuypulse/stuylib/streams/booleans/filters/BDebounce.java:
--------------------------------------------------------------------------------
1 | /* Copyright (c) 2025 StuyPulse Robotics. All rights reserved. */
2 | /* This work is licensed under the terms of the MIT license */
3 | /* found in the root directory of this project. */
4 |
5 | package com.stuypulse.stuylib.streams.booleans.filters;
6 |
7 | import com.stuypulse.stuylib.util.StopWatch;
8 |
9 | /**
10 | * A collection of Debounce Classes in 3 flavors.
11 | *
12 | * 1. Rising - BStream must be true for x seconds
13 | *
14 | * 2. Falling - BStream must be false for x seconds
15 | *
16 | * 3. Both - BStream must remain constant for x seconds to change
17 | *
18 | * @author Sam (sam.belliveau@gmail.com)
19 | */
20 | public interface BDebounce extends BFilter {
21 |
22 | /**
23 | * A Rising Debounce Filter.
24 | *
25 | * The input BStream must remain true for debounceTime before this returns true
26 | */
27 | public static class Rising implements BDebounce {
28 |
29 | private final StopWatch mTimer;
30 | private final Number mDebounceTime;
31 |
32 | /** @param debounceTime amount of time the BStream must remain true before returning true */
33 | public Rising(Number debounceTime) {
34 | mTimer = new StopWatch();
35 | mDebounceTime = debounceTime;
36 | }
37 |
38 | public boolean get(boolean next) {
39 | if (next == false) {
40 | mTimer.reset();
41 | return false;
42 | }
43 |
44 | return (mDebounceTime.doubleValue() < mTimer.getTime()) == true;
45 | }
46 | }
47 |
48 | /**
49 | * A Falling Debounce Filter.
50 | *
51 | * The input BStream must remain false for debounceTime before this returns false
52 | */
53 | public static class Falling implements BDebounce {
54 |
55 | private final StopWatch mTimer;
56 | private final Number mDebounceTime;
57 |
58 | /**
59 | * @param debounceTime amount of time the BStream must remain false before returning false
60 | */
61 | public Falling(Number debounceTime) {
62 | mTimer = new StopWatch();
63 | mDebounceTime = debounceTime;
64 | }
65 |
66 | public boolean get(boolean next) {
67 | if (next == true) {
68 | mTimer.reset();
69 | return true;
70 | }
71 |
72 | return (mDebounceTime.doubleValue() < mTimer.getTime()) == false;
73 | }
74 | }
75 |
76 | /**
77 | * A General Debounce Filter.
78 | *
79 | * The input BStream must remain the constant for debounceTime before it changes the value it
80 | * returns
81 | */
82 | public static class Both implements BDebounce {
83 |
84 | private final StopWatch mTimer;
85 | private final Number mDebounceTime;
86 | private boolean mLastValue;
87 |
88 | /** @param debounceTime amount of time the BStream must remain constant before changing */
89 | public Both(Number debounceTime) {
90 | mTimer = new StopWatch();
91 | mDebounceTime = debounceTime;
92 | mLastValue = false;
93 | }
94 |
95 | public boolean get(boolean next) {
96 | if (next == mLastValue) {
97 | mTimer.reset();
98 | return mLastValue;
99 | } else if (mDebounceTime.doubleValue() < mTimer.getTime()) {
100 | mTimer.reset();
101 | return mLastValue = next;
102 | } else {
103 | return mLastValue;
104 | }
105 | }
106 | }
107 | }
108 |
--------------------------------------------------------------------------------
/src/com/stuypulse/stuylib/streams/booleans/filters/BDebounceRC.java:
--------------------------------------------------------------------------------
1 | /* Copyright (c) 2025 StuyPulse Robotics. All rights reserved. */
2 | /* This work is licensed under the terms of the MIT license */
3 | /* found in the root directory of this project. */
4 |
5 | package com.stuypulse.stuylib.streams.booleans.filters;
6 |
7 | import com.stuypulse.stuylib.streams.numbers.filters.LowPassFilter;
8 |
9 | /**
10 | * An RC Debounce class takes the average of the past few boolean values to remove noise.
11 | * Recommended to be called very often using a PollingBStream.
12 | *
13 | * @author Sam (sam.belliveau@gmail.com)
14 | */
15 | public interface BDebounceRC extends BFilter {
16 |
17 | public static final double kTrue = 1.0;
18 | public static final double kFalse = 0.0;
19 |
20 | public static final double kLowerLimit = 1.0 / Math.E;
21 | public static final double kUpperLimit = 1.0 - kLowerLimit;
22 |
23 | /** An RC Debouncer that has a true bias */
24 | public static class Rising implements BDebounceRC {
25 | private final LowPassFilter mFilter;
26 |
27 | /** @param debounceTime amount of time on average to go from false to true */
28 | public Rising(Number debounceTime) {
29 | mFilter = new LowPassFilter(debounceTime);
30 | }
31 |
32 | public boolean get(boolean next) {
33 | return mFilter.get(next ? kTrue : kFalse) > kUpperLimit;
34 | }
35 | }
36 |
37 | /** An RC Debouncer that has a false bias */
38 | public static class Falling implements BDebounceRC {
39 | private final LowPassFilter mFilter;
40 |
41 | /** @param debounceTime amount of time on average to go from true to false */
42 | public Falling(Number debounceTime) {
43 | mFilter = new LowPassFilter(debounceTime);
44 | }
45 |
46 | public boolean get(boolean next) {
47 | return mFilter.get(next ? kTrue : kFalse) > kLowerLimit;
48 | }
49 | }
50 |
51 | /** An RC Debouncer that has a bias towards the previous value */
52 | public static class Both implements BDebounceRC {
53 |
54 | private final LowPassFilter mFilter;
55 | private boolean mPrev;
56 |
57 | /** @param debounceTime amount of time on average to change value */
58 | public Both(Number debounceTime) {
59 | mFilter = new LowPassFilter(debounceTime);
60 | mPrev = false;
61 | }
62 |
63 | public boolean get(boolean next) {
64 | return mPrev = mFilter.get(next ? kTrue : kFalse) > (mPrev ? kLowerLimit : kUpperLimit);
65 | }
66 | }
67 | }
68 |
--------------------------------------------------------------------------------
/src/com/stuypulse/stuylib/streams/booleans/filters/BFilter.java:
--------------------------------------------------------------------------------
1 | /* Copyright (c) 2025 StuyPulse Robotics. All rights reserved. */
2 | /* This work is licensed under the terms of the MIT license */
3 | /* found in the root directory of this project. */
4 |
5 | package com.stuypulse.stuylib.streams.booleans.filters;
6 |
7 | /**
8 | * This is the BFilter interface class that gives a definition for how to implement a filter.
9 | *
10 | * All that a BFilter does is take in the next boolean in a stream and gives you the filtered
11 | * value.
12 | *
13 | * @author Sam (sam.belliveau@gmail.com)
14 | */
15 | public interface BFilter {
16 |
17 | /**
18 | * Create a BFilter from a list of BFilters. This will create a BFilter with the least possible
19 | * overhead for the number of filters provided.
20 | *
21 | * @param filters list of BFilters to apply
22 | * @return an BFilter that uses every filter given
23 | */
24 | public static BFilter create(BFilter... filters) {
25 | switch (filters.length) {
26 | case 0:
27 | return x -> x;
28 | case 1:
29 | return filters[0];
30 | case 2:
31 | return x -> filters[1].get(filters[0].get(x));
32 | case 3:
33 | return x -> filters[2].get(filters[1].get(filters[0].get(x)));
34 | case 4:
35 | return x -> filters[3].get(filters[2].get(filters[1].get(filters[0].get(x))));
36 | default:
37 | return new BFilterGroup(filters);
38 | }
39 | }
40 |
41 | /**
42 | * Get next value in Filter based on the next value given
43 | *
44 | * @param next next input value in the stream
45 | * @return the output value of the filter
46 | */
47 | public boolean get(boolean next);
48 |
49 | /**
50 | * Combine an BFilter with another BFilter
51 | *
52 | * @param next filter to be evaluated after this one
53 | * @return the combined filter
54 | */
55 | public default BFilter then(BFilter next) {
56 | return x -> next.get(get(x));
57 | }
58 |
59 | /**
60 | * Combine two BFilters by and'ing their results together
61 | *
62 | * @param other other BFilter to and with this one
63 | * @return the resulting BFilter after the and
64 | */
65 | public default BFilter and(BFilter other) {
66 | return x -> get(x) & other.get(x);
67 | }
68 |
69 | /**
70 | * Combine two BFilters by or'ing their results together
71 | *
72 | * @param other other BFilter to or with this one
73 | * @return the resulting BFilter after the or
74 | */
75 | public default BFilter or(BFilter other) {
76 | return x -> get(x) | other.get(x);
77 | }
78 |
79 | /**
80 | * Combine two BFilters by xor'ing their results together
81 | *
82 | * @param other other BFilter to xor with this one
83 | * @return the resulting BFilter after the xor
84 | */
85 | public default BFilter xor(BFilter other) {
86 | return x -> get(x) ^ other.get(x);
87 | }
88 | }
89 |
--------------------------------------------------------------------------------
/src/com/stuypulse/stuylib/streams/booleans/filters/BFilterGroup.java:
--------------------------------------------------------------------------------
1 | /* Copyright (c) 2025 StuyPulse Robotics. All rights reserved. */
2 | /* This work is licensed under the terms of the MIT license */
3 | /* found in the root directory of this project. */
4 |
5 | package com.stuypulse.stuylib.streams.booleans.filters;
6 |
7 | /**
8 | * A class that lets you combine multiple boolean filters into one boolean filter.
9 | *
10 | * @author Sam (sam.belliveau@gmail.com)
11 | */
12 | public class BFilterGroup implements BFilter {
13 |
14 | // Array of all the filters
15 | private final BFilter[] mFilters;
16 |
17 | /**
18 | * Make BFilterGroup out of an array of Filters
19 | *
20 | * @param filters list of filters to combine
21 | */
22 | public BFilterGroup(BFilter... filters) {
23 | mFilters = filters;
24 | }
25 |
26 | public boolean get(boolean next) {
27 | // Put next through each of the filters
28 | for (BFilter filter : mFilters) {
29 | next = filter.get(next);
30 | }
31 |
32 | // Return filtered value
33 | return next;
34 | }
35 | }
36 |
--------------------------------------------------------------------------------
/src/com/stuypulse/stuylib/streams/numbers/FilteredIStream.java:
--------------------------------------------------------------------------------
1 | /* Copyright (c) 2025 StuyPulse Robotics. All rights reserved. */
2 | /* This work is licensed under the terms of the MIT license */
3 | /* found in the root directory of this project. */
4 |
5 | package com.stuypulse.stuylib.streams.numbers;
6 |
7 | import com.stuypulse.stuylib.streams.numbers.filters.IFilter;
8 |
9 | /**
10 | * Takes an {@link IStream} and a {@link IFilter} and makes a {@link FilteredIStream}
11 | *
12 | * This could be used to automatically filter controller inputs
13 | *
14 | * @author Sam (sam.belliveau@gmail.com)
15 | */
16 | public class FilteredIStream implements IStream {
17 |
18 | private IStream mStream; // Stream used
19 | private IFilter mStreamFilter; // StreamFilter used
20 |
21 | /**
22 | * Makes filtered stream from stream and stream filter
23 | *
24 | * @param stream input stream
25 | * @param filter stream filter
26 | */
27 | public FilteredIStream(IStream stream, IFilter... filter) {
28 | mStream = stream;
29 | mStreamFilter = IFilter.create(filter);
30 | }
31 |
32 | /**
33 | * Get next value from filtered stream
34 | *
35 | * @return next value
36 | */
37 | public double get() {
38 | return mStreamFilter.get(mStream.get());
39 | }
40 | }
41 |
--------------------------------------------------------------------------------
/src/com/stuypulse/stuylib/streams/numbers/IStream.java:
--------------------------------------------------------------------------------
1 | /* Copyright (c) 2025 StuyPulse Robotics. All rights reserved. */
2 | /* This work is licensed under the terms of the MIT license */
3 | /* found in the root directory of this project. */
4 |
5 | package com.stuypulse.stuylib.streams.numbers;
6 |
7 | import com.stuypulse.stuylib.streams.angles.AStream;
8 | import com.stuypulse.stuylib.streams.booleans.BStream;
9 | import com.stuypulse.stuylib.streams.numbers.filters.IFilter;
10 |
11 | import java.util.function.DoubleSupplier;
12 |
13 | /**
14 | * A stream of doubles that is accessed with the {@link IStream#get()} function
15 | *
16 | * Can be created with lambdas to represent a stream of inputs.
17 | *
18 | * It can be filitered with the available filtering options.
19 | *
20 | * @author Sam (sam.belliveau@gmail.com)
21 | */
22 | public interface IStream extends DoubleSupplier {
23 |
24 | /**
25 | * Create an IStream from another IStream. This is helpful if you want to use some of the
26 | * decorator functions with a lambda.
27 | *
28 | * @param stream stream to create IStream from
29 | * @return the resulting IStream
30 | */
31 | public static IStream create(IStream stream) {
32 | return stream;
33 | }
34 |
35 | /**
36 | * Create an IStream from another AStream. This will convert the angle to a double in radians.
37 | *
38 | * @param stream stream to create IStream from
39 | * @return the resulting IStream
40 | */
41 | public static IStream create(AStream stream) {
42 | return () -> stream.get().toRadians();
43 | }
44 |
45 | /**
46 | * Create a IStream from another BStream. This will check if the amplitude is above a certain
47 | * threshold.
48 | *
49 | * @param stream stream to create IStream from
50 | * @return the resulting IStream
51 | */
52 | public static IStream create(BStream stream) {
53 | return () -> stream.get() ? 1.0 : 0.0;
54 | }
55 |
56 | /** @return next value in the stream */
57 | public double get();
58 |
59 | /** @return get IStream as a double */
60 | public default double getAsDouble() {
61 | return get();
62 | }
63 |
64 | /**
65 | * Create a new FilteredIStream from the current stream
66 | *
67 | * @param filters the filters you want to apply to the IStream
68 | * @return The FilteredIStream
69 | */
70 | public default FilteredIStream filtered(IFilter... filters) {
71 | return new FilteredIStream(this, filters);
72 | }
73 |
74 | /**
75 | * Create a new PollingIStream from the current stream
76 | *
77 | * @param dt the time inbetween each poll of the IStream
78 | * @return The PollingIStream
79 | */
80 | public default PollingIStream polling(double dt) {
81 | return new PollingIStream(this, dt);
82 | }
83 |
84 | /**
85 | * Combine two IStreams by adding their results together
86 | *
87 | * @param other other IStream to add to this one
88 | * @return the resulting IStream after the sum
89 | */
90 | public default IStream add(IStream other) {
91 | return () -> get() + other.get();
92 | }
93 |
94 | /**
95 | * Combine two IStreams by subtracting their results together
96 | *
97 | * @param other other IStream to subtract from this one
98 | * @return the resulting IStream after the subtraction
99 | */
100 | public default IStream sub(IStream other) {
101 | return () -> get() - other.get();
102 | }
103 |
104 | /**
105 | * Casts an IStream to a Number
106 | *
107 | * @return a Number that reads from this stream
108 | */
109 | public default NumberStream number() {
110 | return new NumberStream(this);
111 | }
112 | }
113 |
--------------------------------------------------------------------------------
/src/com/stuypulse/stuylib/streams/numbers/NumberStream.java:
--------------------------------------------------------------------------------
1 | /* Copyright (c) 2025 StuyPulse Robotics. All rights reserved. */
2 | /* This work is licensed under the terms of the MIT license */
3 | /* found in the root directory of this project. */
4 |
5 | package com.stuypulse.stuylib.streams.numbers;
6 |
7 | /**
8 | * A number which reads from a streams.
9 | *
10 | * Useful for filtering streams (including SmartNumbers) and converting them back into Numbers.
11 | *
12 | * @author Myles Pasetsky (myles.pasetsky@gmail.com)
13 | */
14 | public class NumberStream extends Number implements IStream {
15 |
16 | /** the underlying stream */
17 | private final IStream mStream;
18 |
19 | /**
20 | * Create a number stream given a stream
21 | *
22 | * @param stream IStream to return through number classes
23 | */
24 | public NumberStream(IStream stream) {
25 | mStream = stream;
26 | }
27 |
28 | /** @return value from stream */
29 | @Override
30 | public double get() {
31 | return mStream.get();
32 | }
33 |
34 | /** @return value from stream */
35 | @Override
36 | public double doubleValue() {
37 | return get();
38 | }
39 |
40 | /** @return value from stream as float */
41 | @Override
42 | public float floatValue() {
43 | return (float) get();
44 | }
45 |
46 | /** @return value from stream as integer */
47 | @Override
48 | public int intValue() {
49 | return (int) get();
50 | }
51 |
52 | /** @return value from stream as long */
53 | @Override
54 | public long longValue() {
55 | return (long) get();
56 | }
57 | }
58 |
--------------------------------------------------------------------------------
/src/com/stuypulse/stuylib/streams/numbers/PollingIStream.java:
--------------------------------------------------------------------------------
1 | /* Copyright (c) 2025 StuyPulse Robotics. All rights reserved. */
2 | /* This work is licensed under the terms of the MIT license */
3 | /* found in the root directory of this project. */
4 |
5 | package com.stuypulse.stuylib.streams.numbers;
6 |
7 | import edu.wpi.first.wpilibj.Notifier;
8 |
9 | /**
10 | * A PollingIStream calls {@link IStream#get()} every x milliseconds instead of when the user calls
11 | * get
12 | *
13 | * @author Sam (sam.belliveau@gmail.com)
14 | */
15 | public class PollingIStream implements IStream, AutoCloseable {
16 |
17 | private Notifier mPoller;
18 | private volatile double mResult;
19 |
20 | /**
21 | * Creates a PollingIStream from an IStream and a time value
22 | *
23 | * @param stream istream to poll from
24 | * @param dt Number of calls per second
25 | */
26 | public PollingIStream(IStream stream, double dt) {
27 | if (dt <= 0) {
28 | throw new IllegalArgumentException("dt must be greater than 0");
29 | }
30 |
31 | mResult = 0.0;
32 | mPoller = new Notifier(() -> mResult = stream.get());
33 | mPoller.startPeriodic(dt);
34 | }
35 |
36 | public double get() {
37 | return mResult;
38 | }
39 |
40 | protected void finalize() {
41 | close();
42 | }
43 |
44 | public void close() {
45 | mPoller.close();
46 | mResult = 0.0;
47 | }
48 | }
49 |
--------------------------------------------------------------------------------
/src/com/stuypulse/stuylib/streams/numbers/filters/Derivative.java:
--------------------------------------------------------------------------------
1 | /* Copyright (c) 2025 StuyPulse Robotics. All rights reserved. */
2 | /* This work is licensed under the terms of the MIT license */
3 | /* found in the root directory of this project. */
4 |
5 | package com.stuypulse.stuylib.streams.numbers.filters;
6 |
7 | import com.stuypulse.stuylib.util.StopWatch;
8 |
9 | /**
10 | * This class takes an IStream and gives you the derivative with respect to time.
11 | *
12 | * @author Sam (sam.belliveau@gmail.com)
13 | */
14 | public class Derivative implements IFilter {
15 |
16 | private final StopWatch mTimer;
17 | private double mLastValue;
18 |
19 | public Derivative() {
20 | mTimer = new StopWatch();
21 | mLastValue = 0.0;
22 | }
23 |
24 | public double get(double next) {
25 | double diff = next - mLastValue;
26 | mLastValue = next;
27 | return diff / mTimer.reset();
28 | }
29 | }
30 |
--------------------------------------------------------------------------------
/src/com/stuypulse/stuylib/streams/numbers/filters/HighPassFilter.java:
--------------------------------------------------------------------------------
1 | /* Copyright (c) 2025 StuyPulse Robotics. All rights reserved. */
2 | /* This work is licensed under the terms of the MIT license */
3 | /* found in the root directory of this project. */
4 |
5 | package com.stuypulse.stuylib.streams.numbers.filters;
6 |
7 | /**
8 | * Implementation for of a real time IIR HighPassFilter
9 | *
10 | * @author Sam (sam.belliveau@gmail.com)
11 | */
12 | public class HighPassFilter implements IFilter {
13 |
14 | private LowPassFilter mInverse;
15 |
16 | /** @param rc time constant for high pass filter */
17 | public HighPassFilter(Number rc) {
18 | mInverse = new LowPassFilter(rc);
19 | }
20 |
21 | public double get(double next) {
22 | return next - mInverse.get(next);
23 | }
24 | }
25 |
--------------------------------------------------------------------------------
/src/com/stuypulse/stuylib/streams/numbers/filters/IFilter.java:
--------------------------------------------------------------------------------
1 | /* Copyright (c) 2025 StuyPulse Robotics. All rights reserved. */
2 | /* This work is licensed under the terms of the MIT license */
3 | /* found in the root directory of this project. */
4 |
5 | package com.stuypulse.stuylib.streams.numbers.filters;
6 |
7 | /**
8 | * This is the Filter interface class that gives a definition for how to implement a filter.
9 | *
10 | * All that a filter does is take in the next double in a series and gives you the filtered
11 | * value.
12 | *
13 | * @author Sam (sam.belliveau@gmail.com)
14 | */
15 | public interface IFilter {
16 |
17 | /**
18 | * Create an IFilter from a list of IFilters. This will create an IFilter with the least
19 | * possible overhead for the number of filters provided.
20 | *
21 | * @param filters list of IFilters to apply
22 | * @return an IFilter that uses every filter given
23 | */
24 | public static IFilter create(IFilter... filters) {
25 | switch (filters.length) {
26 | case 0:
27 | return x -> x;
28 | case 1:
29 | return filters[0];
30 | case 2:
31 | return x -> filters[1].get(filters[0].get(x));
32 | case 3:
33 | return x -> filters[2].get(filters[1].get(filters[0].get(x)));
34 | case 4:
35 | return x -> filters[3].get(filters[2].get(filters[1].get(filters[0].get(x))));
36 | default:
37 | return new IFilterGroup(filters);
38 | }
39 | }
40 |
41 | /**
42 | * Get next value in Filter based on the next value given
43 | *
44 | * @param next next input value in the stream
45 | * @return the output value of the filter
46 | */
47 | public double get(double next);
48 |
49 | /**
50 | * Combine an IFilter with another IFilter
51 | *
52 | * @param next filter to be evaluated after this one
53 | * @return the combined filter
54 | */
55 | public default IFilter then(IFilter next) {
56 | return x -> next.get(get(x));
57 | }
58 |
59 | /**
60 | * Combine two IFilters by adding their results together
61 | *
62 | * @param other other IFilter to add to this one
63 | * @return the resulting IFilter after the sum
64 | */
65 | public default IFilter add(IFilter other) {
66 | return x -> get(x) + other.get(x);
67 | }
68 |
69 | /**
70 | * Combine two IFilters by subtracting their results together
71 | *
72 | * @param other other IFilter to subtract from this one
73 | * @return the resulting IFilter after the subtraction
74 | */
75 | public default IFilter sub(IFilter other) {
76 | return x -> get(x) - other.get(x);
77 | }
78 |
79 | /**
80 | * Invert an IFilter by subtracting the input from the result of the IFilter.
81 | *
82 | * Inverting a LowPassFilter gives you a HighPassFilter and vise versa.
83 | *
84 | * Inverting something twice gives you the original value.
85 | *
86 | * @return the inverted filter
87 | */
88 | public default IFilter invert() {
89 | return x -> x - get(x);
90 | }
91 | }
92 |
--------------------------------------------------------------------------------
/src/com/stuypulse/stuylib/streams/numbers/filters/IFilterGroup.java:
--------------------------------------------------------------------------------
1 | /* Copyright (c) 2025 StuyPulse Robotics. All rights reserved. */
2 | /* This work is licensed under the terms of the MIT license */
3 | /* found in the root directory of this project. */
4 |
5 | package com.stuypulse.stuylib.streams.numbers.filters;
6 |
7 | /**
8 | * A class that lets you combine multiple stream filters into one stream filter
9 | *
10 | * @author Sam (sam.belliveau@gmail.com)
11 | */
12 | public class IFilterGroup implements IFilter {
13 |
14 | // Array of all the filters
15 | private final IFilter[] mFilters;
16 |
17 | /**
18 | * Make IFilterGroup out of an array of Filters
19 | *
20 | * @param filters list of filters to combine
21 | */
22 | public IFilterGroup(IFilter... filters) {
23 | mFilters = filters;
24 | }
25 |
26 | public double get(double next) {
27 | // Put next through each of the filters
28 | for (IFilter filter : mFilters) {
29 | next = filter.get(next);
30 | }
31 |
32 | // Return filtered value
33 | return next;
34 | }
35 | }
36 |
--------------------------------------------------------------------------------
/src/com/stuypulse/stuylib/streams/numbers/filters/LowPassFilter.java:
--------------------------------------------------------------------------------
1 | /* Copyright (c) 2025 StuyPulse Robotics. All rights reserved. */
2 | /* This work is licensed under the terms of the MIT license */
3 | /* found in the root directory of this project. */
4 |
5 | package com.stuypulse.stuylib.streams.numbers.filters;
6 |
7 | import com.stuypulse.stuylib.util.StopWatch;
8 |
9 | /**
10 | * Implementation of a real time IIR LowPassFilter
11 | *
12 | * @author Sam (sam.belliveau@gmail.com)
13 | */
14 | public class LowPassFilter implements IFilter {
15 |
16 | // Used to calculate time in between calls
17 | private final StopWatch mTimer;
18 |
19 | // Used to calculate next value based on previous value and time
20 | private final Number mRC;
21 | private double mLastValue;
22 |
23 | /**
24 | * @param rc Time Constant. The time constant is the amount of time in seconds that it takes to
25 | * get 63.2% of the way to the target value. 63.2% is (1 - (1 / e)).
26 | */
27 | public LowPassFilter(Number rc) {
28 | mTimer = new StopWatch();
29 | mLastValue = 0;
30 | mRC = rc;
31 | }
32 |
33 | public double get(double next) {
34 | // If RC will cause errors, disable filter
35 | if (mRC.doubleValue() <= 0.0) {
36 | return mLastValue = next;
37 | }
38 |
39 | // Get a constant, which is determined based on dt and the mRC constant
40 | double a = 1.0 - Math.exp(-mTimer.reset() / mRC.doubleValue());
41 |
42 | // Based on the value of a (which is determined by dt), the next value
43 | // could either change a lot, or not by much. (smaller dt = smaller change)
44 | return mLastValue += a * (next - mLastValue);
45 | }
46 | }
47 |
--------------------------------------------------------------------------------
/src/com/stuypulse/stuylib/streams/numbers/filters/MotionProfile.java:
--------------------------------------------------------------------------------
1 | /* Copyright (c) 2025 StuyPulse Robotics. All rights reserved. */
2 | /* This work is licensed under the terms of the MIT license */
3 | /* found in the root directory of this project. */
4 |
5 | package com.stuypulse.stuylib.streams.numbers.filters;
6 |
7 | import com.stuypulse.stuylib.math.SLMath;
8 | import com.stuypulse.stuylib.util.StopWatch;
9 |
10 | /**
11 | * A filter, that when applied to the input of a motor, will profile it. Similar to the way in which
12 | * motion profiling can limit the amount of acceleration and jerk in an S-Curve, this can do that to
13 | * real time input. Because this will add a delay, it is recommended that the accelLimit is as high
14 | * as possible. Aside from the accelLimit, this is identical to a SlewRateLimiter or TimedRateLimit.
15 | *
16 | * @author Sam (sam.belliveau@gmail.com)
17 | */
18 | public class MotionProfile implements IFilter {
19 |
20 | // Default number of times to apply filter (helps accuracy)
21 | private static final int kDefaultSteps = 64;
22 |
23 | // Stopwatch to Track dt
24 | private StopWatch mTimer;
25 |
26 | // Limits for each of the derivatives
27 | private Number mVelLimit;
28 | private Number mAccelLimit;
29 |
30 | // The last output / acceleration
31 | private double mOutput;
32 | private double mAccel;
33 |
34 | // Number of times to apply filter (helps accuracy)
35 | private final int mSteps;
36 |
37 | /**
38 | * @param velLimit maximum change in displacement per second (u/s)
39 | * @param accelLimit maximum change in velocity per second (u/s/s)
40 | * @param steps number of times to apply filter (improves accuracy)
41 | */
42 | public MotionProfile(Number velLimit, Number accelLimit, int steps) {
43 | mTimer = new StopWatch();
44 |
45 | mVelLimit = velLimit;
46 | mAccelLimit = accelLimit;
47 |
48 | mOutput = 0;
49 | mAccel = 0;
50 |
51 | mSteps = steps;
52 | }
53 |
54 | /**
55 | * @param velLimit maximum change in velocity per second (u/s)
56 | * @param accelLimit maximum change in acceleration per second (u/s/s)
57 | */
58 | public MotionProfile(Number velLimit, Number accelLimit) {
59 | this(velLimit, accelLimit, kDefaultSteps);
60 | }
61 |
62 | public double get(double target) {
63 | double dt = mTimer.reset() / mSteps;
64 |
65 | for (int i = 0; i < mSteps; ++i) {
66 | // if there is a jerk limit, limit the amount the acceleration can change
67 | if (0 < mAccelLimit.doubleValue()) {
68 | // amount of windup in system (how long it would take to slow down)
69 | double windup = Math.abs(mAccel) / mAccelLimit.doubleValue();
70 |
71 | // If the windup is too small, just use normal algorithm to limit acceleration
72 | if (windup < dt) {
73 | // Calculate acceleration needed to reach target
74 | double accel = (target - mOutput) / dt - mAccel;
75 |
76 | // Try to reach it while abiding by jerklimit
77 | mAccel += SLMath.clamp(accel, dt * mAccelLimit.doubleValue());
78 | } else {
79 | // the position it would end up if it attempted to come to a full stop
80 | double windA = 0.5 * mAccel * (dt + windup); // windup caused by acceleration
81 | double future = mOutput + windA; // where the robot will end up
82 |
83 | // Calculate acceleration needed to come to stop at target throughout windup
84 | double accel = (target - future) / windup;
85 |
86 | // Try to reach it while abiding by jerklimit
87 | mAccel += SLMath.clamp(accel, dt * mAccelLimit.doubleValue());
88 | }
89 |
90 | } else {
91 | // make the acceleration the difference between target and current
92 | mAccel = (target - mOutput) / dt;
93 | }
94 |
95 | // if there is an acceleration limit, limit the acceleration
96 | if (0 < mVelLimit.doubleValue()) {
97 | mAccel = SLMath.clamp(mAccel, mVelLimit.doubleValue());
98 | }
99 |
100 | // adjust output by calculated acceleration
101 | mOutput += dt * mAccel;
102 | }
103 |
104 | return mOutput;
105 | }
106 |
107 | public void reset(double position) {
108 | mOutput = position;
109 | mAccel = 0;
110 | }
111 | }
112 |
--------------------------------------------------------------------------------
/src/com/stuypulse/stuylib/streams/numbers/filters/RateLimit.java:
--------------------------------------------------------------------------------
1 | /* Copyright (c) 2025 StuyPulse Robotics. All rights reserved. */
2 | /* This work is licensed under the terms of the MIT license */
3 | /* found in the root directory of this project. */
4 |
5 | package com.stuypulse.stuylib.streams.numbers.filters;
6 |
7 | import com.stuypulse.stuylib.math.SLMath;
8 | import com.stuypulse.stuylib.util.StopWatch;
9 |
10 | /**
11 | * This class lets you rate limit a stream of inputs
12 | *
13 | * Instead of being based on the rate that update is called, the value you give it is based on
14 | * how much it should be able to change in one second.
15 | *
16 | * @author Sam (sam.belliveau@gmail.com)
17 | */
18 | public class RateLimit implements IFilter {
19 |
20 | // Used to get the time since the last get call
21 | private StopWatch mTimer;
22 |
23 | // Used to limit the change from the last value
24 | private double mLastValue;
25 | private Number mRateLimit;
26 |
27 | /** @param rateLimit The amount that the value should be able to change in one second. */
28 | public RateLimit(Number rateLimit) {
29 | if (rateLimit.doubleValue() <= 0) {
30 | throw new IllegalArgumentException("rateLimit must be a positive number");
31 | }
32 |
33 | mTimer = new StopWatch();
34 | mRateLimit = rateLimit;
35 | mLastValue = 0;
36 | }
37 |
38 | public double get(double next) {
39 | return mLastValue +=
40 | SLMath.clamp(next - mLastValue, mRateLimit.doubleValue() * mTimer.reset());
41 | }
42 | }
43 |
--------------------------------------------------------------------------------
/src/com/stuypulse/stuylib/streams/numbers/filters/TimedMovingAverage.java:
--------------------------------------------------------------------------------
1 | /* Copyright (c) 2025 StuyPulse Robotics. All rights reserved. */
2 | /* This work is licensed under the terms of the MIT license */
3 | /* found in the root directory of this project. */
4 |
5 | package com.stuypulse.stuylib.streams.numbers.filters;
6 |
7 | import com.stuypulse.stuylib.util.StopWatch;
8 |
9 | import java.util.LinkedList;
10 |
11 | /**
12 | * A Simple Moving Average where instead of averaging the past x values, you average all the values
13 | * given in the last x seconds.
14 | *
15 | * @author Sam (sam.belliveau@gmail.com)
16 | */
17 | public class TimedMovingAverage implements IFilter {
18 |
19 | /**
20 | * How many removals the moving average can do before it does a complete recount for better
21 | * accuracy.
22 | *
23 | * This is a trade off between speed and accuracy. Having this at 512 means a complete
24 | * recount ~ every 10 seconds on robot code.
25 | */
26 | private static final int kMaxRemovalsBeforeReset = 512;
27 |
28 | /** Class used to store value and time */
29 | private static class Value {
30 |
31 | public double value;
32 | public double time;
33 |
34 | public Value(double value, double time) {
35 | this.value = value;
36 | this.time = time;
37 | }
38 |
39 | public double truncate(double t) {
40 | time -= t;
41 | return value * t;
42 | }
43 |
44 | public double sum() {
45 | return value * time;
46 | }
47 | }
48 |
49 | // All information needed to calculate value
50 | private Number mMaxTime;
51 |
52 | private StopWatch mTimer;
53 |
54 | private LinkedList It works like a VStream, but every time you call .get(), it runs the value through the filters
14 | * you provided it.
15 | *
16 | * @author Sam (sam.belliveau@gmail.com)
17 | */
18 | public class FilteredVStream implements VStream {
19 |
20 | private VStream mStream; // Stream used
21 | private VFilter mStreamFilter; // StreamFilter used
22 |
23 | /**
24 | * Makes filtered stream from stream and stream filter
25 | *
26 | * @param stream input stream
27 | * @param filter stream filter
28 | */
29 | public FilteredVStream(VStream stream, VFilter... filter) {
30 | mStream = stream;
31 | mStreamFilter = VFilter.create(filter);
32 | }
33 |
34 | /**
35 | * Get next value from filtered stream
36 | *
37 | * @return next value
38 | */
39 | public Vector2D get() {
40 | return mStreamFilter.get(mStream.get());
41 | }
42 | }
43 |
--------------------------------------------------------------------------------
/src/com/stuypulse/stuylib/streams/vectors/PollingVStream.java:
--------------------------------------------------------------------------------
1 | /* Copyright (c) 2025 StuyPulse Robotics. All rights reserved. */
2 | /* This work is licensed under the terms of the MIT license */
3 | /* found in the root directory of this project. */
4 |
5 | package com.stuypulse.stuylib.streams.vectors;
6 |
7 | import com.stuypulse.stuylib.math.Vector2D;
8 |
9 | import edu.wpi.first.wpilibj.Notifier;
10 |
11 | /**
12 | * A PollingVStream is a VStream but its .get() method is called for you at a certain rate. This
13 | * contains race conditions.
14 | *
15 | * @author Sam (sam.belliveau@gmail.com)
16 | */
17 | public class PollingVStream implements VStream {
18 |
19 | private Notifier mPoller;
20 | private volatile Vector2D mResult;
21 |
22 | /**
23 | * Creates a PollingVStream from an VStream and a time value
24 | *
25 | * @param stream VStream to poll from
26 | * @param dt time inbetween each poll
27 | */
28 | public PollingVStream(VStream stream, double dt) {
29 | if (dt <= 0) {
30 | throw new IllegalArgumentException("dt must be greater than 0");
31 | }
32 |
33 | mResult = Vector2D.kOrigin;
34 | mPoller = new Notifier(() -> mResult = stream.get());
35 | mPoller.startPeriodic(dt);
36 | }
37 |
38 | public Vector2D get() {
39 | return mResult;
40 | }
41 |
42 | protected void finalize() {
43 | close();
44 | }
45 |
46 | public void close() {
47 | mPoller.close();
48 | mResult = Vector2D.kOrigin;
49 | }
50 | }
51 |
--------------------------------------------------------------------------------
/src/com/stuypulse/stuylib/streams/vectors/VStream.java:
--------------------------------------------------------------------------------
1 | /* Copyright (c) 2025 StuyPulse Robotics. All rights reserved. */
2 | /* This work is licensed under the terms of the MIT license */
3 | /* found in the root directory of this project. */
4 |
5 | package com.stuypulse.stuylib.streams.vectors;
6 |
7 | import com.stuypulse.stuylib.math.Vector2D;
8 | import com.stuypulse.stuylib.streams.angles.AStream;
9 | import com.stuypulse.stuylib.streams.numbers.IStream;
10 | import com.stuypulse.stuylib.streams.vectors.filters.VFilter;
11 |
12 | import java.util.function.Supplier;
13 |
14 | /**
15 | * A VStream is similar to an IStream, but instead of a stream of doubles, it represents a stream of
16 | * Vector2Ds.
17 | *
18 | * @author Sam (sam.belliveau@gmail.com)
19 | */
20 | public interface VStream extends Supplier All that a VFilter does is take in the next vector in a stream and gives you the filtered
14 | * value.
15 | *
16 | * @author Sam (sam.belliveau@gmail.com)
17 | */
18 | public interface VFilter {
19 |
20 | /**
21 | * Create a VFilter from a list of VFilters. This will create a VFilter with the least possible
22 | * overhead for the number of filters provided.
23 | *
24 | * @param filters list of VFilters to apply
25 | * @return an VFilter that uses every filter given
26 | */
27 | public static VFilter create(VFilter... filters) {
28 | switch (filters.length) {
29 | case 0:
30 | return x -> x;
31 | case 1:
32 | return filters[0];
33 | case 2:
34 | return x -> filters[1].get(filters[0].get(x));
35 | case 3:
36 | return x -> filters[2].get(filters[1].get(filters[0].get(x)));
37 | case 4:
38 | return x -> filters[3].get(filters[2].get(filters[1].get(filters[0].get(x))));
39 | default:
40 | return new VFilterGroup(filters);
41 | }
42 | }
43 |
44 | /**
45 | * Create a VFilter by using an IFilter on the x and y components.
46 | *
47 | * @param x IFilter used on x component
48 | * @param y IFilter used on y component
49 | * @return resulting VFilters
50 | */
51 | public static VFilter create(IFilter x, IFilter y) {
52 | return new XYFilter(x, y);
53 | }
54 |
55 | /**
56 | * Get next value in Filter based on the next value given
57 | *
58 | * @param next next input value in the stream
59 | * @return the output value of the filter
60 | */
61 | public Vector2D get(Vector2D next);
62 |
63 | /**
64 | * Combine an VFilter with another VFilter
65 | *
66 | * @param next filter to be evaluated after this one
67 | * @return the combined filter
68 | */
69 | public default VFilter then(VFilter next) {
70 | return x -> next.get(get(x));
71 | }
72 |
73 | /**
74 | * Combine two VFilters by adding their results together
75 | *
76 | * @param other other VFilter to and with this one
77 | * @return the resulting VFilter after the and
78 | */
79 | public default VFilter add(VFilter other) {
80 | return x -> get(x).add(other.get(x));
81 | }
82 |
83 | /**
84 | * Combine two VFilters by subtracting their results together
85 | *
86 | * @param other other VFilter to or with this one
87 | * @return the resulting VFilter after the or
88 | */
89 | public default VFilter sub(VFilter other) {
90 | return x -> get(x).sub(other.get(x));
91 | }
92 | }
93 |
--------------------------------------------------------------------------------
/src/com/stuypulse/stuylib/streams/vectors/filters/VFilterGroup.java:
--------------------------------------------------------------------------------
1 | /* Copyright (c) 2025 StuyPulse Robotics. All rights reserved. */
2 | /* This work is licensed under the terms of the MIT license */
3 | /* found in the root directory of this project. */
4 |
5 | package com.stuypulse.stuylib.streams.vectors.filters;
6 |
7 | import com.stuypulse.stuylib.math.Vector2D;
8 |
9 | /**
10 | * A class that lets you combine multiple vector filters into one vector filter.
11 | *
12 | * @author Sam (sam.belliveau@gmail.com)
13 | */
14 | public class VFilterGroup implements VFilter {
15 |
16 | // Array of all the filters
17 | private final VFilter[] mFilters;
18 |
19 | /**
20 | * Make VFilterGroup out of an array of Filters
21 | *
22 | * @param filters list of filters to combine
23 | */
24 | public VFilterGroup(VFilter... filters) {
25 | mFilters = filters;
26 | }
27 |
28 | public Vector2D get(Vector2D next) {
29 | // Put next through each of the filters
30 | for (VFilter filter : mFilters) {
31 | next = filter.get(next);
32 | }
33 |
34 | // Return filtered value
35 | return next;
36 | }
37 | }
38 |
--------------------------------------------------------------------------------
/src/com/stuypulse/stuylib/streams/vectors/filters/VHighPassFilter.java:
--------------------------------------------------------------------------------
1 | /* Copyright (c) 2025 StuyPulse Robotics. All rights reserved. */
2 | /* This work is licensed under the terms of the MIT license */
3 | /* found in the root directory of this project. */
4 |
5 | package com.stuypulse.stuylib.streams.vectors.filters;
6 |
7 | import com.stuypulse.stuylib.streams.numbers.filters.HighPassFilter;
8 |
9 | /**
10 | * A filter that applies a HighPassFilter to a VStream
11 | *
12 | * @author Sam (sam.belliveau@gmail.com)
13 | */
14 | public class VHighPassFilter extends XYFilter {
15 |
16 | public VHighPassFilter(Number rc) {
17 | super(new HighPassFilter(rc), new HighPassFilter(rc));
18 | }
19 | }
20 |
--------------------------------------------------------------------------------
/src/com/stuypulse/stuylib/streams/vectors/filters/VLowPassFilter.java:
--------------------------------------------------------------------------------
1 | /* Copyright (c) 2025 StuyPulse Robotics. All rights reserved. */
2 | /* This work is licensed under the terms of the MIT license */
3 | /* found in the root directory of this project. */
4 |
5 | package com.stuypulse.stuylib.streams.vectors.filters;
6 |
7 | import com.stuypulse.stuylib.streams.numbers.filters.LowPassFilter;
8 |
9 | /**
10 | * A filter that applies a LowPassFilter to a VStream
11 | *
12 | * @author Sam (sam.belliveau@gmail.com)
13 | */
14 | public class VLowPassFilter extends XYFilter {
15 |
16 | public VLowPassFilter(Number rc) {
17 | super(new LowPassFilter(rc), new LowPassFilter(rc));
18 | }
19 | }
20 |
--------------------------------------------------------------------------------
/src/com/stuypulse/stuylib/streams/vectors/filters/VMotionProfile.java:
--------------------------------------------------------------------------------
1 | /* Copyright (c) 2025 StuyPulse Robotics. All rights reserved. */
2 | /* This work is licensed under the terms of the MIT license */
3 | /* found in the root directory of this project. */
4 |
5 | package com.stuypulse.stuylib.streams.vectors.filters;
6 |
7 | import com.stuypulse.stuylib.math.Vector2D;
8 | import com.stuypulse.stuylib.util.StopWatch;
9 |
10 | /**
11 | * A filter, that when applied to the input of a motor, will profile it. Similar to the way in which
12 | * motion profiling can limit the amount of acceleration and jerk in an S-Curve, this can do that to
13 | * real time input. Because this will add a delay, it is recommended that the accelLimit is as high
14 | * as possible. Aside from the accelLimit, this is identical to a SlewRateLimiter or TimedRateLimit.
15 | *
16 | * @author Sam (sam.belliveau@gmail.com)
17 | */
18 | public class VMotionProfile implements VFilter {
19 |
20 | // Default number of times to apply filter (helps accuracy)
21 | private static final int kDefaultSteps = 64;
22 |
23 | // Stopwatch to Track dt
24 | private StopWatch mTimer;
25 |
26 | // Limits for each of the derivatives
27 | private Number mVelLimit;
28 | private Number mAccelLimit;
29 |
30 | // The last output / acceleration
31 | private Vector2D mOutput;
32 | private Vector2D mAccel;
33 |
34 | // Number of times to apply filter (helps accuracy)
35 | private final int mSteps;
36 |
37 | /**
38 | * @param velLimit maximum change in displacement per second (u/s)
39 | * @param accelLimit maximum change in velocity per second (u/s/s)
40 | * @param steps number of times to apply filter (improves accuracy)
41 | */
42 | public VMotionProfile(Number velLimit, Number accelLimit, int steps) {
43 | mTimer = new StopWatch();
44 |
45 | mVelLimit = velLimit;
46 | mAccelLimit = accelLimit;
47 |
48 | mOutput = Vector2D.kOrigin;
49 | mAccel = Vector2D.kOrigin;
50 |
51 | mSteps = steps;
52 | }
53 |
54 | /**
55 | * @param velLimit maximum change in velocity per second (u/s)
56 | * @param accelLimit maximum change in acceleration per second (u/s/s)
57 | */
58 | public VMotionProfile(Number velLimit, Number accelLimit) {
59 | this(velLimit, accelLimit, kDefaultSteps);
60 | }
61 |
62 | public Vector2D get(Vector2D target) {
63 | double dt = mTimer.reset() / mSteps;
64 |
65 | for (int i = 0; i < mSteps; ++i) {
66 | // if there is a jerk limit, limit the amount the acceleration can change
67 | if (0 < mAccelLimit.doubleValue()) {
68 | // amount of windup in system (how long it would take to slow down)
69 | double windup = mAccel.magnitude() / mAccelLimit.doubleValue();
70 |
71 | // If the windup is too small, just use normal algorithm to limit acceleration
72 | if (windup < dt) {
73 | // Calculate acceleration needed to reach target
74 | Vector2D accel = target.sub(mOutput).div(dt).sub(mAccel);
75 |
76 | // Try to reach it while abiding by jerklimit
77 | mAccel = mAccel.add(accel.clamp(dt * mAccelLimit.doubleValue()));
78 | } else {
79 | // the position it would end up if it attempted to come to a full stop
80 | Vector2D windA =
81 | mAccel.mul(0.5 * (dt + windup)); // windup caused by acceleration
82 | Vector2D future = mOutput.add(windA); // where the robot will end up
83 |
84 | // Calculate acceleration needed to come to stop at target throughout windup
85 | Vector2D accel = target.sub(future).div(windup);
86 |
87 | // Try to reach it while abiding by jerklimit
88 | mAccel = mAccel.add(accel.clamp(dt * mAccelLimit.doubleValue()));
89 | }
90 |
91 | } else {
92 | // make the acceleration the difference between target and current
93 | mAccel = target.sub(mOutput).div(dt);
94 | }
95 |
96 | // if there is an acceleration limit, limit the acceleration
97 | if (0 < mVelLimit.doubleValue()) {
98 | mAccel = mAccel.clamp(mVelLimit.doubleValue());
99 | }
100 |
101 | // adjust output by calculated acceleration
102 | mOutput = mOutput.add(mAccel.mul(dt));
103 | }
104 |
105 | return mOutput;
106 | }
107 | }
108 |
--------------------------------------------------------------------------------
/src/com/stuypulse/stuylib/streams/vectors/filters/VRateLimit.java:
--------------------------------------------------------------------------------
1 | /* Copyright (c) 2025 StuyPulse Robotics. All rights reserved. */
2 | /* This work is licensed under the terms of the MIT license */
3 | /* found in the root directory of this project. */
4 |
5 | package com.stuypulse.stuylib.streams.vectors.filters;
6 |
7 | import com.stuypulse.stuylib.math.Vector2D;
8 | import com.stuypulse.stuylib.util.StopWatch;
9 |
10 | /**
11 | * A filter that limits the amount that a Vector2D can change by per second.
12 | *
13 | * @author Sam (sam.belliveau@gmail.com)
14 | */
15 | public class VRateLimit implements VFilter {
16 |
17 | private final StopWatch mTimer;
18 | private final Number mRateLimit;
19 |
20 | private Vector2D mLastValue;
21 |
22 | public VRateLimit(Number rateLimit) {
23 | mTimer = new StopWatch();
24 | mRateLimit = rateLimit;
25 |
26 | mLastValue = Vector2D.kOrigin;
27 | }
28 |
29 | public Vector2D get(Vector2D next) {
30 | final double dt = mTimer.reset();
31 | return mLastValue =
32 | mLastValue.add(next.sub(mLastValue).clamp(dt * mRateLimit.doubleValue()));
33 | }
34 | }
35 |
--------------------------------------------------------------------------------
/src/com/stuypulse/stuylib/streams/vectors/filters/VTimedMovingAverage.java:
--------------------------------------------------------------------------------
1 | /* Copyright (c) 2025 StuyPulse Robotics. All rights reserved. */
2 | /* This work is licensed under the terms of the MIT license */
3 | /* found in the root directory of this project. */
4 |
5 | package com.stuypulse.stuylib.streams.vectors.filters;
6 |
7 | import com.stuypulse.stuylib.streams.numbers.filters.TimedMovingAverage;
8 |
9 | /**
10 | * A filter that takes a timed moving average of a VStream
11 | *
12 | * @author Sam (sam.belliveau@gmail.com)
13 | */
14 | public class VTimedMovingAverage extends XYFilter {
15 |
16 | public VTimedMovingAverage(Number time) {
17 | super(new TimedMovingAverage(time), new TimedMovingAverage(time));
18 | }
19 | }
20 |
--------------------------------------------------------------------------------
/src/com/stuypulse/stuylib/streams/vectors/filters/XYFilter.java:
--------------------------------------------------------------------------------
1 | /* Copyright (c) 2025 StuyPulse Robotics. All rights reserved. */
2 | /* This work is licensed under the terms of the MIT license */
3 | /* found in the root directory of this project. */
4 |
5 | package com.stuypulse.stuylib.streams.vectors.filters;
6 |
7 | import com.stuypulse.stuylib.math.Vector2D;
8 | import com.stuypulse.stuylib.streams.numbers.filters.IFilter;
9 |
10 | /**
11 | * A filter that applies separate IFilters to the x and y component of a VFilter
12 | *
13 | * @author Sam (sam.belliveau@gmail.com)
14 | */
15 | public class XYFilter implements VFilter {
16 |
17 | private final IFilter mXFilter;
18 | private final IFilter mYFilter;
19 |
20 | public XYFilter(IFilter xFilter, IFilter yFilter) {
21 | mXFilter = xFilter;
22 | mYFilter = yFilter;
23 | }
24 |
25 | public Vector2D get(Vector2D next) {
26 | return new Vector2D(mXFilter.get(next.x), mYFilter.get(next.y));
27 | }
28 | }
29 |
--------------------------------------------------------------------------------
/src/com/stuypulse/stuylib/util/AngleVelocity.java:
--------------------------------------------------------------------------------
1 | /* Copyright (c) 2025 StuyPulse Robotics. All rights reserved. */
2 | /* This work is licensed under the terms of the MIT license */
3 | /* found in the root directory of this project. */
4 |
5 | package com.stuypulse.stuylib.util;
6 |
7 | import com.stuypulse.stuylib.math.Angle;
8 |
9 | public class AngleVelocity {
10 |
11 | private final StopWatch mTimer;
12 | private Angle mPreviousAngle;
13 |
14 | public AngleVelocity() {
15 | mTimer = new StopWatch();
16 | mPreviousAngle = Angle.kZero;
17 | }
18 |
19 | public double get(Angle angle) {
20 | double velocity = angle.velocityRadians(mPreviousAngle, mTimer.reset());
21 | mPreviousAngle = angle;
22 | return velocity;
23 | }
24 | }
25 |
--------------------------------------------------------------------------------
/src/com/stuypulse/stuylib/util/Conversion.java:
--------------------------------------------------------------------------------
1 | /* Copyright (c) 2025 StuyPulse Robotics. All rights reserved. */
2 | /* This work is licensed under the terms of the MIT license */
3 | /* found in the root directory of this project. */
4 |
5 | package com.stuypulse.stuylib.util;
6 |
7 | import java.util.function.Function;
8 |
9 | /**
10 | * Represents a conversion from a type to another type.
11 | *
12 | * Useful when wanting to package two related type/unit conversions functions together.
13 | *
14 | * @author Myles Pasetsky
15 | */
16 | public interface Conversion This is better than just doing it in the class because it stores the time as a long to keep
14 | * accuracy, but converts it into a double for convenience.
15 | *
16 | * @author Sam (sam.belliveau@gmail.com)
17 | */
18 | public class StopWatch {
19 |
20 | private TimeEngine mEngine;
21 | private long mLastTime;
22 |
23 | /**
24 | * Creates timer and reset it to now.
25 | *
26 | * @param engine the method that is used to get the current time
27 | */
28 | public StopWatch(TimeEngine engine) {
29 | mEngine = engine;
30 | mLastTime = mEngine.getRawTime();
31 | }
32 |
33 | /** Creates timer and reset it to now. */
34 | public StopWatch() {
35 | this(kDefaultEngine);
36 | }
37 |
38 | /**
39 | * Resets the stop watch to the current time and returns time since last reset.
40 | *
41 | * @return the time since the last reset was called in seconds. The result is always a non 0
42 | * positive number.
43 | */
44 | public double reset() {
45 | long time = mEngine.getRawTime();
46 | long delta = Math.max(1, time - mLastTime);
47 | mLastTime = time;
48 | return mEngine.toSeconds(delta);
49 | }
50 |
51 | /**
52 | * Gets the time since the stop watch was reset
53 | *
54 | * @return the time since the last reset was called in seconds. The result is always a non 0
55 | * positive number.
56 | */
57 | public double getTime() {
58 | long delta = Math.max(1, mEngine.getRawTime() - mLastTime);
59 | return mEngine.toSeconds(delta);
60 | }
61 |
62 | // Engine interface used to get the current time
63 | private interface TimeEngine {
64 |
65 | // Get the raw time as a long
66 | public long getRawTime();
67 |
68 | // Convert the integer time into a double
69 | public double toSeconds(long raw);
70 | }
71 |
72 | /** This engine is used to get the current time with the system function System.nanoTime() */
73 | public static final TimeEngine kNanoEngine =
74 | new TimeEngine() {
75 |
76 | public long getRawTime() {
77 | return System.nanoTime();
78 | }
79 |
80 | public double toSeconds(long raw) {
81 | return raw / 1_000_000_000.0;
82 | }
83 | };
84 |
85 | /**
86 | * This engine is used to get the current time with the system function
87 | * System.currentTimeMillis()
88 | *
89 | * This may have a lower resolution, but it is stable.
90 | */
91 | public static final TimeEngine kMillisEngine =
92 | new TimeEngine() {
93 |
94 | public long getRawTime() {
95 | return System.currentTimeMillis();
96 | }
97 |
98 | public double toSeconds(long raw) {
99 | return raw / 1_000.0;
100 | }
101 | };
102 |
103 | /**
104 | * This engine is used to get the current time with the WPI function Timer.getFPGATimestamp()
105 | *
106 | * This is more accurate when used on a real robot.
107 | */
108 | public static final TimeEngine kFPGAEngine =
109 | new TimeEngine() {
110 |
111 | public long getRawTime() {
112 | return RobotController.getFPGATime();
113 | }
114 |
115 | public double toSeconds(long raw) {
116 | return raw / 1_000_000.0;
117 | }
118 | };
119 |
120 | /** Variable that stores the time engine we want to use by default */
121 | private static TimeEngine kSelectedDefaultEngine = kNanoEngine;
122 |
123 | /** This engine will return the same values as whatever is selected as the default engine */
124 | public static final TimeEngine kDefaultEngine =
125 | new TimeEngine() {
126 | public long getRawTime() {
127 | return kSelectedDefaultEngine.getRawTime();
128 | }
129 |
130 | public double toSeconds(long raw) {
131 | return kSelectedDefaultEngine.toSeconds(raw);
132 | }
133 | };
134 |
135 | /**
136 | * Set the default engine for the StopWatch
137 | *
138 | * This will be the engine used by every PID Loop and IFilter
139 | *
140 | * @param defaultEngine the engine that you want to be used by default
141 | */
142 | public static void setDefaultEngine(TimeEngine defaultEngine) {
143 | kSelectedDefaultEngine = defaultEngine;
144 | }
145 | }
146 |
--------------------------------------------------------------------------------
/src/com/stuypulse/stuylib/util/readme.md:
--------------------------------------------------------------------------------
1 | # StuyLib Misc Utils
2 |
3 | WIP...
4 |
--------------------------------------------------------------------------------
/vendordeps/WPILibNewCommands.json:
--------------------------------------------------------------------------------
1 | {
2 | "fileName": "WPILibNewCommands.json",
3 | "name": "WPILib-New-Commands",
4 | "version": "1.0.0",
5 | "uuid": "111e20f7-815e-48f8-9dd6-e675ce75b266",
6 | "frcYear": "2025",
7 | "mavenUrls": [],
8 | "jsonUrl": "",
9 | "javaDependencies": [
10 | {
11 | "groupId": "edu.wpi.first.wpilibNewCommands",
12 | "artifactId": "wpilibNewCommands-java",
13 | "version": "wpilib"
14 | }
15 | ],
16 | "jniDependencies": [],
17 | "cppDependencies": [
18 | {
19 | "groupId": "edu.wpi.first.wpilibNewCommands",
20 | "artifactId": "wpilibNewCommands-cpp",
21 | "version": "wpilib",
22 | "libName": "wpilibNewCommands",
23 | "headerClassifier": "headers",
24 | "sourcesClassifier": "sources",
25 | "sharedLibrary": true,
26 | "skipInvalidPlatforms": true,
27 | "binaryPlatforms": [
28 | "linuxathena",
29 | "linuxarm32",
30 | "linuxarm64",
31 | "windowsx86-64",
32 | "windowsx86",
33 | "linuxx86-64",
34 | "osxuniversal"
35 | ]
36 | }
37 | ]
38 | }
39 |
--------------------------------------------------------------------------------