Useful when wanting to package two related type/unit conversions functions together.
13 | *
14 | * @author Myles Pasetsky
15 | */
16 | public interface Conversion 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/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/angles/FilteredAStream.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 | import com.stuypulse.stuylib.streams.angles.filters.AFilter;
9 |
10 | /**
11 | * Takes an {@link AStream} and a {@link AFilter} and makes a {@link FilteredAStream}
12 | *
13 | * 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/AStick.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 | import com.stuypulse.stuylib.math.Vector2D;
9 | import com.stuypulse.stuylib.streams.vectors.VStream;
10 |
11 | /**
12 | * When controlling angle with a joystick that has a deadzone filter, the angle will snap to 0
13 | * degrees while in the deadzone. This class prevents the angle snapping and will return the most
14 | * recent angle from outside of the deadzone.
15 | *
16 | * @author Benjamin Goldfisher
17 | */
18 | public class AStick implements AStream {
19 |
20 | private final VStream mStick;
21 | private final Number mDeadzone;
22 |
23 | private Angle mPrevious;
24 |
25 | public AStick(VStream stick, Number deadzone) {
26 | mStick = stick;
27 | mDeadzone = deadzone;
28 |
29 | mPrevious = Angle.kNull;
30 | }
31 |
32 | @Override
33 | public Angle get() {
34 | final Vector2D stick = mStick.get();
35 |
36 | if (mDeadzone.doubleValue() < stick.magnitude()) {
37 | mPrevious = stick.getAngle();
38 | }
39 |
40 | return mPrevious;
41 | }
42 | }
43 |
--------------------------------------------------------------------------------
/src/com/stuypulse/stuylib/streams/vectors/FilteredVStream.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.vectors.filters.VFilter;
9 |
10 | /**
11 | * A FilteredVStream is similar to a FilteredIStream.
12 | *
13 | * 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/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/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/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/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/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/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/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/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/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/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/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/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/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/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/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/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/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 |
--------------------------------------------------------------------------------
/.github/workflows/build.yml:
--------------------------------------------------------------------------------
1 | name: Build
2 |
3 | on: [push, fork, pull_request]
4 |
5 | env:
6 | JAVA_VERSION: 1.17
7 | GRADLE_VERSION: 8.12.1
8 |
9 | jobs:
10 | spotless-check:
11 | name: Spotless Apply
12 |
13 | runs-on: ubuntu-latest
14 | steps:
15 | - name: Checkout
16 | uses: actions/checkout@v3
17 |
18 | - name: Set up JDK ${{ env.JAVA_VERSION }}
19 | uses: actions/setup-java@v1
20 | with:
21 | java-version: ${{ env.JAVA_VERSION }}
22 |
23 | - name: Spotless Check - Gradle ${{ env.GRADLE_VERSION }}
24 | uses: gradle/gradle-build-action@v2.3.0-beta.1
25 | with:
26 | gradle-version: ${{ env.GRADLE_VERSION }}
27 | arguments: spotlessCheck
28 |
29 | javadocs-check:
30 | name: Javadocs
31 |
32 | runs-on: ubuntu-latest
33 | steps:
34 | - name: Checkout
35 | uses: actions/checkout@v3
36 |
37 | - name: Set up JDK ${{ env.JAVA_VERSION }}
38 | uses: actions/setup-java@v1
39 | with:
40 | java-version: ${{ env.JAVA_VERSION }}
41 |
42 | - name: Build Javadocs - Gradle ${{ env.GRADLE_VERSION }}
43 | uses: gradle/gradle-build-action@v2.3.0-beta.1
44 | with:
45 | arguments: javadoc
46 |
47 | final-build:
48 | name: Finalize
49 |
50 | needs:
51 | - spotless-check
52 | - javadocs-check
53 |
54 | runs-on: ubuntu-latest
55 | steps:
56 | - name: Checkout
57 | uses: actions/checkout@v3
58 |
59 | - name: Set up JDK ${{ env.JAVA_VERSION }}
60 | uses: actions/setup-java@v1
61 | with:
62 | java-version: ${{ env.JAVA_VERSION }}
63 |
64 | - name: Build - Gradle ${{ env.GRADLE_VERSION }}
65 | uses: gradle/gradle-build-action@v2.3.0-beta.1
66 | with:
67 | arguments: build
68 |
69 | - name: Upload StuyLib Build
70 | uses: actions/upload-artifact@v4.0.0
71 | with:
72 | name: StuyLib-Release
73 | path: ./build/libs/*.jar
74 | retention-days: 90
75 |
--------------------------------------------------------------------------------
/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/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 |
--------------------------------------------------------------------------------
/.github/workflows/build_release.yml:
--------------------------------------------------------------------------------
1 | name: Prepare Release
2 |
3 | on:
4 | release:
5 | types: [created]
6 |
7 | permissions:
8 | contents: write
9 | pages: write
10 | id-token: write
11 |
12 | env:
13 | JAVA_VERSION: 1.17
14 | GRADLE_VERSION: 8.12.1
15 |
16 | jobs:
17 | pages:
18 | name: Deploy Javadocs
19 | environment:
20 | name: github-pages
21 | url: ${{ steps.deployment.outputs.page_url }}
22 | runs-on: ubuntu-latest
23 | steps:
24 | - name: Checkout
25 | uses: actions/checkout@v3
26 |
27 | - name: Set up JDK ${{ env.JAVA_VERSION }}
28 | uses: actions/setup-java@v1
29 | with:
30 | java-version: ${{ env.JAVA_VERSION }}
31 |
32 | - name: Build Javadocs - Gradle ${{ env.GRADLE_VERSION }}
33 | uses: gradle/gradle-build-action@v2.11.1
34 | with:
35 | arguments: javadoc -Pversion=${{ github.event.release.tag_name }}
36 |
37 | - name: Setup Pages
38 | uses: actions/configure-pages@v2.0.0
39 |
40 | - name: Upload artifact
41 | uses: actions/upload-pages-artifact@v4.0.0
42 | with:
43 | path: './docs/'
44 | name: github-pages
45 |
46 | - name: Deploy to GitHub Pages
47 | id: deployment
48 | uses: actions/deploy-pages@v4.0.3
49 | with:
50 | artifact_name: github-pages
51 |
52 | deploy:
53 | name: Prepare Jar & JitPack
54 |
55 | runs-on: ubuntu-latest
56 | steps:
57 | - name: Checkout
58 | uses: actions/checkout@v3
59 |
60 | - name: Set up JDK ${{ env.JAVA_VERSION }}
61 | uses: actions/setup-java@v1
62 | with:
63 | java-version: ${{ env.JAVA_VERSION }}
64 |
65 | - name: Build - Gradle ${{ env.GRADLE_VERSION }}
66 | uses: gradle/gradle-build-action@v2.11.1
67 | with:
68 | arguments: build -Pversion=${{ github.event.release.tag_name }}
69 |
70 | - name: Upload Assets to Release
71 | uses: AButler/upload-release-assets@v2.0
72 | with:
73 | files: ./build/libs/*.jar
74 | repo-token: ${{ secrets.GITHUB_TOKEN }}
75 |
76 | - name: Update JitPack
77 | uses: badasintended/autojitpack@v1
78 | with:
79 | version: ${{ github.event.release.tag_name }}
80 |
--------------------------------------------------------------------------------
/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 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/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 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/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 |
--------------------------------------------------------------------------------
/gradlew.bat:
--------------------------------------------------------------------------------
1 | @rem
2 | @rem Copyright 2015 the original author or authors.
3 | @rem
4 | @rem Licensed under the Apache License, Version 2.0 (the "License");
5 | @rem you may not use this file except in compliance with the License.
6 | @rem You may obtain a copy of the License at
7 | @rem
8 | @rem https://www.apache.org/licenses/LICENSE-2.0
9 | @rem
10 | @rem Unless required by applicable law or agreed to in writing, software
11 | @rem distributed under the License is distributed on an "AS IS" BASIS,
12 | @rem WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 | @rem See the License for the specific language governing permissions and
14 | @rem limitations under the License.
15 | @rem
16 |
17 | @if "%DEBUG%"=="" @echo off
18 | @rem ##########################################################################
19 | @rem
20 | @rem Gradle startup script for Windows
21 | @rem
22 | @rem ##########################################################################
23 |
24 | @rem Set local scope for the variables with windows NT shell
25 | if "%OS%"=="Windows_NT" setlocal
26 |
27 | set DIRNAME=%~dp0
28 | if "%DIRNAME%"=="" set DIRNAME=.
29 | @rem This is normally unused
30 | set APP_BASE_NAME=%~n0
31 | set APP_HOME=%DIRNAME%
32 |
33 | @rem Resolve any "." and ".." in APP_HOME to make it shorter.
34 | for %%i in ("%APP_HOME%") do set APP_HOME=%%~fi
35 |
36 | @rem Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script.
37 | set DEFAULT_JVM_OPTS="-Xmx64m" "-Xms64m"
38 |
39 | @rem Find java.exe
40 | if defined JAVA_HOME goto findJavaFromJavaHome
41 |
42 | set JAVA_EXE=java.exe
43 | %JAVA_EXE% -version >NUL 2>&1
44 | if %ERRORLEVEL% equ 0 goto execute
45 |
46 | echo.
47 | echo ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH.
48 | echo.
49 | echo Please set the JAVA_HOME variable in your environment to match the
50 | echo location of your Java installation.
51 |
52 | goto fail
53 |
54 | :findJavaFromJavaHome
55 | set JAVA_HOME=%JAVA_HOME:"=%
56 | set JAVA_EXE=%JAVA_HOME%/bin/java.exe
57 |
58 | if exist "%JAVA_EXE%" goto execute
59 |
60 | echo.
61 | echo ERROR: JAVA_HOME is set to an invalid directory: %JAVA_HOME%
62 | echo.
63 | echo Please set the JAVA_HOME variable in your environment to match the
64 | echo location of your Java installation.
65 |
66 | goto fail
67 |
68 | :execute
69 | @rem Setup the command line
70 |
71 | set CLASSPATH=%APP_HOME%\gradle\wrapper\gradle-wrapper.jar
72 |
73 |
74 | @rem Execute Gradle
75 | "%JAVA_EXE%" %DEFAULT_JVM_OPTS% %JAVA_OPTS% %GRADLE_OPTS% "-Dorg.gradle.appname=%APP_BASE_NAME%" -classpath "%CLASSPATH%" org.gradle.wrapper.GradleWrapperMain %*
76 |
77 | :end
78 | @rem End local scope for the variables with windows NT shell
79 | if %ERRORLEVEL% equ 0 goto mainEnd
80 |
81 | :fail
82 | rem Set variable GRADLE_EXIT_CONSOLE if you need the _script_ return code instead of
83 | rem the _cmd.exe /c_ return code!
84 | set EXIT_CODE=%ERRORLEVEL%
85 | if %EXIT_CODE% equ 0 set EXIT_CODE=1
86 | if not ""=="%GRADLE_EXIT_CONSOLE%" exit %EXIT_CODE%
87 | exit /b %EXIT_CODE%
88 |
89 | :mainEnd
90 | if "%OS%"=="Windows_NT" endlocal
91 |
92 | :omega
93 |
--------------------------------------------------------------------------------
/src/com/stuypulse/stuylib/streams/vectors/filters/VFilter.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 | * This is the VFilter interface class that gives a definition for how to implement a filter.
12 | *
13 | * 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/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 |
--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
1 | 
2 |
3 | [](https://jitpack.io/#StuyPulse/StuyLib)
4 | [](https://github.com/StuyPulse/StuyLib/actions/workflows/build.yml)
5 |
6 | ---
7 |
8 | # What is StuyLib?
9 |
10 | StuyLib is an FRC library / toolkit which includes many different utilities for programming. It includes things like a Gamepad Library, a Limelight Library, a Digitial Filter / Streams Library, and many other utilities relating to math and programming.
11 |
12 | # Awards
13 |
14 | - IStreams / IStreamFilters:
15 | - [Innovation in Control Award sponsored by Rockwell Automation](https://www.thebluealliance.com/event/2020scmb#awards)
16 | - Limelight + Encoder Fusion / Drive Filters:
17 | - [Innovation in Control Award](https://www.thebluealliance.com/event/2022nyro#awards)
18 | - Clean & Concise Code / Smart Network Library:
19 | - [Innovation in Control Award](https://www.thebluealliance.com/event/2022nyli2#awards)
20 | - Usage of Encoders / Limelight + Encoder Fusion:
21 | - [Innovation in Control Award](https://www.thebluealliance.com/event/2022nyny#awards)
22 |
23 | # How do I use StuyLib?
24 |
25 | ### Setting up StuyLib
26 |
27 | [Instructions from JitPack.io](https://jitpack.io/#StuyPulse/StuyLib)
28 |
29 | Step 0. This is done by default for robot projects, but if your doing a local project you must remember to include this at the top
30 | ```groovy
31 | plugins {
32 | id "java"
33 |
34 | // You can use different versions of GradleRIO between your robot
35 | // code and Stuylib as long as the updates don't break anything
36 | id "edu.wpi.first.GradleRIO" version "2020.2.2"
37 | }
38 | ```
39 |
40 | Step 1. Add the JitPack repository to your `build.gradle`
41 |
42 | ```groovy
43 | allprojects {
44 | repositories {
45 | maven { url 'https://jitpack.io' }
46 | }
47 | }
48 | ```
49 |
50 | - Step 2. Add the dependency to `build.gradle`
51 |
52 | [](https://jitpack.io/#StuyPulse/StuyLib)
53 | ```groovy
54 | dependencies {
55 | // check tag above for most recent version
56 | compile 'com.github.StuyPulse:StuyLib:VERSION'
57 | }
58 | ```
59 |
60 |
61 | ### Other Dependencies
62 |
63 | If you want the NetworkTable classes to work on something other than the robot, then you need to include other dependencies.
64 |
65 | If you are using network tables anywhere else than the robot, ie. on a laptop, you will need to include the jar files in the `./lib` folder. For example, when using NetKeyboard, you need to run a program to send the keyboard input, [here is the program](https://github.com/Sam-Belliveau/NetworkKeyboardServer). This program uses these jar files to send network table data to the robot. It is otherwise pretty rare that you would need to use these
66 |
67 | # Criteria for Code Contribution
68 |
69 | When contributing code, as always, make a branch and a pull request.
70 |
71 | All code MUST be commented, and no single class should be too complicated.
72 |
73 | At the top of each class, give a brief description of the class, along with an author tag which contains an email just in case anybody has any questions on how to use it.
74 |
75 | # Star History
76 |
77 | [](https://star-history.com/#StuyPulse/StuyLib&Date)
78 |
--------------------------------------------------------------------------------
/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/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/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/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/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/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/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/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/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/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/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 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/streams/numbers/filters/readme.md:
--------------------------------------------------------------------------------
1 | # StuyLib Filters Library
2 |
3 | ## What is a filter?
4 |
5 | A filter is basically a class / function that takes in a value, and returns the "filtered" value.
6 |
7 | For example say we made a Moving Average Filter.
8 |
9 | ```java
10 | // Returns the average of the last 3 values
11 | IFilter avg = new MovingAverage(3);
12 |
13 | System.out.println(avg.get(1)); // Prints: 1.0
14 | System.out.println(avg.get(2)); // Prints: 1.5
15 | System.out.println(avg.get(3)); // Prints: 2.0
16 | System.out.println(avg.get(4)); // Prints: 3.0
17 | System.out.println(avg.get(5)); // Prints: 4.0
18 | ```
19 |
20 | Everytime we called `.get()` on the filter, it returned the averagee of the last 3 values, because thats what the filter was made to do.
21 |
22 | ## What should we use them for?
23 |
24 | ### Ramping Motors
25 |
26 | When sending speed values to motors, you might not always want them to update immediately, as that could cause jerk and current spikes.
27 |
28 | In WPILib, there is the option to ramp motors when configuring them. This ramping algorithm is analygous to the filtering done by the filter called [Rate Limit](https://github.com/StuyPulse/StuyLib/blob/main/src/com/stuypulse/stuylib/streams/filters/RateLimit.java). RateLimit works good for smoothing out the motion of the robot, but through testing we have found that the filter called [LowPass Filter](https://github.com/StuyPulse/StuyLib/blob/main/src/com/stuypulse/stuylib/streams/filters/LowPassFilter.java), does a much nicer job of providing smooth motion without introducing a large delay.
29 |
30 | We did experiment with [Speed Profile](https://github.com/StuyPulse/StuyLib/blob/main/src/com/stuypulse/stuylib/streams/filters/SpeedProfile.java), which algebraicly removes jerk from the inputs of a conroller, however the delay it introduced killed its usefulness compared to [LowPass Filter](https://github.com/StuyPulse/StuyLib/blob/main/src/com/stuypulse/stuylib/streams/filters/LowPassFilter.java).
31 |
32 | The reduced jerk when controlling the motors is a huge advantage as it uses less power, and prevents the robot from skipping / tilting, which can mess with sensor data. This allows the driver to drive faster without worrying about damaging the robot.
33 |
34 | Delay is the biggest issue with filters. Because they can not predict the future, it's impossible for it to smooth out values without introducing some amount of delay. We've found that filters that prioritize recent values more than past ones, are able to do the best.
35 |
36 | ### Filtering Data
37 |
38 | When reading data from something like the Limelight or an encoder, sometimes noise can be an issue. Encoders sometimes include a form of [Moving Average](https://github.com/StuyPulse/StuyLib/blob/main/src/com/stuypulse/stuylib/streams/filters/MovingAverage.java) to smooth out noise in data. However, filters can often add a delay, which if fed into a PID loop, can often cause decreased performance. We've found that using a small [LowPass Filter](https://github.com/StuyPulse/StuyLib/blob/main/src/com/stuypulse/stuylib/streams/filters/LowPassFilter.java) on data that has a slight bit of noise in it can help a lot, however it will decrease performance.
39 |
40 | A recently added filter named [Median Filter](https://github.com/StuyPulse/StuyLib/blob/main/src/com/stuypulse/stuylib/streams/filters/MedianFilter.java) is able to reduce noise while not having a huge impact on delay. It is good to combine this filter with other filters to get an optimal result.
41 |
42 | ## Which Filter should I use?
43 |
44 | Some filters have internal timers in them which keep track of real world time. This is helpful if the data your collecting is real time, and in a time series. When it comes to FRC Robotics, almost all data that we have used filters on is real time. Be sure to read the documentation of certain filters to see which ones use timers in them.
45 |
46 | Depending on which type of data you are working with, some filters might be better than others. However, we've found the [LowPass Filter](https://github.com/StuyPulse/StuyLib/blob/main/src/com/stuypulse/stuylib/streams/filters/LowPassFilter.java) to be an amazing all around filter, and should be used as a Rule of Thumb when testing out filters. *(meaning that LowPass Filters will usually be the best)*
47 |
48 | Also, combine filters using [IFilterGroup](https://github.com/StuyPulse/StuyLib/blob/main/src/com/stuypulse/stuylib/streams/filters/IFilterGroup.java), as filtering filtered values can often have interesting and desirable results.
49 |
50 | ## Why do filters take in `Numbers` instead of `double`?
51 |
52 | In order to help make filters more configurable, the configuation is done using the Number class. Why? Because [SmartNumber](https://github.com/StuyPulse/StuyLib/blob/main/src/com/stuypulse/stuylib/network/SmartNumber.java) extends `Number`, which makes it extremely easy to change values like the RC of a [LowPass Filter](https://github.com/StuyPulse/StuyLib/blob/main/src/com/stuypulse/stuylib/streams/filters/LowPassFilter.java) on the fly.
53 |
--------------------------------------------------------------------------------
/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/util/StopWatch.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 edu.wpi.first.wpilibj.RobotController;
8 |
9 | /**
10 | * This StopWatch class helps classes who want their functions to be time independent do that by
11 | * giving them an easy way to get intervals of time.
12 | *
13 | * 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/control/Controller.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 | import com.stuypulse.stuylib.streams.numbers.filters.IFilter;
8 |
9 | /**
10 | * A controller calculates an output variable given a setpoint and measurement of a single variable.
11 | *
12 | * 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/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/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/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/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/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 |
--------------------------------------------------------------------------------