From 6bd720f1a1fbcffd59da9487ffa1ec5fea4e1883 Mon Sep 17 00:00:00 2001 From: "daniel.yang" Date: Mon, 27 Oct 2025 22:57:17 -0700 Subject: [PATCH 01/23] feat(hardware): added Encoder and EncoderBase --- .../solverslib/hardware/Encoder.java | 34 ++++++++++++++ .../solverslib/hardware/EncoderBase.java | 47 +++++++++++++++++++ 2 files changed, 81 insertions(+) create mode 100644 core/src/main/java/com/seattlesolvers/solverslib/hardware/Encoder.java create mode 100644 core/src/main/java/com/seattlesolvers/solverslib/hardware/EncoderBase.java diff --git a/core/src/main/java/com/seattlesolvers/solverslib/hardware/Encoder.java b/core/src/main/java/com/seattlesolvers/solverslib/hardware/Encoder.java new file mode 100644 index 00000000..31762eaa --- /dev/null +++ b/core/src/main/java/com/seattlesolvers/solverslib/hardware/Encoder.java @@ -0,0 +1,34 @@ +package com.seattlesolvers.solverslib.hardware; + +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; + +public interface Encoder extends HardwareDevice { + /** + * @return The normalized angular position of the encoder + */ + double getAngle(); + + /** + * @return The angle unit associated with the encoder + */ + AngleUnit getAngleUnit(); + + /** + * Sets whether or not the encoder should be reversed for any future values returned when reading the encoder + * @param reversed Whether or not the encoder should be reversed for any future values + * @return The object itself for chaining purposes + */ + Encoder setReversed(boolean reversed); + + /** + * @return Whether the encoder is reversed + */ + boolean getReversed(); + + /** + * Sets an angular offset for any future values returned when reading the encoder + * @param offset The angular offset in the units specified by the user previously + * @return The object itself for chaining purposes + */ + Encoder resetAngle(double offset); +} diff --git a/core/src/main/java/com/seattlesolvers/solverslib/hardware/EncoderBase.java b/core/src/main/java/com/seattlesolvers/solverslib/hardware/EncoderBase.java new file mode 100644 index 00000000..ddd36e4b --- /dev/null +++ b/core/src/main/java/com/seattlesolvers/solverslib/hardware/EncoderBase.java @@ -0,0 +1,47 @@ +package com.seattlesolvers.solverslib.hardware; + +import com.seattlesolvers.solverslib.util.MathUtils; + +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; + +public abstract class EncoderBase implements Encoder { + protected boolean reversed = false; + protected double angleOffset = 0.0; + protected AngleUnit angleUnit; + + @Override + public EncoderBase setReversed(boolean reversed) { + this.reversed = reversed; + return this; + } + + @Override + public boolean getReversed() { + return reversed; + } + + @Override + public EncoderBase resetAngle(double offset) { + this.angleOffset = offset; + return this; + } + + @Override + public double getAngle() { + return MathUtils.normalizeAngle( + (reversed ? -1 : 1) * (getRawAngle() + angleOffset), + true, + angleUnit + ); + } + + @Override + public AngleUnit getAngleUnit() { + return angleUnit; + } + + /** + * @return The raw calculated angle, before reversing or offsetting + */ + public abstract double getRawAngle(); +} \ No newline at end of file From 39182c85fa519c1183d6aaa94a06592766bde098 Mon Sep 17 00:00:00 2001 From: "daniel.yang" Date: Mon, 27 Oct 2025 22:57:55 -0700 Subject: [PATCH 02/23] refactor(hardware): refactor AbsoluteAnalogEncoder to extend EncoderBase --- .../hardware/AbsoluteAnalogEncoder.java | 56 ++----------------- .../solverslib/hardware/motors/CRServoEx.java | 2 +- 2 files changed, 7 insertions(+), 51 deletions(-) diff --git a/core/src/main/java/com/seattlesolvers/solverslib/hardware/AbsoluteAnalogEncoder.java b/core/src/main/java/com/seattlesolvers/solverslib/hardware/AbsoluteAnalogEncoder.java index 301f9844..6404f1cb 100644 --- a/core/src/main/java/com/seattlesolvers/solverslib/hardware/AbsoluteAnalogEncoder.java +++ b/core/src/main/java/com/seattlesolvers/solverslib/hardware/AbsoluteAnalogEncoder.java @@ -11,14 +11,11 @@ * * @author Saket */ -public class AbsoluteAnalogEncoder implements HardwareDevice { +public class AbsoluteAnalogEncoder extends EncoderBase { public static double DEFAULT_RANGE = 3.3; private final AnalogInput encoder; private final String id; - private double offset = 0.0; private final double range; - private final AngleUnit angleUnit; - private boolean reversed; /** * The constructor for absolute analog encoders @@ -43,45 +40,6 @@ public AbsoluteAnalogEncoder(HardwareMap hwMap, String id) { this(hwMap, id, DEFAULT_RANGE, AngleUnit.RADIANS); } - /** - * Sets an angular offset for any future values returned when reading the encoder - * @param offset The angular offset in the units specified by the user previously - * @return The object itself for chaining purposes - */ - public AbsoluteAnalogEncoder zero(double offset) { - this.offset = offset; - return this; - } - - /** - * Sets whether or not the encoder should be reversed for any future values returned when reading the encoder - * @param reversed Whether or not the encoder should be reversed for any future values - * @return The object itself for chaining purposes - */ - public AbsoluteAnalogEncoder setReversed(boolean reversed) { - this.reversed = reversed; - return this; - } - - /** - * Gets whether the encoder is reversed or not - * @return Whether the encoder is reversed - */ - public boolean getReversed() { - return reversed; - } - - /** - * @return The normalized angular position of the encoder in the unit previously specified by the user from 0 to max - */ - public double getCurrentPosition() { - return MathUtils.normalizeAngle( - (!reversed ? 1 - getVoltage() / range : getVoltage() / range) * MathUtils.returnMaxForAngleUnit(angleUnit) - offset, - true, - angleUnit - ); - } - /** * @return The AnalogInput object of the encoder itself */ @@ -96,6 +54,11 @@ public double getVoltage(){ return encoder.getVoltage(); } + @Override + public double getRawAngle() { + return getVoltage() / range * MathUtils.returnMaxForAngleUnit(angleUnit); + } + @Override public void disable() { // "take no action" (encoder.close() call in SDK) @@ -105,11 +68,4 @@ public void disable() { public String getDeviceType() { return "Absolute Analog Encoder; " + id; } - - /** - * @return The angle unit associated with the absolute encoder - */ - public AngleUnit getAngleUnit() { - return angleUnit; - } } diff --git a/core/src/main/java/com/seattlesolvers/solverslib/hardware/motors/CRServoEx.java b/core/src/main/java/com/seattlesolvers/solverslib/hardware/motors/CRServoEx.java index 112862b3..c02a869b 100644 --- a/core/src/main/java/com/seattlesolvers/solverslib/hardware/motors/CRServoEx.java +++ b/core/src/main/java/com/seattlesolvers/solverslib/hardware/motors/CRServoEx.java @@ -139,7 +139,7 @@ public void set(double output) { throw new IllegalStateException("Must have absolute encoder and PIDF coefficients for CR Servo to be in positional control"); } double normalizedTarget = MathUtils.normalizeAngle(output, true, absoluteEncoder.getAngleUnit()); - double error = MathUtils.normalizeAngle(normalizedTarget - absoluteEncoder.getCurrentPosition(), false, absoluteEncoder.getAngleUnit()); + double error = MathUtils.normalizeAngle(normalizedTarget - absoluteEncoder.getAngle(), false, absoluteEncoder.getAngleUnit()); double power = pidf.calculate(0, error); setPower(power); } else { From b7dcb674d763bf0f5fa2690089a6385cecd50c3d Mon Sep 17 00:00:00 2001 From: "daniel.yang" Date: Tue, 28 Oct 2025 00:38:49 -0700 Subject: [PATCH 03/23] feat(hardware) + refactor: added IncrementalEncoder and updated to use it --- .../solverslib/command/old/Subsystem.java | 2 +- .../hardware/AbsoluteAnalogEncoder.java | 20 ++- .../solverslib/hardware/Encoder.java | 20 ++- .../solverslib/hardware/EncoderBase.java | 37 +++-- .../hardware/IncrementalEncoder.java | 153 ++++++++++++++++++ .../solverslib/hardware/motors/Motor.java | 145 ++--------------- .../hardware/motors/MotorGroup.java | 7 +- .../DifferentialDriveOdometry.java | 8 +- .../MecanumDriveOdometry.java | 2 +- .../wpilibkinematics/SwerveDriveOdometry.java | 2 +- .../waypoints/GeneralWaypoint.java | 2 +- .../solverslib/util/Timing.java | 2 +- .../CommandSample/DriveSubsystem.java | 8 +- .../ftc/teamcode/DeadWheelsSample.java | 4 +- 14 files changed, 236 insertions(+), 176 deletions(-) create mode 100644 core/src/main/java/com/seattlesolvers/solverslib/hardware/IncrementalEncoder.java diff --git a/core/src/main/java/com/seattlesolvers/solverslib/command/old/Subsystem.java b/core/src/main/java/com/seattlesolvers/solverslib/command/old/Subsystem.java index d7d80ee7..3cbf5314 100644 --- a/core/src/main/java/com/seattlesolvers/solverslib/command/old/Subsystem.java +++ b/core/src/main/java/com/seattlesolvers/solverslib/command/old/Subsystem.java @@ -16,7 +16,7 @@ public interface Subsystem { public void initialize(); /** - * The reset method. Returns the subsystem back to its original + * The zero method. Returns the subsystem back to its original * position and resets any saved data. */ public void reset(); diff --git a/core/src/main/java/com/seattlesolvers/solverslib/hardware/AbsoluteAnalogEncoder.java b/core/src/main/java/com/seattlesolvers/solverslib/hardware/AbsoluteAnalogEncoder.java index 6404f1cb..646568cf 100644 --- a/core/src/main/java/com/seattlesolvers/solverslib/hardware/AbsoluteAnalogEncoder.java +++ b/core/src/main/java/com/seattlesolvers/solverslib/hardware/AbsoluteAnalogEncoder.java @@ -16,6 +16,7 @@ public class AbsoluteAnalogEncoder extends EncoderBase { private final AnalogInput encoder; private final String id; private final double range; + private double offset; /** * The constructor for absolute analog encoders @@ -28,6 +29,7 @@ public AbsoluteAnalogEncoder(HardwareMap hwMap, String id, double range, AngleUn this.angleUnit = angleUnit; this.range = range; this.id = id; + offset = 0.0; reversed = false; } @@ -50,15 +52,29 @@ public AnalogInput getEncoder() { /** * @return The raw voltage returned by the encoder */ - public double getVoltage(){ + public double getVoltage() { return encoder.getVoltage(); } - @Override public double getRawAngle() { return getVoltage() / range * MathUtils.returnMaxForAngleUnit(angleUnit); } + @Override + public EncoderBase zero() { + offset += getAngle(); + return this; + } + + @Override + public double getAngle() { + return MathUtils.normalizeAngle( + getDirectionMultiplier() * (getRawAngle() + offset), + true, + angleUnit + ); + } + @Override public void disable() { // "take no action" (encoder.close() call in SDK) diff --git a/core/src/main/java/com/seattlesolvers/solverslib/hardware/Encoder.java b/core/src/main/java/com/seattlesolvers/solverslib/hardware/Encoder.java index 31762eaa..c40c5af7 100644 --- a/core/src/main/java/com/seattlesolvers/solverslib/hardware/Encoder.java +++ b/core/src/main/java/com/seattlesolvers/solverslib/hardware/Encoder.java @@ -1,5 +1,7 @@ package com.seattlesolvers.solverslib.hardware; +import com.seattlesolvers.solverslib.hardware.motors.Motor; + import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; public interface Encoder extends HardwareDevice { @@ -20,15 +22,27 @@ public interface Encoder extends HardwareDevice { */ Encoder setReversed(boolean reversed); + /** + * Sets whether or not the encoder should be reversed for any future values returned when reading the encoder + * @param direction The direction of the encoder should be reversed for any future values + * @return The object itself for chaining purposes + */ + Encoder setDirection(Motor.Direction direction); + /** * @return Whether the encoder is reversed */ boolean getReversed(); /** - * Sets an angular offset for any future values returned when reading the encoder - * @param offset The angular offset in the units specified by the user previously + * Resets the encoder for any future values returned + * @return The object itself for chaining purposes + */ + Encoder zero(); + + /** + * Resets the offset for any future values returned * @return The object itself for chaining purposes */ - Encoder resetAngle(double offset); + Encoder resetOffset(); } diff --git a/core/src/main/java/com/seattlesolvers/solverslib/hardware/EncoderBase.java b/core/src/main/java/com/seattlesolvers/solverslib/hardware/EncoderBase.java index ddd36e4b..3ad06bc6 100644 --- a/core/src/main/java/com/seattlesolvers/solverslib/hardware/EncoderBase.java +++ b/core/src/main/java/com/seattlesolvers/solverslib/hardware/EncoderBase.java @@ -1,14 +1,22 @@ package com.seattlesolvers.solverslib.hardware; +import com.seattlesolvers.solverslib.hardware.motors.Motor; import com.seattlesolvers.solverslib.util.MathUtils; import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; public abstract class EncoderBase implements Encoder { + protected double offset = 0.0; protected boolean reversed = false; - protected double angleOffset = 0.0; protected AngleUnit angleUnit; + /** + * @return 1 for normal, -1 for reversed + */ + public int getDirectionMultiplier() { + return reversed ? -1 : 1; + } + @Override public EncoderBase setReversed(boolean reversed) { this.reversed = reversed; @@ -16,23 +24,13 @@ public EncoderBase setReversed(boolean reversed) { } @Override - public boolean getReversed() { - return reversed; + public EncoderBase setDirection(Motor.Direction direction) { + return setReversed(direction == Motor.Direction.REVERSE); } @Override - public EncoderBase resetAngle(double offset) { - this.angleOffset = offset; - return this; - } - - @Override - public double getAngle() { - return MathUtils.normalizeAngle( - (reversed ? -1 : 1) * (getRawAngle() + angleOffset), - true, - angleUnit - ); + public boolean getReversed() { + return reversed; } @Override @@ -40,8 +38,9 @@ public AngleUnit getAngleUnit() { return angleUnit; } - /** - * @return The raw calculated angle, before reversing or offsetting - */ - public abstract double getRawAngle(); + @Override + public EncoderBase resetOffset() { + this.offset = 0; + return this; + } } \ No newline at end of file diff --git a/core/src/main/java/com/seattlesolvers/solverslib/hardware/IncrementalEncoder.java b/core/src/main/java/com/seattlesolvers/solverslib/hardware/IncrementalEncoder.java new file mode 100644 index 00000000..a0bf4e2e --- /dev/null +++ b/core/src/main/java/com/seattlesolvers/solverslib/hardware/IncrementalEncoder.java @@ -0,0 +1,153 @@ +package com.seattlesolvers.solverslib.hardware; + +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.HardwareMap; +import com.seattlesolvers.solverslib.util.MathUtils; + +public class IncrementalEncoder extends EncoderBase { + private final DcMotor encoder; + private final double ticksPerRevolution; + + private int lastPosition; + private double lastTimeStamp, veloEstimate, dpp, accel, lastVelo; + + /** + * The constructor for incremental encoders + * @param hwMap the hardwareMap + * @param id the ID of the encoder as configured + * @param ticksPerRevolution the number of encoder ticks per full revolution + */ + public IncrementalEncoder(HardwareMap hwMap, String id, double ticksPerRevolution) { + this(hwMap.get(DcMotor.class, id), ticksPerRevolution); + } + + /** + * The constructor for incremental encoders + * @param encoder the DcMotor which encoder is bound to + * @param ticksPerRevolution the number of encoder ticks per full revolution + */ + public IncrementalEncoder(DcMotor encoder, double ticksPerRevolution) { + this.encoder = encoder; + this.ticksPerRevolution = ticksPerRevolution; + dpp = 1; + lastPosition = 0; + veloEstimate = 0; + lastTimeStamp = (double) System.nanoTime() / 1E9; + } + + /** + * @return the current position of the encoder + */ + public int getPosition() { + int currentPosition = encoder.getCurrentPosition(); + if (currentPosition != lastPosition) { + double currentTime = (double) System.nanoTime() / 1E9; + double dt = currentTime - lastTimeStamp; + veloEstimate = (currentPosition - lastPosition) / dt; + lastPosition = currentPosition; + lastTimeStamp = currentTime; + } + return getDirectionMultiplier() * currentPosition - (int) offset; + } + + /** + * @return the distance per pulse + */ + public double getDpp() { + return dpp; + } + + /** + * @return the distance traveled by the encoder + */ + public double getDistance() { + return dpp * getPosition(); + } + + /** + * @return the velocity of the encoder adjusted to account for the distance per pulse + */ + public double getRate() { + return dpp * ((DcMotorEx) encoder).getVelocity(); + } + + /** + * Sets the distance per pulse of the encoder. + * + * @param distancePerPulse the desired distance per pulse (in units per tick) + */ + public IncrementalEncoder setDistancePerPulse(double distancePerPulse) { + dpp = distancePerPulse; + return this; + } + + /** + * @return the number of revolutions turned by the encoder + */ + public double getRevolutions() { + return getPosition() / ticksPerRevolution; + } + + /** + * @return the raw velocity of the motor reported by the encoder + */ + public double getRawVelocity() { + double velo = ((DcMotorEx) encoder).getVelocity(); + if (velo != lastVelo) { + double currentTime = (double) System.nanoTime() / 1E9; + double dt = currentTime - lastTimeStamp; + accel = (velo - lastVelo) / dt; + lastVelo = velo; + lastTimeStamp = currentTime; + } + return velo; + } + + /** + * @return the estimated acceleration of the motor in ticks per second squared + */ + public double getAcceleration() { + return accel; + } + + private final static int CPS_STEP = 0x10000; + + /** + * Corrects for velocity overflow + * + * @return the corrected velocity + */ + public double getCorrectedVelocity() { + double real = getRawVelocity(); + while (Math.abs(veloEstimate - real) > CPS_STEP / 2.0) { + real += Math.signum(veloEstimate - real) * CPS_STEP; + } + return real; + } + + @Override + public IncrementalEncoder zero() { + offset = getPosition(); + return this; + } + + @Override + public double getAngle() { // TODO: implement + return MathUtils.normalizeAngle( + (getPosition() % ticksPerRevolution) * 360, + true, + angleUnit + ); + } + + @Override + public void disable() { + // "take no action" (encoder.close() call in SDK) + } + + @Override + public String getDeviceType() { + return "Incremental Encoder; " + encoder.getDeviceName(); + } +} \ No newline at end of file diff --git a/core/src/main/java/com/seattlesolvers/solverslib/hardware/motors/Motor.java b/core/src/main/java/com/seattlesolvers/solverslib/hardware/motors/Motor.java index 02667b58..5fdf1679 100644 --- a/core/src/main/java/com/seattlesolvers/solverslib/hardware/motors/Motor.java +++ b/core/src/main/java/com/seattlesolvers/solverslib/hardware/motors/Motor.java @@ -10,8 +10,7 @@ import com.seattlesolvers.solverslib.controller.PIDController; import com.seattlesolvers.solverslib.controller.wpilibcontroller.SimpleMotorFeedforward; import com.seattlesolvers.solverslib.hardware.HardwareDevice; - -import java.util.function.Supplier; +import com.seattlesolvers.solverslib.hardware.IncrementalEncoder; /** * This is the common wrapper for the {@link DcMotor} object in the @@ -26,7 +25,7 @@ public enum GoBILDA { RPM_117(1425.2, 117), RPM_223(753.2, 223), RPM_312(537.6, 312), RPM_435(383.6, 435), RPM_1150(145.6, 1150), RPM_1620(103.6, 1620), BARE(28, 6000), NONE(0, 0); - private double cpr, rpm; + final private double cpr, rpm; GoBILDA(double cpr, double rpm) { this.cpr = cpr; @@ -49,7 +48,7 @@ public double getAchievableMaxTicksPerSecond() { public enum Direction { FORWARD(1), REVERSE(-1); - private int val; + final private int val; Direction(int multiplier) { val = multiplier; @@ -60,130 +59,6 @@ public int getMultiplier() { } } - public class Encoder { - - private Supplier m_position; - private int resetVal, lastPosition; - private Direction direction; - private double lastTimeStamp, veloEstimate, dpp, accel, lastVelo; - - /** - * The encoder object for the motor. - * - * @param position the position supplier which just points to the - * current position of the motor in ticks - */ - public Encoder(Supplier position) { - m_position = position; - dpp = 1; - resetVal = 0; - lastPosition = 0; - veloEstimate = 0; - direction = Direction.FORWARD; - lastTimeStamp = (double) System.nanoTime() / 1E9; - } - - /** - * @return the current position of the encoder - */ - public int getPosition() { - int currentPosition = m_position.get(); - if (currentPosition != lastPosition) { - double currentTime = (double) System.nanoTime() / 1E9; - double dt = currentTime - lastTimeStamp; - veloEstimate = (currentPosition - lastPosition) / dt; - lastPosition = currentPosition; - lastTimeStamp = currentTime; - } - return direction.getMultiplier() * currentPosition - resetVal; - } - - /** - * @return the distance traveled by the encoder - */ - public double getDistance() { - return dpp * getPosition(); - } - - /** - * @return the velocity of the encoder adjusted to account for the distance per pulse - */ - public double getRate() { - return dpp * getVelocity(); - } - - /** - * Resets the encoder without having to stop the motor. - */ - public void reset() { - resetVal += getPosition(); - } - - /** - * Sets the distance per pulse of the encoder. - * - * @param distancePerPulse the desired distance per pulse (in units per tick) - */ - public Encoder setDistancePerPulse(double distancePerPulse) { - dpp = distancePerPulse; - return this; - } - - /** - * Sets the direction of the encoder to forward or reverse - * - * @param direction the desired direction - */ - public void setDirection(Direction direction) { - this.direction = direction; - } - - /** - * @return the number of revolutions turned by the encoder - */ - public double getRevolutions() { - return getPosition() / getCPR(); - } - - /** - * @return the raw velocity of the motor reported by the encoder - */ - public double getRawVelocity() { - double velo = getVelocity(); - if (velo != lastVelo) { - double currentTime = (double) System.nanoTime() / 1E9; - double dt = currentTime - lastTimeStamp; - accel = (velo - lastVelo) / dt; - lastVelo = velo; - lastTimeStamp = currentTime; - } - return velo; - } - - /** - * @return the estimated acceleration of the motor in ticks per second squared - */ - public double getAcceleration() { - return accel; - } - - private final static int CPS_STEP = 0x10000; - - /** - * Corrects for velocity overflow - * - * @return the corrected velocity - */ - public double getCorrectedVelocity() { - double real = getRawVelocity(); - while (Math.abs(veloEstimate - real) > CPS_STEP / 2.0) { - real += Math.signum(veloEstimate - real) * CPS_STEP; - } - return real; - } - - } - /** * The RunMode of the motor. */ @@ -208,7 +83,7 @@ public DcMotor.ZeroPowerBehavior getBehavior() { } public DcMotor motor; - public Encoder encoder; + public IncrementalEncoder encoder; /** * The runmode of the motor @@ -258,7 +133,7 @@ public Motor(@NonNull HardwareMap hMap, String id) { */ public Motor(@NonNull HardwareMap hMap, String id, @NonNull GoBILDA gobildaType) { motor = hMap.get(DcMotor.class, id); - encoder = new Encoder(motor::getCurrentPosition); + encoder = new IncrementalEncoder(motor, gobildaType.getCPR()); runmode = RunMode.RawPower; type = gobildaType; @@ -309,7 +184,7 @@ public void set(double output) { * @param distancePerPulse the desired distance per pulse * @return an encoder an object with the specified distance per pulse */ - public Encoder setDistancePerPulse(double distancePerPulse) { + public IncrementalEncoder setDistancePerPulse(double distancePerPulse) { return encoder.setDistancePerPulse(distancePerPulse); } @@ -338,14 +213,14 @@ public boolean atTargetPosition() { * Resets the external encoder wrapper value. */ public void resetEncoder() { - encoder.reset(); + encoder.zero(); } /** * Resets the internal position of the motor. */ public void stopAndResetEncoder() { - encoder.resetVal = 0; + encoder.resetOffset(); motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); motor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); } @@ -456,7 +331,7 @@ public double get() { * @param target the target position in ticks */ public void setTargetPosition(int target) { - setTargetDistance(target * encoder.dpp); + setTargetDistance(target * encoder.getDpp()); } /** @@ -483,7 +358,7 @@ public void setPositionTolerance(double tolerance) { /** * Common method for inverting direction of a motor. * - * @param isInverted The state of inversion true is inverted. + * @param isInverted The state of inversion, true is inverted. * @return This object for chaining purposes. */ public Motor setInverted(boolean isInverted) { diff --git a/core/src/main/java/com/seattlesolvers/solverslib/hardware/motors/MotorGroup.java b/core/src/main/java/com/seattlesolvers/solverslib/hardware/motors/MotorGroup.java index a2202ebb..8d96dea2 100644 --- a/core/src/main/java/com/seattlesolvers/solverslib/hardware/motors/MotorGroup.java +++ b/core/src/main/java/com/seattlesolvers/solverslib/hardware/motors/MotorGroup.java @@ -2,6 +2,9 @@ import androidx.annotation.NonNull; +import com.seattlesolvers.solverslib.hardware.Encoder; +import com.seattlesolvers.solverslib.hardware.IncrementalEncoder; + import java.util.Arrays; import java.util.Iterator; import java.util.List; @@ -81,8 +84,8 @@ public Iterator iterator() { } @Override - public Encoder setDistancePerPulse(double distancePerPulse) { - Encoder leaderEncoder = group[0].setDistancePerPulse(distancePerPulse); + public IncrementalEncoder setDistancePerPulse(double distancePerPulse) { + IncrementalEncoder leaderEncoder = group[0].setDistancePerPulse(distancePerPulse); for (int i = 1; i < group.length; i++) { group[i].setDistancePerPulse(distancePerPulse); } diff --git a/core/src/main/java/com/seattlesolvers/solverslib/kinematics/wpilibkinematics/DifferentialDriveOdometry.java b/core/src/main/java/com/seattlesolvers/solverslib/kinematics/wpilibkinematics/DifferentialDriveOdometry.java index 6ebb4a08..b3eb6a79 100644 --- a/core/src/main/java/com/seattlesolvers/solverslib/kinematics/wpilibkinematics/DifferentialDriveOdometry.java +++ b/core/src/main/java/com/seattlesolvers/solverslib/kinematics/wpilibkinematics/DifferentialDriveOdometry.java @@ -21,8 +21,8 @@ * path following. Furthermore, odometry can be used for latency compensation * when using computer-vision systems. * - *

It is important that you reset your encoders to zero before using this class. - * Any subsequent pose resets also require the encoders to be reset to zero. + *

It is important that you zero your encoders to zero before using this class. + * Any subsequent pose resets also require the encoders to be zero to zero. */ public class DifferentialDriveOdometry { private Pose2d m_poseMeters; @@ -58,9 +58,9 @@ public DifferentialDriveOdometry(Rotation2d gyroAngle) { /** * Resets the robot's position on the field. * - *

You NEED to reset your encoders (to zero) when calling this method. + *

You NEED to zero your encoders (to zero) when calling this method. * - *

The gyroscope angle does not need to be reset here on the user's robot code. + *

The gyroscope angle does not need to be zero here on the user's robot code. * The library automatically takes care of offsetting the gyro angle. * * @param poseMeters The position on the field that your robot is at. diff --git a/core/src/main/java/com/seattlesolvers/solverslib/kinematics/wpilibkinematics/MecanumDriveOdometry.java b/core/src/main/java/com/seattlesolvers/solverslib/kinematics/wpilibkinematics/MecanumDriveOdometry.java index bcfa4de4..ae5d1650 100644 --- a/core/src/main/java/com/seattlesolvers/solverslib/kinematics/wpilibkinematics/MecanumDriveOdometry.java +++ b/core/src/main/java/com/seattlesolvers/solverslib/kinematics/wpilibkinematics/MecanumDriveOdometry.java @@ -57,7 +57,7 @@ public MecanumDriveOdometry(MecanumDriveKinematics kinematics, Rotation2d gyroAn /** * Resets the robot's position on the field. * - *

The gyroscope angle does not need to be reset here on the user's robot code. + *

The gyroscope angle does not need to be zero here on the user's robot code. * The library automatically takes care of offsetting the gyro angle. * * @param poseMeters The position on the field that your robot is at. diff --git a/core/src/main/java/com/seattlesolvers/solverslib/kinematics/wpilibkinematics/SwerveDriveOdometry.java b/core/src/main/java/com/seattlesolvers/solverslib/kinematics/wpilibkinematics/SwerveDriveOdometry.java index 3f855c0a..d0e7173a 100644 --- a/core/src/main/java/com/seattlesolvers/solverslib/kinematics/wpilibkinematics/SwerveDriveOdometry.java +++ b/core/src/main/java/com/seattlesolvers/solverslib/kinematics/wpilibkinematics/SwerveDriveOdometry.java @@ -58,7 +58,7 @@ public SwerveDriveOdometry(SwerveDriveKinematics kinematics, Rotation2d gyroAngl /** * Resets the robot's position on the field. * - *

The gyroscope angle does not need to be reset here on the user's robot code. + *

The gyroscope angle does not need to be zero here on the user's robot code. * The library automatically takes care of offsetting the gyro angle. * * @param pose The position on the field that your robot is at. diff --git a/core/src/main/java/com/seattlesolvers/solverslib/purepursuit/waypoints/GeneralWaypoint.java b/core/src/main/java/com/seattlesolvers/solverslib/purepursuit/waypoints/GeneralWaypoint.java index 8fa9814e..56f5efc0 100644 --- a/core/src/main/java/com/seattlesolvers/solverslib/purepursuit/waypoints/GeneralWaypoint.java +++ b/core/src/main/java/com/seattlesolvers/solverslib/purepursuit/waypoints/GeneralWaypoint.java @@ -253,7 +253,7 @@ public GeneralWaypoint disablePreferredAngle() { * Resets this waypoint. This is called by Path. */ public void reset() { - // GeneralWaypoints don't have anything to reset. + // GeneralWaypoints don't have anything to zero. } /** diff --git a/core/src/main/java/com/seattlesolvers/solverslib/util/Timing.java b/core/src/main/java/com/seattlesolvers/solverslib/util/Timing.java index 55b4b3f6..0bf95f6f 100644 --- a/core/src/main/java/com/seattlesolvers/solverslib/util/Timing.java +++ b/core/src/main/java/com/seattlesolvers/solverslib/util/Timing.java @@ -119,7 +119,7 @@ public boolean isTimerOn() { /** * Very simple class for a refresh rate timer. Can be used to limit hardware writing/reading, - * or other fast-time cases. Only uses milliseconds. Starts counting on creation, can be reset. + * or other fast-time cases. Only uses milliseconds. Starts counting on creation, can be zero. */ public class Rate { diff --git a/examples/src/main/java/org/firstinspires/ftc/teamcode/CommandSample/DriveSubsystem.java b/examples/src/main/java/org/firstinspires/ftc/teamcode/CommandSample/DriveSubsystem.java index 3b458cc4..53139c29 100644 --- a/examples/src/main/java/org/firstinspires/ftc/teamcode/CommandSample/DriveSubsystem.java +++ b/examples/src/main/java/org/firstinspires/ftc/teamcode/CommandSample/DriveSubsystem.java @@ -2,7 +2,7 @@ import com.seattlesolvers.solverslib.command.SubsystemBase; import com.seattlesolvers.solverslib.drivebase.DifferentialDrive; -import com.seattlesolvers.solverslib.hardware.motors.Motor.Encoder; +import com.seattlesolvers.solverslib.hardware.IncrementalEncoder; import com.seattlesolvers.solverslib.hardware.motors.MotorEx; import com.qualcomm.robotcore.hardware.HardwareMap; @@ -10,7 +10,7 @@ public class DriveSubsystem extends SubsystemBase { private final DifferentialDrive m_drive; - private final Encoder m_left, m_right; + private final IncrementalEncoder m_left, m_right; private final double WHEEL_DIAMETER; @@ -61,8 +61,8 @@ public double getRightEncoderDistance() { } public void resetEncoders() { - m_left.reset(); - m_right.reset(); + m_left.zero(); + m_right.zero(); } public double getAverageEncoderDistance() { diff --git a/examples/src/main/java/org/firstinspires/ftc/teamcode/DeadWheelsSample.java b/examples/src/main/java/org/firstinspires/ftc/teamcode/DeadWheelsSample.java index 0e1bf5dd..bb806d9c 100644 --- a/examples/src/main/java/org/firstinspires/ftc/teamcode/DeadWheelsSample.java +++ b/examples/src/main/java/org/firstinspires/ftc/teamcode/DeadWheelsSample.java @@ -1,8 +1,8 @@ package org.firstinspires.ftc.teamcode; import com.seattlesolvers.solverslib.drivebase.MecanumDrive; +import com.seattlesolvers.solverslib.hardware.IncrementalEncoder; import com.seattlesolvers.solverslib.hardware.motors.Motor; -import com.seattlesolvers.solverslib.hardware.motors.Motor.Encoder; import com.seattlesolvers.solverslib.hardware.motors.MotorEx; import com.seattlesolvers.solverslib.kinematics.HolonomicOdometry; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; @@ -40,7 +40,7 @@ public class DeadWheelsSample extends LinearOpMode { private MotorEx frontLeft, frontRight, backLeft, backRight; private MecanumDrive driveTrain; private Motor intakeLeft, intakeRight, liftLeft, liftRight; - private Encoder leftOdometer, rightOdometer, centerOdometer; + private IncrementalEncoder leftOdometer, rightOdometer, centerOdometer; private HolonomicOdometry odometry; @Override From 3087eeb7ff7abe7e3c4784663780763ccf3bb14d Mon Sep 17 00:00:00 2001 From: "daniel.yang" Date: Tue, 28 Oct 2025 00:46:38 -0700 Subject: [PATCH 04/23] refactor(util): changed Motor.Direction to RotationDirection --- .../solverslib/hardware/Encoder.java | 4 ++-- .../solverslib/hardware/EncoderBase.java | 7 +++---- .../solverslib/hardware/motors/Motor.java | 14 -------------- .../solverslib/util/RotationDirection.java | 15 +++++++++++++++ .../ftc/teamcode/DeadWheelsSample.java | 3 ++- 5 files changed, 22 insertions(+), 21 deletions(-) create mode 100644 core/src/main/java/com/seattlesolvers/solverslib/util/RotationDirection.java diff --git a/core/src/main/java/com/seattlesolvers/solverslib/hardware/Encoder.java b/core/src/main/java/com/seattlesolvers/solverslib/hardware/Encoder.java index c40c5af7..b0f24417 100644 --- a/core/src/main/java/com/seattlesolvers/solverslib/hardware/Encoder.java +++ b/core/src/main/java/com/seattlesolvers/solverslib/hardware/Encoder.java @@ -1,6 +1,6 @@ package com.seattlesolvers.solverslib.hardware; -import com.seattlesolvers.solverslib.hardware.motors.Motor; +import com.seattlesolvers.solverslib.util.RotationDirection; import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; @@ -27,7 +27,7 @@ public interface Encoder extends HardwareDevice { * @param direction The direction of the encoder should be reversed for any future values * @return The object itself for chaining purposes */ - Encoder setDirection(Motor.Direction direction); + Encoder setDirection(RotationDirection direction); /** * @return Whether the encoder is reversed diff --git a/core/src/main/java/com/seattlesolvers/solverslib/hardware/EncoderBase.java b/core/src/main/java/com/seattlesolvers/solverslib/hardware/EncoderBase.java index 3ad06bc6..71cea9d4 100644 --- a/core/src/main/java/com/seattlesolvers/solverslib/hardware/EncoderBase.java +++ b/core/src/main/java/com/seattlesolvers/solverslib/hardware/EncoderBase.java @@ -1,7 +1,6 @@ package com.seattlesolvers.solverslib.hardware; -import com.seattlesolvers.solverslib.hardware.motors.Motor; -import com.seattlesolvers.solverslib.util.MathUtils; +import com.seattlesolvers.solverslib.util.RotationDirection; import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; @@ -24,8 +23,8 @@ public EncoderBase setReversed(boolean reversed) { } @Override - public EncoderBase setDirection(Motor.Direction direction) { - return setReversed(direction == Motor.Direction.REVERSE); + public EncoderBase setDirection(RotationDirection direction) { + return setReversed(direction == RotationDirection.REVERSE); } @Override diff --git a/core/src/main/java/com/seattlesolvers/solverslib/hardware/motors/Motor.java b/core/src/main/java/com/seattlesolvers/solverslib/hardware/motors/Motor.java index 5fdf1679..5d5594d5 100644 --- a/core/src/main/java/com/seattlesolvers/solverslib/hardware/motors/Motor.java +++ b/core/src/main/java/com/seattlesolvers/solverslib/hardware/motors/Motor.java @@ -45,20 +45,6 @@ public double getAchievableMaxTicksPerSecond() { } } - public enum Direction { - FORWARD(1), REVERSE(-1); - - final private int val; - - Direction(int multiplier) { - val = multiplier; - } - - public int getMultiplier() { - return val; - } - } - /** * The RunMode of the motor. */ diff --git a/core/src/main/java/com/seattlesolvers/solverslib/util/RotationDirection.java b/core/src/main/java/com/seattlesolvers/solverslib/util/RotationDirection.java new file mode 100644 index 00000000..e31a781d --- /dev/null +++ b/core/src/main/java/com/seattlesolvers/solverslib/util/RotationDirection.java @@ -0,0 +1,15 @@ +package com.seattlesolvers.solverslib.util; + +public enum RotationDirection { + FORWARD(1), REVERSE(-1); + + final private int val; + + RotationDirection(int multiplier) { + val = multiplier; + } + + public int getMultiplier() { + return val; + } +} diff --git a/examples/src/main/java/org/firstinspires/ftc/teamcode/DeadWheelsSample.java b/examples/src/main/java/org/firstinspires/ftc/teamcode/DeadWheelsSample.java index bb806d9c..342743dd 100644 --- a/examples/src/main/java/org/firstinspires/ftc/teamcode/DeadWheelsSample.java +++ b/examples/src/main/java/org/firstinspires/ftc/teamcode/DeadWheelsSample.java @@ -8,6 +8,7 @@ import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.seattlesolvers.solverslib.util.RotationDirection; /** * This sample shows how to use dead wheels with external encoders @@ -63,7 +64,7 @@ public void runOpMode() throws InterruptedException { rightOdometer = frontRight.encoder.setDistancePerPulse(DISTANCE_PER_PULSE); centerOdometer = backLeft.encoder.setDistancePerPulse(DISTANCE_PER_PULSE); - rightOdometer.setDirection(Motor.Direction.REVERSE); + rightOdometer.setDirection(RotationDirection.REVERSE); odometry = new HolonomicOdometry( leftOdometer::getDistance, From 3dd5c123efe10e722728b7be9f876e4c12d670d4 Mon Sep 17 00:00:00 2001 From: "daniel.yang" Date: Tue, 28 Oct 2025 00:54:32 -0700 Subject: [PATCH 05/23] fix(hardware): resetOffset() now is void --- .../java/com/seattlesolvers/solverslib/hardware/Encoder.java | 3 +-- .../com/seattlesolvers/solverslib/hardware/EncoderBase.java | 3 +-- 2 files changed, 2 insertions(+), 4 deletions(-) diff --git a/core/src/main/java/com/seattlesolvers/solverslib/hardware/Encoder.java b/core/src/main/java/com/seattlesolvers/solverslib/hardware/Encoder.java index b0f24417..095b6a1e 100644 --- a/core/src/main/java/com/seattlesolvers/solverslib/hardware/Encoder.java +++ b/core/src/main/java/com/seattlesolvers/solverslib/hardware/Encoder.java @@ -42,7 +42,6 @@ public interface Encoder extends HardwareDevice { /** * Resets the offset for any future values returned - * @return The object itself for chaining purposes */ - Encoder resetOffset(); + void resetOffset(); } diff --git a/core/src/main/java/com/seattlesolvers/solverslib/hardware/EncoderBase.java b/core/src/main/java/com/seattlesolvers/solverslib/hardware/EncoderBase.java index 71cea9d4..f97253be 100644 --- a/core/src/main/java/com/seattlesolvers/solverslib/hardware/EncoderBase.java +++ b/core/src/main/java/com/seattlesolvers/solverslib/hardware/EncoderBase.java @@ -38,8 +38,7 @@ public AngleUnit getAngleUnit() { } @Override - public EncoderBase resetOffset() { + public void resetOffset() { this.offset = 0; - return this; } } \ No newline at end of file From cda52064de43b8da6320aa01cf67dee93a3f5bbe Mon Sep 17 00:00:00 2001 From: "daniel.yang" Date: Tue, 28 Oct 2025 01:13:10 -0700 Subject: [PATCH 06/23] feat(hardware): encoder setReversed and getReversed for backwards compat --- .../hardware/AbsoluteAnalogEncoder.java | 1 - .../solverslib/hardware/Encoder.java | 20 ++++++++++++----- .../solverslib/hardware/EncoderBase.java | 22 +++++++++++-------- 3 files changed, 28 insertions(+), 15 deletions(-) diff --git a/core/src/main/java/com/seattlesolvers/solverslib/hardware/AbsoluteAnalogEncoder.java b/core/src/main/java/com/seattlesolvers/solverslib/hardware/AbsoluteAnalogEncoder.java index 646568cf..e4bfae7e 100644 --- a/core/src/main/java/com/seattlesolvers/solverslib/hardware/AbsoluteAnalogEncoder.java +++ b/core/src/main/java/com/seattlesolvers/solverslib/hardware/AbsoluteAnalogEncoder.java @@ -30,7 +30,6 @@ public AbsoluteAnalogEncoder(HardwareMap hwMap, String id, double range, AngleUn this.range = range; this.id = id; offset = 0.0; - reversed = false; } /** diff --git a/core/src/main/java/com/seattlesolvers/solverslib/hardware/Encoder.java b/core/src/main/java/com/seattlesolvers/solverslib/hardware/Encoder.java index 095b6a1e..32650d5d 100644 --- a/core/src/main/java/com/seattlesolvers/solverslib/hardware/Encoder.java +++ b/core/src/main/java/com/seattlesolvers/solverslib/hardware/Encoder.java @@ -16,11 +16,9 @@ public interface Encoder extends HardwareDevice { AngleUnit getAngleUnit(); /** - * Sets whether or not the encoder should be reversed for any future values returned when reading the encoder - * @param reversed Whether or not the encoder should be reversed for any future values - * @return The object itself for chaining purposes + * @return Direction multiplier: 1 for normal, -1 for reversed */ - Encoder setReversed(boolean reversed); + int getDirectionMultiplier(); /** * Sets whether or not the encoder should be reversed for any future values returned when reading the encoder @@ -30,7 +28,19 @@ public interface Encoder extends HardwareDevice { Encoder setDirection(RotationDirection direction); /** - * @return Whether the encoder is reversed + * Sets whether or not the encoder should be reversed for any future values returned when reading the encoder + * @param reversed Whether or not the encoder should be reversed for any future values + * @return The object itself for chaining purposes + */ + Encoder setReversed(boolean reversed); + + /** + * @return The direction of the encoder + */ + RotationDirection getDirection(); + + /** + * @return Whether or not an encoder is reversed */ boolean getReversed(); diff --git a/core/src/main/java/com/seattlesolvers/solverslib/hardware/EncoderBase.java b/core/src/main/java/com/seattlesolvers/solverslib/hardware/EncoderBase.java index f97253be..9ae68671 100644 --- a/core/src/main/java/com/seattlesolvers/solverslib/hardware/EncoderBase.java +++ b/core/src/main/java/com/seattlesolvers/solverslib/hardware/EncoderBase.java @@ -6,30 +6,34 @@ public abstract class EncoderBase implements Encoder { protected double offset = 0.0; - protected boolean reversed = false; + protected RotationDirection direction = RotationDirection.FORWARD; protected AngleUnit angleUnit; - /** - * @return 1 for normal, -1 for reversed - */ + @Override public int getDirectionMultiplier() { - return reversed ? -1 : 1; + return direction.getMultiplier(); + } + + @Override + public EncoderBase setDirection(RotationDirection direction) { + this.direction = direction; + return this; } @Override public EncoderBase setReversed(boolean reversed) { - this.reversed = reversed; + direction = reversed ? RotationDirection.REVERSE : RotationDirection.FORWARD; return this; } @Override - public EncoderBase setDirection(RotationDirection direction) { - return setReversed(direction == RotationDirection.REVERSE); + public RotationDirection getDirection() { + return direction; } @Override public boolean getReversed() { - return reversed; + return direction == RotationDirection.REVERSE; } @Override From b13752207759d166ac1905ef12f30644fc2a0154 Mon Sep 17 00:00:00 2001 From: "daniel.yang" Date: Tue, 28 Oct 2025 01:15:17 -0700 Subject: [PATCH 07/23] feat(hardware): angle unit bugfix --- .../solverslib/hardware/IncrementalEncoder.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/core/src/main/java/com/seattlesolvers/solverslib/hardware/IncrementalEncoder.java b/core/src/main/java/com/seattlesolvers/solverslib/hardware/IncrementalEncoder.java index a0bf4e2e..8926cfd4 100644 --- a/core/src/main/java/com/seattlesolvers/solverslib/hardware/IncrementalEncoder.java +++ b/core/src/main/java/com/seattlesolvers/solverslib/hardware/IncrementalEncoder.java @@ -133,9 +133,9 @@ public IncrementalEncoder zero() { } @Override - public double getAngle() { // TODO: implement + public double getAngle() { return MathUtils.normalizeAngle( - (getPosition() % ticksPerRevolution) * 360, + (getPosition() % ticksPerRevolution) * MathUtils.returnMaxForAngleUnit(angleUnit), true, angleUnit ); From bed0a8b6e177545aa56f97ac7381211672ae6cda Mon Sep 17 00:00:00 2001 From: "daniel.yang" Date: Mon, 3 Nov 2025 08:10:52 -0800 Subject: [PATCH 08/23] feat: CallbackCommand added and methods in Command --- .../solverslib/command/CallbackCommand.java | 121 ++++++++++++++++++ .../solverslib/command/Command.java | 21 +++ .../command/UninterruptibleCommand.java | 4 +- 3 files changed, 144 insertions(+), 2 deletions(-) create mode 100644 core/src/main/java/com/seattlesolvers/solverslib/command/CallbackCommand.java diff --git a/core/src/main/java/com/seattlesolvers/solverslib/command/CallbackCommand.java b/core/src/main/java/com/seattlesolvers/solverslib/command/CallbackCommand.java new file mode 100644 index 00000000..af9e8f0d --- /dev/null +++ b/core/src/main/java/com/seattlesolvers/solverslib/command/CallbackCommand.java @@ -0,0 +1,121 @@ +package com.seattlesolvers.solverslib.command; + +import java.util.HashMap; +import java.util.Iterator; +import java.util.Map; +import java.util.function.BooleanSupplier; +import java.util.function.Function; + +/** + * Schedules a Command as uninterruptible + * @author Daniel - FTC 7854 + */ +public class CallbackCommand extends CommandBase { + private final Map whenRunnables = new HashMap<>(); + private final Map whenCommands = new HashMap<>(); + private final Map, Runnable> whenSelfRunnables = new HashMap<>(); + private final Map, Command> whenSelfCommands = new HashMap<>(); + private final Command command; + + /** + * Wrapper for adding custom callbacks to commands. This expects a single command, + * so multiple commands need to be put in a CommandGroup first: + * @param command the command to be schedules as uninterruptible + * {@link SequentialCommandGroup} + * {@link ParallelCommandGroup} + */ + public CallbackCommand(Command command) { + this.command = command; + } + + /** + * Adds a callback with a boolean supplier + * @param condition Runs the runnable the first time this is true + * @param runnable Callback to run + * @return Itself for chaining purposes + */ + @Override + public CallbackCommand when(BooleanSupplier condition, Runnable runnable) { + whenRunnables.put(condition, runnable); + return this; + } + + /** + * Adds a callback with a boolean supplier + * @param condition Schedules the command the first time this is true + * @param command Command to schedule + * @return Itself for chaining purposes + */ + @Override + public CallbackCommand when(BooleanSupplier condition, Command command) { + whenCommands.put(condition, command); + return this; + } + + /** + * Adds a callback with access to the inner command + * @param condition Runs the runnable the first time this is true + * @param runnable Callback to run + * @return Itself for chaining purposes + */ + @Override + public CallbackCommand whenSelf(Function condition, Runnable runnable) { + whenSelfRunnables.put(condition, runnable); + return this; + } + + /** + * Adds a callback with access to the inner command + * @param condition Schedules the command the first time this is true + * @param command Command to schedule + * @return Itself for chaining purposes + */ + @Override + public CallbackCommand whenSelf(Function condition, Command command) { + whenSelfCommands.put(condition, command); + return this; + } + + @Override + public void initialize() { + command.schedule(); + } + + @Override + public void execute() { + for (Iterator> it = whenRunnables.entrySet().iterator(); it.hasNext();) { + Map.Entry action = it.next(); + if (action.getKey().getAsBoolean()) { + action.getValue().run(); + it.remove(); + } + } + for (Iterator> it = whenCommands.entrySet().iterator(); it.hasNext();) { + Map.Entry action = it.next(); + if (action.getKey().getAsBoolean()) { + action.getValue().schedule(); + it.remove(); + } + } + + for (Iterator, Runnable>> it = whenSelfRunnables.entrySet().iterator(); it.hasNext();) { + Map.Entry, Runnable> action = it.next(); + if (action.getKey().apply(command)) { + action.getValue().run(); + it.remove(); + } + } + for (Iterator, Command>> it = whenSelfCommands.entrySet().iterator(); it.hasNext();) { + Map.Entry, Command> action = it.next(); + if (action.getKey().apply(command)) { + action.getValue().schedule(); + it.remove(); + } + } + } + + @Override + public boolean isFinished() { + return !CommandScheduler.getInstance().isScheduled(command); + } +} diff --git a/core/src/main/java/com/seattlesolvers/solverslib/command/Command.java b/core/src/main/java/com/seattlesolvers/solverslib/command/Command.java index 236f1912..488390d9 100644 --- a/core/src/main/java/com/seattlesolvers/solverslib/command/Command.java +++ b/core/src/main/java/com/seattlesolvers/solverslib/command/Command.java @@ -9,6 +9,7 @@ import java.util.Set; import java.util.function.BooleanSupplier; +import java.util.function.Function; /** * A state machine representing a complete action to be performed by the robot. Commands are @@ -319,4 +320,24 @@ default String getName() { return this.getClass().getSimpleName(); } + default Command setUninterruptible() { + return new UninterruptibleCommand(this); + } + + default Command when(BooleanSupplier condition, Runnable runnable) { + return new CallbackCommand(this).when(condition, runnable); + } + + default Command when(BooleanSupplier condition, Command command) { + return new CallbackCommand(this).when(condition, command); + } + + default Command whenSelf(Function condition, Runnable runnable) { + return new CallbackCommand(this).whenSelf(condition, runnable); + } + + default Command whenSelf(Function condition, Command command) { + return new CallbackCommand(this).whenSelf(condition, command); + } + } \ No newline at end of file diff --git a/core/src/main/java/com/seattlesolvers/solverslib/command/UninterruptibleCommand.java b/core/src/main/java/com/seattlesolvers/solverslib/command/UninterruptibleCommand.java index c69ff1e0..bf425ca4 100644 --- a/core/src/main/java/com/seattlesolvers/solverslib/command/UninterruptibleCommand.java +++ b/core/src/main/java/com/seattlesolvers/solverslib/command/UninterruptibleCommand.java @@ -4,13 +4,13 @@ * Schedules a Command as uninterruptible * @author Arush - FTC 23511 */ - public class UninterruptibleCommand extends CommandBase { +public class UninterruptibleCommand extends CommandBase { private final Command command; /** - * @param command the command to be schedules as uninterruptible * This expects a single command, so multiple commands need to be put in a * CommandGroup first: + * @param command the command to be schedules as uninterruptible * {@link SequentialCommandGroup} * {@link ParallelCommandGroup} */ From 40241b184ebc6d118204222b2ffa65c925bf62bb Mon Sep 17 00:00:00 2001 From: "daniel.yang" Date: Mon, 3 Nov 2025 08:10:52 -0800 Subject: [PATCH 09/23] feat: CallbackCommand added and methods in Command --- .../solverslib/command/CallbackCommand.java | 123 ++++++++++++++++++ .../solverslib/command/Command.java | 21 +++ .../command/UninterruptibleCommand.java | 4 +- 3 files changed, 146 insertions(+), 2 deletions(-) create mode 100644 core/src/main/java/com/seattlesolvers/solverslib/command/CallbackCommand.java diff --git a/core/src/main/java/com/seattlesolvers/solverslib/command/CallbackCommand.java b/core/src/main/java/com/seattlesolvers/solverslib/command/CallbackCommand.java new file mode 100644 index 00000000..03461c4b --- /dev/null +++ b/core/src/main/java/com/seattlesolvers/solverslib/command/CallbackCommand.java @@ -0,0 +1,123 @@ +package com.seattlesolvers.solverslib.command; + +import java.util.HashMap; +import java.util.Iterator; +import java.util.Map; +import java.util.function.BooleanSupplier; +import java.util.function.Function; + +/** + * Schedules a Command as uninterruptible + * @author Daniel - FTC 7854 + */ +public class CallbackCommand extends CommandBase { + private final Map whenRunnables = new HashMap<>(); + private final Map whenCommands = new HashMap<>(); + private final Map, Runnable> whenSelfRunnables = new HashMap<>(); + private final Map, Command> whenSelfCommands = new HashMap<>(); + private final Command command; + + /** + * Wrapper for adding custom callbacks to commands. This expects a single command, + * so multiple commands need to be put in a CommandGroup first: + * @param command the command to be schedules as uninterruptible + * {@link SequentialCommandGroup} + * {@link ParallelCommandGroup} + */ + public CallbackCommand(Command command) { + this.command = command; + } + + /** + * Adds a callback with a boolean supplier + * @param condition Runs the runnable the first time this is true + * @param runnable Callback to run + * @return Itself for chaining purposes + */ + @Override + public CallbackCommand when(BooleanSupplier condition, Runnable runnable) { + whenRunnables.put(condition, runnable); + return this; + } + + /** + * Adds a callback with a boolean supplier + * @param condition Schedules the command the first time this is true + * @param command Command to schedule + * @return Itself for chaining purposes + */ + @Override + public CallbackCommand when(BooleanSupplier condition, Command command) { + whenCommands.put(condition, command); + return this; + } + + /** + * Adds a callback with access to the inner command + * @param condition Runs the runnable the first time this is true + * @param runnable Callback to run + * @return Itself for chaining purposes + */ + @Override + public CallbackCommand whenSelf(Function condition, Runnable runnable) { + whenSelfRunnables.put(condition, runnable); + return this; + } + + /** + * Adds a callback with access to the inner command + * @param condition Schedules the command the first time this is true + * @param command Command to schedule + * @return Itself for chaining purposes + */ + @Override + public CallbackCommand whenSelf(Function condition, Command command) { + whenSelfCommands.put(condition, command); + return this; + } + + @Override + public void initialize() { + command.schedule(); + } + + @Override + public void execute() { + // Callbacks + for (Iterator> it = whenRunnables.entrySet().iterator(); it.hasNext();) { + Map.Entry action = it.next(); + if (action.getKey().getAsBoolean()) { + action.getValue().run(); + it.remove(); + } + } + for (Iterator> it = whenCommands.entrySet().iterator(); it.hasNext();) { + Map.Entry action = it.next(); + if (action.getKey().getAsBoolean()) { + action.getValue().schedule(); + it.remove(); + } + } + + // Self callbacks + for (Iterator, Runnable>> it = whenSelfRunnables.entrySet().iterator(); it.hasNext();) { + Map.Entry, Runnable> action = it.next(); + if (action.getKey().apply(command)) { + action.getValue().run(); + it.remove(); + } + } + for (Iterator, Command>> it = whenSelfCommands.entrySet().iterator(); it.hasNext();) { + Map.Entry, Command> action = it.next(); + if (action.getKey().apply(command)) { + action.getValue().schedule(); + it.remove(); + } + } + } + + @Override + public boolean isFinished() { + return !CommandScheduler.getInstance().isScheduled(command); + } +} diff --git a/core/src/main/java/com/seattlesolvers/solverslib/command/Command.java b/core/src/main/java/com/seattlesolvers/solverslib/command/Command.java index 236f1912..488390d9 100644 --- a/core/src/main/java/com/seattlesolvers/solverslib/command/Command.java +++ b/core/src/main/java/com/seattlesolvers/solverslib/command/Command.java @@ -9,6 +9,7 @@ import java.util.Set; import java.util.function.BooleanSupplier; +import java.util.function.Function; /** * A state machine representing a complete action to be performed by the robot. Commands are @@ -319,4 +320,24 @@ default String getName() { return this.getClass().getSimpleName(); } + default Command setUninterruptible() { + return new UninterruptibleCommand(this); + } + + default Command when(BooleanSupplier condition, Runnable runnable) { + return new CallbackCommand(this).when(condition, runnable); + } + + default Command when(BooleanSupplier condition, Command command) { + return new CallbackCommand(this).when(condition, command); + } + + default Command whenSelf(Function condition, Runnable runnable) { + return new CallbackCommand(this).whenSelf(condition, runnable); + } + + default Command whenSelf(Function condition, Command command) { + return new CallbackCommand(this).whenSelf(condition, command); + } + } \ No newline at end of file diff --git a/core/src/main/java/com/seattlesolvers/solverslib/command/UninterruptibleCommand.java b/core/src/main/java/com/seattlesolvers/solverslib/command/UninterruptibleCommand.java index c69ff1e0..bf425ca4 100644 --- a/core/src/main/java/com/seattlesolvers/solverslib/command/UninterruptibleCommand.java +++ b/core/src/main/java/com/seattlesolvers/solverslib/command/UninterruptibleCommand.java @@ -4,13 +4,13 @@ * Schedules a Command as uninterruptible * @author Arush - FTC 23511 */ - public class UninterruptibleCommand extends CommandBase { +public class UninterruptibleCommand extends CommandBase { private final Command command; /** - * @param command the command to be schedules as uninterruptible * This expects a single command, so multiple commands need to be put in a * CommandGroup first: + * @param command the command to be schedules as uninterruptible * {@link SequentialCommandGroup} * {@link ParallelCommandGroup} */ From 1635ef1839e31fa796c499915227c8c990cd74da Mon Sep 17 00:00:00 2001 From: "daniel.yang" Date: Mon, 3 Nov 2025 13:46:06 -0800 Subject: [PATCH 10/23] feat: upgrade Encoder and EncoderBase --- .../hardware/AbsoluteAnalogEncoder.java | 25 ++---- .../solverslib/hardware/Encoder.java | 25 +++++- .../solverslib/hardware/EncoderBase.java | 32 ++++++-- .../hardware/IncrementalEncoder.java | 81 +++++++++++++------ 4 files changed, 108 insertions(+), 55 deletions(-) diff --git a/core/src/main/java/com/seattlesolvers/solverslib/hardware/AbsoluteAnalogEncoder.java b/core/src/main/java/com/seattlesolvers/solverslib/hardware/AbsoluteAnalogEncoder.java index e4bfae7e..3b55c4ad 100644 --- a/core/src/main/java/com/seattlesolvers/solverslib/hardware/AbsoluteAnalogEncoder.java +++ b/core/src/main/java/com/seattlesolvers/solverslib/hardware/AbsoluteAnalogEncoder.java @@ -11,12 +11,10 @@ * * @author Saket */ -public class AbsoluteAnalogEncoder extends EncoderBase { +public class AbsoluteAnalogEncoder extends EncoderBase { public static double DEFAULT_RANGE = 3.3; - private final AnalogInput encoder; private final String id; private final double range; - private double offset; /** * The constructor for absolute analog encoders @@ -25,11 +23,10 @@ public class AbsoluteAnalogEncoder extends EncoderBase { * @param range the range of voltage returned by the sensor */ public AbsoluteAnalogEncoder(HardwareMap hwMap, String id, double range, AngleUnit angleUnit) { - this.encoder = hwMap.get(AnalogInput.class, id); + super(hwMap.get(AnalogInput.class, id)); this.angleUnit = angleUnit; this.range = range; this.id = id; - offset = 0.0; } /** @@ -41,13 +38,6 @@ public AbsoluteAnalogEncoder(HardwareMap hwMap, String id) { this(hwMap, id, DEFAULT_RANGE, AngleUnit.RADIANS); } - /** - * @return The AnalogInput object of the encoder itself - */ - public AnalogInput getEncoder() { - return encoder; - } - /** * @return The raw voltage returned by the encoder */ @@ -60,9 +50,9 @@ public double getRawAngle() { } @Override - public EncoderBase zero() { - offset += getAngle(); - return this; + public AbsoluteAnalogEncoder setAngle(double angle) { + offset += getAngle() - angle; + return null; } @Override @@ -74,11 +64,6 @@ public double getAngle() { ); } - @Override - public void disable() { - // "take no action" (encoder.close() call in SDK) - } - @Override public String getDeviceType() { return "Absolute Analog Encoder; " + id; diff --git a/core/src/main/java/com/seattlesolvers/solverslib/hardware/Encoder.java b/core/src/main/java/com/seattlesolvers/solverslib/hardware/Encoder.java index 32650d5d..b9af609a 100644 --- a/core/src/main/java/com/seattlesolvers/solverslib/hardware/Encoder.java +++ b/core/src/main/java/com/seattlesolvers/solverslib/hardware/Encoder.java @@ -1,10 +1,11 @@ package com.seattlesolvers.solverslib.hardware; +import com.seattlesolvers.solverslib.hardware.HardwareDevice; import com.seattlesolvers.solverslib.util.RotationDirection; import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; -public interface Encoder extends HardwareDevice { +public interface Encoder> extends HardwareDevice { /** * @return The normalized angular position of the encoder */ @@ -25,14 +26,14 @@ public interface Encoder extends HardwareDevice { * @param direction The direction of the encoder should be reversed for any future values * @return The object itself for chaining purposes */ - Encoder setDirection(RotationDirection direction); + T setDirection(RotationDirection direction); /** * Sets whether or not the encoder should be reversed for any future values returned when reading the encoder * @param reversed Whether or not the encoder should be reversed for any future values * @return The object itself for chaining purposes */ - Encoder setReversed(boolean reversed); + T setReversed(boolean reversed); /** * @return The direction of the encoder @@ -48,10 +49,26 @@ public interface Encoder extends HardwareDevice { * Resets the encoder for any future values returned * @return The object itself for chaining purposes */ - Encoder zero(); + T zero(); + + /** + * Resets the encoder to a particular angle + * @return The object itself for chaining purposes + */ + T setAngle(double angle); /** * Resets the offset for any future values returned */ void resetOffset(); + + /** + * @return The inner encoder + */ + E getEncoder(); + + @Override + default void disable() { + // "take no action" (encoder.close() call in SDK) + } } diff --git a/core/src/main/java/com/seattlesolvers/solverslib/hardware/EncoderBase.java b/core/src/main/java/com/seattlesolvers/solverslib/hardware/EncoderBase.java index 9ae68671..61552913 100644 --- a/core/src/main/java/com/seattlesolvers/solverslib/hardware/EncoderBase.java +++ b/core/src/main/java/com/seattlesolvers/solverslib/hardware/EncoderBase.java @@ -1,13 +1,19 @@ package com.seattlesolvers.solverslib.hardware; +import com.qualcomm.robotcore.hardware.AnalogInput; import com.seattlesolvers.solverslib.util.RotationDirection; import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; -public abstract class EncoderBase implements Encoder { +public abstract class EncoderBase> implements Encoder { protected double offset = 0.0; protected RotationDirection direction = RotationDirection.FORWARD; - protected AngleUnit angleUnit; + protected AngleUnit angleUnit = AngleUnit.RADIANS; + protected final E encoder; + + protected EncoderBase(E encoder) { + this.encoder = encoder; + } @Override public int getDirectionMultiplier() { @@ -15,15 +21,24 @@ public int getDirectionMultiplier() { } @Override - public EncoderBase setDirection(RotationDirection direction) { + @SuppressWarnings("unchecked") + public T setDirection(RotationDirection direction) { this.direction = direction; - return this; + return (T) this; } @Override - public EncoderBase setReversed(boolean reversed) { + @SuppressWarnings("unchecked") + public T setReversed(boolean reversed) { direction = reversed ? RotationDirection.REVERSE : RotationDirection.FORWARD; - return this; + return (T) this; + } + + @Override + @SuppressWarnings("unchecked") + public T zero() { + this.setAngle(0); + return (T) this; } @Override @@ -45,4 +60,9 @@ public AngleUnit getAngleUnit() { public void resetOffset() { this.offset = 0; } + + @Override + public E getEncoder() { + return encoder; + } } \ No newline at end of file diff --git a/core/src/main/java/com/seattlesolvers/solverslib/hardware/IncrementalEncoder.java b/core/src/main/java/com/seattlesolvers/solverslib/hardware/IncrementalEncoder.java index 8926cfd4..a0d602c2 100644 --- a/core/src/main/java/com/seattlesolvers/solverslib/hardware/IncrementalEncoder.java +++ b/core/src/main/java/com/seattlesolvers/solverslib/hardware/IncrementalEncoder.java @@ -5,35 +5,58 @@ import com.qualcomm.robotcore.hardware.HardwareMap; import com.seattlesolvers.solverslib.util.MathUtils; -public class IncrementalEncoder extends EncoderBase { - private final DcMotor encoder; - private final double ticksPerRevolution; +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; + +public class IncrementalEncoder extends EncoderBase { + private final double cpr; private int lastPosition; private double lastTimeStamp, veloEstimate, dpp, accel, lastVelo; + /** + * The constructor for incremental encoders + * @param encoder the DcMotor which encoder is bound to + * @param countsPerRevolution the number of encoder ticks per full revolution, aka cycles per revolution + * @param angleUnit the angle unit of the encoder + */ + public IncrementalEncoder(DcMotor encoder, double countsPerRevolution, AngleUnit angleUnit) { + super(encoder); + this.cpr = countsPerRevolution; + this.angleUnit = angleUnit; + dpp = 1; + lastPosition = 0; + veloEstimate = 0; + lastTimeStamp = (double) System.nanoTime() / 1E9; + } + /** * The constructor for incremental encoders * @param hwMap the hardwareMap * @param id the ID of the encoder as configured - * @param ticksPerRevolution the number of encoder ticks per full revolution + * @param countsPerRevolution the number of encoder ticks per full revolution, aka cycles per revolution + * @param angleUnit the angle unit of the encoder */ - public IncrementalEncoder(HardwareMap hwMap, String id, double ticksPerRevolution) { - this(hwMap.get(DcMotor.class, id), ticksPerRevolution); + public IncrementalEncoder(HardwareMap hwMap, String id, double countsPerRevolution, AngleUnit angleUnit) { + this(hwMap.get(DcMotor.class, id), countsPerRevolution, angleUnit); } /** - * The constructor for incremental encoders + * The constructor for incremental encoders, defaults to radians * @param encoder the DcMotor which encoder is bound to - * @param ticksPerRevolution the number of encoder ticks per full revolution + * @param countsPerRevolution the number of encoder ticks per full revolution, aka cycles per revolution */ - public IncrementalEncoder(DcMotor encoder, double ticksPerRevolution) { - this.encoder = encoder; - this.ticksPerRevolution = ticksPerRevolution; - dpp = 1; - lastPosition = 0; - veloEstimate = 0; - lastTimeStamp = (double) System.nanoTime() / 1E9; + public IncrementalEncoder(DcMotor encoder, double countsPerRevolution) { + this(encoder, countsPerRevolution, AngleUnit.RADIANS); + } + + /** + * The constructor for incremental encoders, defaults to radians + * @param hwMap the hardwareMap + * @param id the ID of the encoder as configured + * @param countsPerRevolution the number of encoder ticks per full revolution, aka cycles per revolution + */ + public IncrementalEncoder(HardwareMap hwMap, String id, double countsPerRevolution) { + this(hwMap.get(DcMotor.class, id), countsPerRevolution, AngleUnit.RADIANS); } /** @@ -42,7 +65,7 @@ public IncrementalEncoder(DcMotor encoder, double ticksPerRevolution) { public int getPosition() { int currentPosition = encoder.getCurrentPosition(); if (currentPosition != lastPosition) { - double currentTime = (double) System.nanoTime() / 1E9; + double currentTime = System.nanoTime() / 1E9; double dt = currentTime - lastTimeStamp; veloEstimate = (currentPosition - lastPosition) / dt; lastPosition = currentPosition; @@ -86,7 +109,7 @@ public IncrementalEncoder setDistancePerPulse(double distancePerPulse) { * @return the number of revolutions turned by the encoder */ public double getRevolutions() { - return getPosition() / ticksPerRevolution; + return getPosition() / cpr; } /** @@ -126,26 +149,34 @@ public double getCorrectedVelocity() { return real; } + public int angleToTicks(double angle) { + return (int) (angle * cpr / MathUtils.returnMaxForAngleUnit(angleUnit)); + } + + public double getAngleUnnormalized() { + return getRevolutions() * MathUtils.returnMaxForAngleUnit(angleUnit); + } + @Override - public IncrementalEncoder zero() { - offset = getPosition(); + public IncrementalEncoder setAngle(double angle) { + offset = getPosition() - angleToTicks(angle); return this; } + @Override + public DcMotor getEncoder() { + return encoder; + } + @Override public double getAngle() { return MathUtils.normalizeAngle( - (getPosition() % ticksPerRevolution) * MathUtils.returnMaxForAngleUnit(angleUnit), + getAngleUnnormalized(), true, angleUnit ); } - @Override - public void disable() { - // "take no action" (encoder.close() call in SDK) - } - @Override public String getDeviceType() { return "Incremental Encoder; " + encoder.getDeviceName(); From c32af3bfd4b3aea67b39b7ef32b7d8bf94c3ae4c Mon Sep 17 00:00:00 2001 From: "daniel.yang" Date: Mon, 3 Nov 2025 13:59:17 -0800 Subject: [PATCH 11/23] feat: upgrade Encoder and EncoderBase --- .../java/com/seattlesolvers/solverslib/hardware/Encoder.java | 1 - .../java/com/seattlesolvers/solverslib/hardware/EncoderBase.java | 1 - 2 files changed, 2 deletions(-) diff --git a/core/src/main/java/com/seattlesolvers/solverslib/hardware/Encoder.java b/core/src/main/java/com/seattlesolvers/solverslib/hardware/Encoder.java index b9af609a..e25d87f9 100644 --- a/core/src/main/java/com/seattlesolvers/solverslib/hardware/Encoder.java +++ b/core/src/main/java/com/seattlesolvers/solverslib/hardware/Encoder.java @@ -1,6 +1,5 @@ package com.seattlesolvers.solverslib.hardware; -import com.seattlesolvers.solverslib.hardware.HardwareDevice; import com.seattlesolvers.solverslib.util.RotationDirection; import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; diff --git a/core/src/main/java/com/seattlesolvers/solverslib/hardware/EncoderBase.java b/core/src/main/java/com/seattlesolvers/solverslib/hardware/EncoderBase.java index 61552913..e188cbd5 100644 --- a/core/src/main/java/com/seattlesolvers/solverslib/hardware/EncoderBase.java +++ b/core/src/main/java/com/seattlesolvers/solverslib/hardware/EncoderBase.java @@ -1,6 +1,5 @@ package com.seattlesolvers.solverslib.hardware; -import com.qualcomm.robotcore.hardware.AnalogInput; import com.seattlesolvers.solverslib.util.RotationDirection; import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; From 9021cda26f2a03c837c39844fb65e7a6b385e516 Mon Sep 17 00:00:00 2001 From: "daniel.yang" Date: Mon, 3 Nov 2025 15:27:49 -0800 Subject: [PATCH 12/23] feat: upgrade Encoder and EncoderBase --- .../hardware/AbsoluteAnalogEncoder.java | 30 +++++++++++++++++++ .../solverslib/hardware/Encoder.java | 18 ++++++++--- .../solverslib/hardware/EncoderBase.java | 28 ++++++++++++----- 3 files changed, 64 insertions(+), 12 deletions(-) diff --git a/core/src/main/java/com/seattlesolvers/solverslib/hardware/AbsoluteAnalogEncoder.java b/core/src/main/java/com/seattlesolvers/solverslib/hardware/AbsoluteAnalogEncoder.java index 3b55c4ad..1d6066ac 100644 --- a/core/src/main/java/com/seattlesolvers/solverslib/hardware/AbsoluteAnalogEncoder.java +++ b/core/src/main/java/com/seattlesolvers/solverslib/hardware/AbsoluteAnalogEncoder.java @@ -49,6 +49,36 @@ public double getRawAngle() { return getVoltage() / range * MathUtils.returnMaxForAngleUnit(angleUnit); } + /** + * Alias for {@link #getAngle()} for backwards compatibility + * @return angle of the encoder + */ + @Deprecated + public double getCurrentPosition() { + return getAngle(); + } + + + /** + * Manually sets the offset. Use setOffset instead + * @param offset The offset angle + * @return The object itself for chaining purposes + */ + @Deprecated + public AbsoluteAnalogEncoder zero(double offset) { + return setOffset(offset); + } + + /** + * Manually sets the offset. + * @param offset The offset angle + * @return The object itself for chaining purposes + */ + public AbsoluteAnalogEncoder setOffset(double offset) { + this.offset = offset; + return this; + } + @Override public AbsoluteAnalogEncoder setAngle(double angle) { offset += getAngle() - angle; diff --git a/core/src/main/java/com/seattlesolvers/solverslib/hardware/Encoder.java b/core/src/main/java/com/seattlesolvers/solverslib/hardware/Encoder.java index e25d87f9..3e7bf1d3 100644 --- a/core/src/main/java/com/seattlesolvers/solverslib/hardware/Encoder.java +++ b/core/src/main/java/com/seattlesolvers/solverslib/hardware/Encoder.java @@ -4,6 +4,11 @@ import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +/** + * + * @param + * @param + */ public interface Encoder> extends HardwareDevice { /** * @return The normalized angular position of the encoder @@ -45,21 +50,26 @@ public interface Encoder> extends HardwareDevice { boolean getReversed(); /** - * Resets the encoder for any future values returned + * Resets the encoder to 0 * @return The object itself for chaining purposes */ T zero(); /** - * Resets the encoder to a particular angle + * Resets the encoder to a particular angle. * @return The object itself for chaining purposes */ T setAngle(double angle); /** - * Resets the offset for any future values returned + * Manually zeroes the offset for any future values returned */ - void resetOffset(); + T resetOffset(); + + /** + * Manually sets the offset for any future values returned + */ + T setOffset(double offset); /** * @return The inner encoder diff --git a/core/src/main/java/com/seattlesolvers/solverslib/hardware/EncoderBase.java b/core/src/main/java/com/seattlesolvers/solverslib/hardware/EncoderBase.java index e188cbd5..cf88580e 100644 --- a/core/src/main/java/com/seattlesolvers/solverslib/hardware/EncoderBase.java +++ b/core/src/main/java/com/seattlesolvers/solverslib/hardware/EncoderBase.java @@ -4,6 +4,11 @@ import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +/** + * Provides basic implementations for the {@link Encoder} interface. + * @param The encoder type (e.g. DcMotor, AnalogInput) + * @param The encoder class itself (e.g. IncrementalEncoder, AbsoluteAnalogEncoder) + */ public abstract class EncoderBase> implements Encoder { protected double offset = 0.0; protected RotationDirection direction = RotationDirection.FORWARD; @@ -19,27 +24,39 @@ public int getDirectionMultiplier() { return direction.getMultiplier(); } - @Override @SuppressWarnings("unchecked") + @Override public T setDirection(RotationDirection direction) { this.direction = direction; return (T) this; } - @Override @SuppressWarnings("unchecked") + @Override public T setReversed(boolean reversed) { direction = reversed ? RotationDirection.REVERSE : RotationDirection.FORWARD; return (T) this; } - @Override @SuppressWarnings("unchecked") + @Override public T zero() { this.setAngle(0); return (T) this; } + @Override + public T resetOffset() { + return setOffset(0); + } + + @SuppressWarnings("unchecked") + @Override + public T setOffset(double offset) { + this.offset = offset; + return (T) this; + } + @Override public RotationDirection getDirection() { return direction; @@ -55,11 +72,6 @@ public AngleUnit getAngleUnit() { return angleUnit; } - @Override - public void resetOffset() { - this.offset = 0; - } - @Override public E getEncoder() { return encoder; From bc1878ce6888bdda5ea56620e12eb996d58705f7 Mon Sep 17 00:00:00 2001 From: "daniel.yang" Date: Mon, 3 Nov 2025 16:47:34 -0800 Subject: [PATCH 13/23] refactor: getCurrentPosition rename to getAngle --- .../solverslib/command/Command.java | 36 ++++++++++++++++--- .../swerve/coaxial/CoaxialSwerveModule.java | 4 +-- .../solverslib/hardware/Encoder.java | 26 ++++++++------ .../solverslib/hardware/EncoderBase.java | 12 ------- 4 files changed, 49 insertions(+), 29 deletions(-) diff --git a/core/src/main/java/com/seattlesolvers/solverslib/command/Command.java b/core/src/main/java/com/seattlesolvers/solverslib/command/Command.java index 488390d9..b3db04e6 100644 --- a/core/src/main/java/com/seattlesolvers/solverslib/command/Command.java +++ b/core/src/main/java/com/seattlesolvers/solverslib/command/Command.java @@ -316,28 +316,56 @@ default boolean runsWhenDisabled() { return false; } - default String getName() { - return this.getClass().getSimpleName(); - } - + /** + * Sets this command as uninterruptible. + * Wraps the command in {@link UninterruptibleCommand} internally. + * @return the decorated command + */ default Command setUninterruptible() { return new UninterruptibleCommand(this); } + /** + * Adds a callback with a boolean supplier + * @param condition Runs the runnable the first time this is true + * @param runnable Callback to run + * @return the decorated command + */ default Command when(BooleanSupplier condition, Runnable runnable) { return new CallbackCommand(this).when(condition, runnable); } + /** + * Adds a callback with a boolean supplier + * @param condition Schedules the command the first time this is true + * @param command Command to schedule + * @return the decorated command + */ default Command when(BooleanSupplier condition, Command command) { return new CallbackCommand(this).when(condition, command); } + /** + * Adds a callback with access to the inner command + * @param condition Runs the runnable the first time this is true + * @param runnable Callback to run + * @return the decorated command + */ default Command whenSelf(Function condition, Runnable runnable) { return new CallbackCommand(this).whenSelf(condition, runnable); } + /** + * Adds a callback with access to the inner command + * @param condition Schedules the command the first time this is true + * @param command Command to schedule + * @return the decorated command + */ default Command whenSelf(Function condition, Command command) { return new CallbackCommand(this).whenSelf(condition, command); } + default String getName() { + return this.getClass().getSimpleName(); + } } \ No newline at end of file diff --git a/core/src/main/java/com/seattlesolvers/solverslib/drivebase/swerve/coaxial/CoaxialSwerveModule.java b/core/src/main/java/com/seattlesolvers/solverslib/drivebase/swerve/coaxial/CoaxialSwerveModule.java index 53a4fcf7..aeaf75c8 100644 --- a/core/src/main/java/com/seattlesolvers/solverslib/drivebase/swerve/coaxial/CoaxialSwerveModule.java +++ b/core/src/main/java/com/seattlesolvers/solverslib/drivebase/swerve/coaxial/CoaxialSwerveModule.java @@ -69,7 +69,7 @@ public void setTargetVelocity(Vector2d velocity) { public void updateModule() { // Wheel flipping optimization (if its quicker to swap motor direction and rotate the pod less, then do that) wheelFlipped = false; - angleError = MathUtils.normalizeRadians(MathUtils.normalizeRadians(targetVelocity.angle(), true) - swervo.getAbsoluteEncoder().getCurrentPosition(), false); + angleError = MathUtils.normalizeRadians(MathUtils.normalizeRadians(targetVelocity.angle(), true) - swervo.getAbsoluteEncoder().getAngle(), false); if (Math.abs(angleError) > Math.PI/2) { angleError += Math.PI * -Math.signum(angleError); wheelFlipped = true; @@ -116,7 +116,7 @@ public CoaxialSwerveModule setCachingTolerance(double motorCachingTolerance, dou public String getPowerTelemetry() { return "Motor: " + MathUtils.round(motor.get(), 3) + "; Servo: " + MathUtils.round(swervo.get(), 3) + - "; Absolute Encoder: " + MathUtils.round(swervo.getAbsoluteEncoder().getCurrentPosition(), 3); + "; Absolute Encoder: " + MathUtils.round(swervo.getAbsoluteEncoder().getAngle(), 3); } public Vector2d getTargetVelocity() { diff --git a/core/src/main/java/com/seattlesolvers/solverslib/hardware/Encoder.java b/core/src/main/java/com/seattlesolvers/solverslib/hardware/Encoder.java index 3e7bf1d3..8fbaf35b 100644 --- a/core/src/main/java/com/seattlesolvers/solverslib/hardware/Encoder.java +++ b/core/src/main/java/com/seattlesolvers/solverslib/hardware/Encoder.java @@ -49,23 +49,12 @@ public interface Encoder> extends HardwareDevice { */ boolean getReversed(); - /** - * Resets the encoder to 0 - * @return The object itself for chaining purposes - */ - T zero(); - /** * Resets the encoder to a particular angle. * @return The object itself for chaining purposes */ T setAngle(double angle); - /** - * Manually zeroes the offset for any future values returned - */ - T resetOffset(); - /** * Manually sets the offset for any future values returned */ @@ -76,6 +65,21 @@ public interface Encoder> extends HardwareDevice { */ E getEncoder(); + /** + * Manually zeroes the offset for any future values returned + */ + default T resetOffset() { + return setOffset(0); + } + + /** + * Resets the encoder to 0 + * @return The object itself for chaining purposes + */ + default T zero() { + return setAngle(0); + } + @Override default void disable() { // "take no action" (encoder.close() call in SDK) diff --git a/core/src/main/java/com/seattlesolvers/solverslib/hardware/EncoderBase.java b/core/src/main/java/com/seattlesolvers/solverslib/hardware/EncoderBase.java index cf88580e..ce8ef10e 100644 --- a/core/src/main/java/com/seattlesolvers/solverslib/hardware/EncoderBase.java +++ b/core/src/main/java/com/seattlesolvers/solverslib/hardware/EncoderBase.java @@ -38,18 +38,6 @@ public T setReversed(boolean reversed) { return (T) this; } - @SuppressWarnings("unchecked") - @Override - public T zero() { - this.setAngle(0); - return (T) this; - } - - @Override - public T resetOffset() { - return setOffset(0); - } - @SuppressWarnings("unchecked") @Override public T setOffset(double offset) { From a2188059edb4dfe9650030c31f0defa7dc2a4fbd Mon Sep 17 00:00:00 2001 From: "daniel.yang" Date: Mon, 3 Nov 2025 17:12:32 -0800 Subject: [PATCH 14/23] docs: add docs and contributions refactor: renamed zero to reset method in Encoder --- .../solverslib/command/CallbackCommand.java | 2 +- .../solverslib/command/MecanumControllerCommand.java | 4 ++-- .../solverslib/command/ProfiledPIDCommand.java | 4 ++-- .../solverslib/command/RepeatCommand.java | 2 +- .../solverslib/command/old/Subsystem.java | 2 +- .../controller/wpilibcontroller/ArmFeedforward.java | 4 ++-- .../wpilibcontroller/ElevatorFeedforward.java | 4 ++-- .../wpilibcontroller/ProfiledPIDController.java | 2 +- .../wpilibcontroller/SimpleMotorFeedforward.java | 4 ++-- .../solverslib/geometry/Translation2d.java | 2 +- .../solverslib/hardware/AbsoluteAnalogEncoder.java | 1 + .../seattlesolvers/solverslib/hardware/Encoder.java | 10 ++++++---- .../solverslib/hardware/EncoderBase.java | 5 ++--- .../solverslib/hardware/IncrementalEncoder.java | 8 ++++++-- .../solverslib/hardware/motors/Motor.java | 10 +++++----- .../kinematics/wpilibkinematics/ChassisSpeeds.java | 2 +- .../wpilibkinematics/DifferentialDriveOdometry.java | 8 ++++---- .../wpilibkinematics/MecanumDriveOdometry.java | 2 +- .../wpilibkinematics/SwerveDriveOdometry.java | 2 +- .../seattlesolvers/solverslib/purepursuit/Path.java | 2 +- .../purepursuit/waypoints/GeneralWaypoint.java | 2 +- .../seattlesolvers/solverslib/spline/SplineHelper.java | 6 +++--- .../solverslib/trajectory/TrapezoidProfile.java | 2 +- .../com/seattlesolvers/solverslib/util/Timing.java | 2 +- 24 files changed, 49 insertions(+), 43 deletions(-) diff --git a/core/src/main/java/com/seattlesolvers/solverslib/command/CallbackCommand.java b/core/src/main/java/com/seattlesolvers/solverslib/command/CallbackCommand.java index 03461c4b..9aff64ba 100644 --- a/core/src/main/java/com/seattlesolvers/solverslib/command/CallbackCommand.java +++ b/core/src/main/java/com/seattlesolvers/solverslib/command/CallbackCommand.java @@ -7,7 +7,7 @@ import java.util.function.Function; /** - * Schedules a Command as uninterruptible + * Wrapper to easily add callbacks to a command * @author Daniel - FTC 7854 */ public class CallbackCommand extends CommandBase { diff --git a/core/src/main/java/com/seattlesolvers/solverslib/command/MecanumControllerCommand.java b/core/src/main/java/com/seattlesolvers/solverslib/command/MecanumControllerCommand.java index 479175f3..34f3b512 100644 --- a/core/src/main/java/com/seattlesolvers/solverslib/command/MecanumControllerCommand.java +++ b/core/src/main/java/com/seattlesolvers/solverslib/command/MecanumControllerCommand.java @@ -63,7 +63,7 @@ public class MecanumControllerCommand extends CommandBase { * trajectory. PID control and feedforward are handled internally. Outputs are scaled from -12 to * 12 as a voltage output to the motor. * - *

Note: The controllers will *not* set the outputVolts to zero upon completion of the path + *

Note: The controllers will *not* set the outputVolts to reset upon completion of the path * this is left to the user, since it is not appropriate for paths with nonstationary endstates. * *

Note 2: The rotation controller will calculate the rotation based on the final pose in the @@ -145,7 +145,7 @@ public MecanumControllerCommand(Trajectory trajectory, * Constructs a new MecanumControllerCommand that when executed will follow the provided * trajectory. The user should implement a velocity PID on the desired output wheel velocities. * - *

Note: The controllers will *not* set the outputVolts to zero upon completion of the path - + *

Note: The controllers will *not* set the outputVolts to reset upon completion of the path - * this is left to the user, since it is not appropriate for paths with non-stationary end-states. * *

Note2: The rotation controller will calculate the rotation based on the final pose diff --git a/core/src/main/java/com/seattlesolvers/solverslib/command/ProfiledPIDCommand.java b/core/src/main/java/com/seattlesolvers/solverslib/command/ProfiledPIDCommand.java index 910bba15..6ca6f297 100644 --- a/core/src/main/java/com/seattlesolvers/solverslib/command/ProfiledPIDCommand.java +++ b/core/src/main/java/com/seattlesolvers/solverslib/command/ProfiledPIDCommand.java @@ -54,7 +54,7 @@ public ProfiledPIDCommand(@NonNull ProfiledPIDController controller, @NonNull Do /** * Creates a new PIDCommand, which controls the given output with a ProfiledPIDController. - * Goal velocity is implicitly zero. + * Goal velocity is implicitly reset. * * @param controller the controller that controls the output. * @param measurementSource the measurement of the process variable @@ -91,7 +91,7 @@ public ProfiledPIDCommand(ProfiledPIDController controller, DoubleSupplier measu /** * Creates a new PIDCommand, which controls the given output with a ProfiledPIDController. Goal - * velocity is implicitly zero. + * velocity is implicitly reset. * * @param controller the controller that controls the output. * @param measurementSource the measurement of the process variable diff --git a/core/src/main/java/com/seattlesolvers/solverslib/command/RepeatCommand.java b/core/src/main/java/com/seattlesolvers/solverslib/command/RepeatCommand.java index 0f4e822f..48a0f7db 100644 --- a/core/src/main/java/com/seattlesolvers/solverslib/command/RepeatCommand.java +++ b/core/src/main/java/com/seattlesolvers/solverslib/command/RepeatCommand.java @@ -73,7 +73,7 @@ public RepeatCommand(Command command, int maxRepeatTimes) { timesRepeated = 0; if (maxRepeatTimes <= 0) { - throw new IllegalArgumentException("RepeatCommands' maxRepeatTimes cannot be negative or zero!"); + throw new IllegalArgumentException("RepeatCommands' maxRepeatTimes cannot be negative or reset!"); } requireUngrouped(command); diff --git a/core/src/main/java/com/seattlesolvers/solverslib/command/old/Subsystem.java b/core/src/main/java/com/seattlesolvers/solverslib/command/old/Subsystem.java index 3cbf5314..d7d80ee7 100644 --- a/core/src/main/java/com/seattlesolvers/solverslib/command/old/Subsystem.java +++ b/core/src/main/java/com/seattlesolvers/solverslib/command/old/Subsystem.java @@ -16,7 +16,7 @@ public interface Subsystem { public void initialize(); /** - * The zero method. Returns the subsystem back to its original + * The reset method. Returns the subsystem back to its original * position and resets any saved data. */ public void reset(); diff --git a/core/src/main/java/com/seattlesolvers/solverslib/controller/wpilibcontroller/ArmFeedforward.java b/core/src/main/java/com/seattlesolvers/solverslib/controller/wpilibcontroller/ArmFeedforward.java index ec4c7893..f96f0ef6 100644 --- a/core/src/main/java/com/seattlesolvers/solverslib/controller/wpilibcontroller/ArmFeedforward.java +++ b/core/src/main/java/com/seattlesolvers/solverslib/controller/wpilibcontroller/ArmFeedforward.java @@ -36,7 +36,7 @@ public ArmFeedforward(double ks, double kcos, double kv, double ka) { /** * Creates a new ArmFeedforward with the specified gains. Acceleration gain is - * defaulted to zero. Units of the gain values will dictate units of the computed feedforward. + * defaulted to reset. Units of the gain values will dictate units of the computed feedforward. * * @param ks The static gain. * @param kcos The gravity gain. @@ -62,7 +62,7 @@ public double calculate(double positionRadians, double velocityRadPerSec, /** * Calculates the feedforward from the gains and velocity setpoint (acceleration is assumed to - * be zero). + * be reset). * * @param velocity The velocity setpoint. * @return The computed feedforward. diff --git a/core/src/main/java/com/seattlesolvers/solverslib/controller/wpilibcontroller/ElevatorFeedforward.java b/core/src/main/java/com/seattlesolvers/solverslib/controller/wpilibcontroller/ElevatorFeedforward.java index 61fb3fcf..11df9aa5 100644 --- a/core/src/main/java/com/seattlesolvers/solverslib/controller/wpilibcontroller/ElevatorFeedforward.java +++ b/core/src/main/java/com/seattlesolvers/solverslib/controller/wpilibcontroller/ElevatorFeedforward.java @@ -36,7 +36,7 @@ public ElevatorFeedforward(double ks, double kg, double kv, double ka) { /** * Creates a new ElevatorFeedforward with the specified gains. Acceleration gain is - * defaulted to zero. Units of the gain values will dictate units of the computed feedforward. + * defaulted to reset. Units of the gain values will dictate units of the computed feedforward. * * @param ks The static gain. * @param kg The gravity gain. @@ -59,7 +59,7 @@ public double calculate(double velocity, double acceleration) { /** * Calculates the feedforward from the gains and velocity setpoint (acceleration is assumed to - * be zero). + * be reset). * * @param velocity The velocity setpoint. * @return The computed feedforward. diff --git a/core/src/main/java/com/seattlesolvers/solverslib/controller/wpilibcontroller/ProfiledPIDController.java b/core/src/main/java/com/seattlesolvers/solverslib/controller/wpilibcontroller/ProfiledPIDController.java index 4ef06287..fe4c9704 100644 --- a/core/src/main/java/com/seattlesolvers/solverslib/controller/wpilibcontroller/ProfiledPIDController.java +++ b/core/src/main/java/com/seattlesolvers/solverslib/controller/wpilibcontroller/ProfiledPIDController.java @@ -290,7 +290,7 @@ public void reset(double measuredPosition, double measuredVelocity) { * Reset the previous error and the integral term. * * @param measuredPosition The current measured position of the system. The velocity is - * assumed to be zero. + * assumed to be reset. */ public void reset(double measuredPosition) { reset(measuredPosition, 0.0); diff --git a/core/src/main/java/com/seattlesolvers/solverslib/controller/wpilibcontroller/SimpleMotorFeedforward.java b/core/src/main/java/com/seattlesolvers/solverslib/controller/wpilibcontroller/SimpleMotorFeedforward.java index 9c9eda1f..d34ed097 100644 --- a/core/src/main/java/com/seattlesolvers/solverslib/controller/wpilibcontroller/SimpleMotorFeedforward.java +++ b/core/src/main/java/com/seattlesolvers/solverslib/controller/wpilibcontroller/SimpleMotorFeedforward.java @@ -32,7 +32,7 @@ public SimpleMotorFeedforward(double ks, double kv, double ka) { /** * Creates a new SimpleMotorFeedforward with the specified gains. Acceleration gain is - * defaulted to zero. Units of the gain values will dictate units of the computed feedforward. + * defaulted to reset. Units of the gain values will dictate units of the computed feedforward. * * @param ks The static gain. * @param kv The velocity gain. @@ -57,7 +57,7 @@ public double calculate(double velocity, double acceleration) { /** * Calculates the feedforward from the gains and velocity setpoint (acceleration is assumed to - * be zero). + * be reset). * * @param velocity The velocity setpoint. * @return The computed feedforward. diff --git a/core/src/main/java/com/seattlesolvers/solverslib/geometry/Translation2d.java b/core/src/main/java/com/seattlesolvers/solverslib/geometry/Translation2d.java index 88f41e0c..40437293 100644 --- a/core/src/main/java/com/seattlesolvers/solverslib/geometry/Translation2d.java +++ b/core/src/main/java/com/seattlesolvers/solverslib/geometry/Translation2d.java @@ -5,7 +5,7 @@ public class Translation2d { private final double m_y; /** - * Constructs a Translation2d with X and Y components equal to zero. + * Constructs a Translation2d with X and Y components equal to reset. */ public Translation2d() { this(0.0, 0.0); diff --git a/core/src/main/java/com/seattlesolvers/solverslib/hardware/AbsoluteAnalogEncoder.java b/core/src/main/java/com/seattlesolvers/solverslib/hardware/AbsoluteAnalogEncoder.java index 1d6066ac..7d377a5a 100644 --- a/core/src/main/java/com/seattlesolvers/solverslib/hardware/AbsoluteAnalogEncoder.java +++ b/core/src/main/java/com/seattlesolvers/solverslib/hardware/AbsoluteAnalogEncoder.java @@ -10,6 +10,7 @@ * An extended wrapper class for AnalogInput absolute encoders. * * @author Saket + * @author Daniel - 7854 */ public class AbsoluteAnalogEncoder extends EncoderBase { public static double DEFAULT_RANGE = 3.3; diff --git a/core/src/main/java/com/seattlesolvers/solverslib/hardware/Encoder.java b/core/src/main/java/com/seattlesolvers/solverslib/hardware/Encoder.java index 8fbaf35b..ddd567f3 100644 --- a/core/src/main/java/com/seattlesolvers/solverslib/hardware/Encoder.java +++ b/core/src/main/java/com/seattlesolvers/solverslib/hardware/Encoder.java @@ -5,9 +5,11 @@ import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; /** + * The Encoder interface that defines common angle methods + * @param The hardware class (e.g. DcMotor, AnalogInput) + * @param The encoder class itself (e.g. IncrementalEncoder, AbsoluteAnalogEncoder) * - * @param - * @param + * @author Daniel */ public interface Encoder> extends HardwareDevice { /** @@ -68,7 +70,7 @@ public interface Encoder> extends HardwareDevice { /** * Manually zeroes the offset for any future values returned */ - default T resetOffset() { + default T zeroOffset() { return setOffset(0); } @@ -76,7 +78,7 @@ default T resetOffset() { * Resets the encoder to 0 * @return The object itself for chaining purposes */ - default T zero() { + default T reset() { return setAngle(0); } diff --git a/core/src/main/java/com/seattlesolvers/solverslib/hardware/EncoderBase.java b/core/src/main/java/com/seattlesolvers/solverslib/hardware/EncoderBase.java index ce8ef10e..e663cb11 100644 --- a/core/src/main/java/com/seattlesolvers/solverslib/hardware/EncoderBase.java +++ b/core/src/main/java/com/seattlesolvers/solverslib/hardware/EncoderBase.java @@ -5,9 +5,8 @@ import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; /** - * Provides basic implementations for the {@link Encoder} interface. - * @param The encoder type (e.g. DcMotor, AnalogInput) - * @param The encoder class itself (e.g. IncrementalEncoder, AbsoluteAnalogEncoder) + * A base class for {@link Encoder}. + * @author Daniel - 7854 */ public abstract class EncoderBase> implements Encoder { protected double offset = 0.0; diff --git a/core/src/main/java/com/seattlesolvers/solverslib/hardware/IncrementalEncoder.java b/core/src/main/java/com/seattlesolvers/solverslib/hardware/IncrementalEncoder.java index a0d602c2..463724c0 100644 --- a/core/src/main/java/com/seattlesolvers/solverslib/hardware/IncrementalEncoder.java +++ b/core/src/main/java/com/seattlesolvers/solverslib/hardware/IncrementalEncoder.java @@ -7,9 +7,13 @@ import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +/** + * An extended wrapper class for DcMotor incremental/quadrature encoders. + * + * @author Daniel - 7854 + */ public class IncrementalEncoder extends EncoderBase { private final double cpr; - private int lastPosition; private double lastTimeStamp, veloEstimate, dpp, accel, lastVelo; @@ -77,7 +81,7 @@ public int getPosition() { /** * @return the distance per pulse */ - public double getDpp() { + public double setDistancePerPulse() { return dpp; } diff --git a/core/src/main/java/com/seattlesolvers/solverslib/hardware/motors/Motor.java b/core/src/main/java/com/seattlesolvers/solverslib/hardware/motors/Motor.java index 5d5594d5..d379b509 100644 --- a/core/src/main/java/com/seattlesolvers/solverslib/hardware/motors/Motor.java +++ b/core/src/main/java/com/seattlesolvers/solverslib/hardware/motors/Motor.java @@ -25,7 +25,7 @@ public enum GoBILDA { RPM_117(1425.2, 117), RPM_223(753.2, 223), RPM_312(537.6, 312), RPM_435(383.6, 435), RPM_1150(145.6, 1150), RPM_1620(103.6, 1620), BARE(28, 6000), NONE(0, 0); - final private double cpr, rpm; + private final double cpr, rpm; GoBILDA(double cpr, double rpm) { this.cpr = cpr; @@ -199,14 +199,14 @@ public boolean atTargetPosition() { * Resets the external encoder wrapper value. */ public void resetEncoder() { - encoder.zero(); + encoder.reset(); } /** * Resets the internal position of the motor. */ public void stopAndResetEncoder() { - encoder.resetOffset(); + encoder.zeroOffset(); motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); motor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); } @@ -233,7 +233,7 @@ public double[] getFeedforwardCoefficients() { } /** - * A wrapper method for the zero power behavior + * A wrapper method for the reset power behavior * * @param behavior the behavior desired */ @@ -317,7 +317,7 @@ public double get() { * @param target the target position in ticks */ public void setTargetPosition(int target) { - setTargetDistance(target * encoder.getDpp()); + setTargetDistance(target * encoder.setDistancePerPulse()); } /** diff --git a/core/src/main/java/com/seattlesolvers/solverslib/kinematics/wpilibkinematics/ChassisSpeeds.java b/core/src/main/java/com/seattlesolvers/solverslib/kinematics/wpilibkinematics/ChassisSpeeds.java index 0627c117..39f5976f 100644 --- a/core/src/main/java/com/seattlesolvers/solverslib/kinematics/wpilibkinematics/ChassisSpeeds.java +++ b/core/src/main/java/com/seattlesolvers/solverslib/kinematics/wpilibkinematics/ChassisSpeeds.java @@ -69,7 +69,7 @@ public ChassisSpeeds(double vxMetersPerSecond, double vyMetersPerSecond, * Positive y is to your left when standing behind the alliance wall. * @param omegaRadiansPerSecond The angular rate of the robot. * @param robotAngle The angle of the robot as measured by a gyroscope. The robot's - * angle is considered to be zero when it is facing directly away + * angle is considered to be reset when it is facing directly away * from your alliance station wall. Remember that this should * be CCW positive. * @return ChassisSpeeds object representing the speeds in the robot's frame of reference. diff --git a/core/src/main/java/com/seattlesolvers/solverslib/kinematics/wpilibkinematics/DifferentialDriveOdometry.java b/core/src/main/java/com/seattlesolvers/solverslib/kinematics/wpilibkinematics/DifferentialDriveOdometry.java index b3eb6a79..7d0a1829 100644 --- a/core/src/main/java/com/seattlesolvers/solverslib/kinematics/wpilibkinematics/DifferentialDriveOdometry.java +++ b/core/src/main/java/com/seattlesolvers/solverslib/kinematics/wpilibkinematics/DifferentialDriveOdometry.java @@ -21,8 +21,8 @@ * path following. Furthermore, odometry can be used for latency compensation * when using computer-vision systems. * - *

It is important that you zero your encoders to zero before using this class. - * Any subsequent pose resets also require the encoders to be zero to zero. + *

It is important that you reset your encoders to reset before using this class. + * Any subsequent pose resets also require the encoders to be reset to reset. */ public class DifferentialDriveOdometry { private Pose2d m_poseMeters; @@ -58,9 +58,9 @@ public DifferentialDriveOdometry(Rotation2d gyroAngle) { /** * Resets the robot's position on the field. * - *

You NEED to zero your encoders (to zero) when calling this method. + *

You NEED to reset your encoders (to reset) when calling this method. * - *

The gyroscope angle does not need to be zero here on the user's robot code. + *

The gyroscope angle does not need to be reset here on the user's robot code. * The library automatically takes care of offsetting the gyro angle. * * @param poseMeters The position on the field that your robot is at. diff --git a/core/src/main/java/com/seattlesolvers/solverslib/kinematics/wpilibkinematics/MecanumDriveOdometry.java b/core/src/main/java/com/seattlesolvers/solverslib/kinematics/wpilibkinematics/MecanumDriveOdometry.java index ae5d1650..bcfa4de4 100644 --- a/core/src/main/java/com/seattlesolvers/solverslib/kinematics/wpilibkinematics/MecanumDriveOdometry.java +++ b/core/src/main/java/com/seattlesolvers/solverslib/kinematics/wpilibkinematics/MecanumDriveOdometry.java @@ -57,7 +57,7 @@ public MecanumDriveOdometry(MecanumDriveKinematics kinematics, Rotation2d gyroAn /** * Resets the robot's position on the field. * - *

The gyroscope angle does not need to be zero here on the user's robot code. + *

The gyroscope angle does not need to be reset here on the user's robot code. * The library automatically takes care of offsetting the gyro angle. * * @param poseMeters The position on the field that your robot is at. diff --git a/core/src/main/java/com/seattlesolvers/solverslib/kinematics/wpilibkinematics/SwerveDriveOdometry.java b/core/src/main/java/com/seattlesolvers/solverslib/kinematics/wpilibkinematics/SwerveDriveOdometry.java index d0e7173a..3f855c0a 100644 --- a/core/src/main/java/com/seattlesolvers/solverslib/kinematics/wpilibkinematics/SwerveDriveOdometry.java +++ b/core/src/main/java/com/seattlesolvers/solverslib/kinematics/wpilibkinematics/SwerveDriveOdometry.java @@ -58,7 +58,7 @@ public SwerveDriveOdometry(SwerveDriveKinematics kinematics, Rotation2d gyroAngl /** * Resets the robot's position on the field. * - *

The gyroscope angle does not need to be zero here on the user's robot code. + *

The gyroscope angle does not need to be reset here on the user's robot code. * The library automatically takes care of offsetting the gyro angle. * * @param pose The position on the field that your robot is at. diff --git a/core/src/main/java/com/seattlesolvers/solverslib/purepursuit/Path.java b/core/src/main/java/com/seattlesolvers/solverslib/purepursuit/Path.java index 41e7c95f..c22618f2 100644 --- a/core/src/main/java/com/seattlesolvers/solverslib/purepursuit/Path.java +++ b/core/src/main/java/com/seattlesolvers/solverslib/purepursuit/Path.java @@ -174,7 +174,7 @@ public boolean followPath(MecanumDrive mecanumDrive, Odometry odometry) { * This is the principle path method. After everything is configured and initiated, this method can be used. * Using the robot's horizontal, y, and rotation, this method calculates the appropriate motor powers for the * robot to follow the path. This method calls all triggered/interrupted actions automatically. If this - * returns zero motor speeds {0, 0, 0} that means the path has either (1) timed out, (2) lost the path and + * returns reset motor speeds {0, 0, 0} that means the path has either (1) timed out, (2) lost the path and * retrace was disabled, or (3) reached the destination. Use isFinished() and timedOut() to troubleshoot. * * @param vPosition Robot's current vertical position. diff --git a/core/src/main/java/com/seattlesolvers/solverslib/purepursuit/waypoints/GeneralWaypoint.java b/core/src/main/java/com/seattlesolvers/solverslib/purepursuit/waypoints/GeneralWaypoint.java index 56f5efc0..8fa9814e 100644 --- a/core/src/main/java/com/seattlesolvers/solverslib/purepursuit/waypoints/GeneralWaypoint.java +++ b/core/src/main/java/com/seattlesolvers/solverslib/purepursuit/waypoints/GeneralWaypoint.java @@ -253,7 +253,7 @@ public GeneralWaypoint disablePreferredAngle() { * Resets this waypoint. This is called by Path. */ public void reset() { - // GeneralWaypoints don't have anything to zero. + // GeneralWaypoints don't have anything to reset. } /** diff --git a/core/src/main/java/com/seattlesolvers/solverslib/spline/SplineHelper.java b/core/src/main/java/com/seattlesolvers/solverslib/spline/SplineHelper.java index dc795d7f..bb35aaf5 100644 --- a/core/src/main/java/com/seattlesolvers/solverslib/spline/SplineHelper.java +++ b/core/src/main/java/com/seattlesolvers/solverslib/spline/SplineHelper.java @@ -114,14 +114,14 @@ public static CubicHermiteSpline[] getCubicSplinesFromControlVectors( https://www.uio.no/studier/emner/matnat/ifi/nedlagte-emner/INF-MAT4350/h08 /undervisningsmateriale/chap7alecture.pdf */ - // Above-diagonal of tridiagonal matrix, zero-padded + // Above-diagonal of tridiagonal matrix, reset-padded final double[] a = new double[newWaypts.length - 2]; // Diagonal of tridiagonal matrix final double[] b = new double[newWaypts.length - 2]; Arrays.fill(b, 4.0); - // Below-diagonal of tridiagonal matrix, zero-padded + // Below-diagonal of tridiagonal matrix, reset-padded final double[] c = new double[newWaypts.length - 2]; // rhs vectors @@ -244,7 +244,7 @@ private static void thomasAlgorithm(double[] a, double[] b, double[] dStar = new double[N]; // This updates the coefficients in the first row - // Note that we should be checking for division by zero here + // Note that we should be checking for division by reset here cStar[0] = c[0] / b[0]; dStar[0] = d[0] / b[0]; diff --git a/core/src/main/java/com/seattlesolvers/solverslib/trajectory/TrapezoidProfile.java b/core/src/main/java/com/seattlesolvers/solverslib/trajectory/TrapezoidProfile.java index 24508a3b..38baef4d 100644 --- a/core/src/main/java/com/seattlesolvers/solverslib/trajectory/TrapezoidProfile.java +++ b/core/src/main/java/com/seattlesolvers/solverslib/trajectory/TrapezoidProfile.java @@ -123,7 +123,7 @@ public TrapezoidProfile(Constraints constraints, State goal, State initial) { // Deal with a possibly truncated motion profile (with nonzero initial or // final velocity) by calculating the parameters as if the profile began and - // ended at zero velocity + // ended at reset velocity double cutoffBegin = m_initial.velocity / m_constraints.maxAcceleration; double cutoffDistBegin = cutoffBegin * cutoffBegin * m_constraints.maxAcceleration / 2.0; diff --git a/core/src/main/java/com/seattlesolvers/solverslib/util/Timing.java b/core/src/main/java/com/seattlesolvers/solverslib/util/Timing.java index 0bf95f6f..55b4b3f6 100644 --- a/core/src/main/java/com/seattlesolvers/solverslib/util/Timing.java +++ b/core/src/main/java/com/seattlesolvers/solverslib/util/Timing.java @@ -119,7 +119,7 @@ public boolean isTimerOn() { /** * Very simple class for a refresh rate timer. Can be used to limit hardware writing/reading, - * or other fast-time cases. Only uses milliseconds. Starts counting on creation, can be zero. + * or other fast-time cases. Only uses milliseconds. Starts counting on creation, can be reset. */ public class Rate { From ee290234f2c26eb3eb5681a717b0cfce979fb9f8 Mon Sep 17 00:00:00 2001 From: "daniel.yang" Date: Mon, 3 Nov 2025 17:12:32 -0800 Subject: [PATCH 15/23] docs: add docs and contributions refactor: renamed zero to reset method in Encoder --- .../solverslib/command/CallbackCommand.java | 2 +- .../solverslib/command/old/Subsystem.java | 2 +- .../solverslib/hardware/AbsoluteAnalogEncoder.java | 1 + .../seattlesolvers/solverslib/hardware/Encoder.java | 10 ++++++---- .../solverslib/hardware/EncoderBase.java | 5 ++--- .../solverslib/hardware/IncrementalEncoder.java | 8 ++++++-- .../solverslib/hardware/motors/Motor.java | 8 ++++---- .../wpilibkinematics/DifferentialDriveOdometry.java | 8 ++++---- .../wpilibkinematics/MecanumDriveOdometry.java | 2 +- .../wpilibkinematics/SwerveDriveOdometry.java | 2 +- .../purepursuit/waypoints/GeneralWaypoint.java | 2 +- .../com/seattlesolvers/solverslib/util/Timing.java | 2 +- .../ftc/teamcode/CommandSample/DriveSubsystem.java | 4 ++-- 13 files changed, 31 insertions(+), 25 deletions(-) diff --git a/core/src/main/java/com/seattlesolvers/solverslib/command/CallbackCommand.java b/core/src/main/java/com/seattlesolvers/solverslib/command/CallbackCommand.java index 03461c4b..9aff64ba 100644 --- a/core/src/main/java/com/seattlesolvers/solverslib/command/CallbackCommand.java +++ b/core/src/main/java/com/seattlesolvers/solverslib/command/CallbackCommand.java @@ -7,7 +7,7 @@ import java.util.function.Function; /** - * Schedules a Command as uninterruptible + * Wrapper to easily add callbacks to a command * @author Daniel - FTC 7854 */ public class CallbackCommand extends CommandBase { diff --git a/core/src/main/java/com/seattlesolvers/solverslib/command/old/Subsystem.java b/core/src/main/java/com/seattlesolvers/solverslib/command/old/Subsystem.java index 3cbf5314..d7d80ee7 100644 --- a/core/src/main/java/com/seattlesolvers/solverslib/command/old/Subsystem.java +++ b/core/src/main/java/com/seattlesolvers/solverslib/command/old/Subsystem.java @@ -16,7 +16,7 @@ public interface Subsystem { public void initialize(); /** - * The zero method. Returns the subsystem back to its original + * The reset method. Returns the subsystem back to its original * position and resets any saved data. */ public void reset(); diff --git a/core/src/main/java/com/seattlesolvers/solverslib/hardware/AbsoluteAnalogEncoder.java b/core/src/main/java/com/seattlesolvers/solverslib/hardware/AbsoluteAnalogEncoder.java index 1d6066ac..7d377a5a 100644 --- a/core/src/main/java/com/seattlesolvers/solverslib/hardware/AbsoluteAnalogEncoder.java +++ b/core/src/main/java/com/seattlesolvers/solverslib/hardware/AbsoluteAnalogEncoder.java @@ -10,6 +10,7 @@ * An extended wrapper class for AnalogInput absolute encoders. * * @author Saket + * @author Daniel - 7854 */ public class AbsoluteAnalogEncoder extends EncoderBase { public static double DEFAULT_RANGE = 3.3; diff --git a/core/src/main/java/com/seattlesolvers/solverslib/hardware/Encoder.java b/core/src/main/java/com/seattlesolvers/solverslib/hardware/Encoder.java index 8fbaf35b..ddd567f3 100644 --- a/core/src/main/java/com/seattlesolvers/solverslib/hardware/Encoder.java +++ b/core/src/main/java/com/seattlesolvers/solverslib/hardware/Encoder.java @@ -5,9 +5,11 @@ import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; /** + * The Encoder interface that defines common angle methods + * @param The hardware class (e.g. DcMotor, AnalogInput) + * @param The encoder class itself (e.g. IncrementalEncoder, AbsoluteAnalogEncoder) * - * @param - * @param + * @author Daniel */ public interface Encoder> extends HardwareDevice { /** @@ -68,7 +70,7 @@ public interface Encoder> extends HardwareDevice { /** * Manually zeroes the offset for any future values returned */ - default T resetOffset() { + default T zeroOffset() { return setOffset(0); } @@ -76,7 +78,7 @@ default T resetOffset() { * Resets the encoder to 0 * @return The object itself for chaining purposes */ - default T zero() { + default T reset() { return setAngle(0); } diff --git a/core/src/main/java/com/seattlesolvers/solverslib/hardware/EncoderBase.java b/core/src/main/java/com/seattlesolvers/solverslib/hardware/EncoderBase.java index ce8ef10e..e663cb11 100644 --- a/core/src/main/java/com/seattlesolvers/solverslib/hardware/EncoderBase.java +++ b/core/src/main/java/com/seattlesolvers/solverslib/hardware/EncoderBase.java @@ -5,9 +5,8 @@ import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; /** - * Provides basic implementations for the {@link Encoder} interface. - * @param The encoder type (e.g. DcMotor, AnalogInput) - * @param The encoder class itself (e.g. IncrementalEncoder, AbsoluteAnalogEncoder) + * A base class for {@link Encoder}. + * @author Daniel - 7854 */ public abstract class EncoderBase> implements Encoder { protected double offset = 0.0; diff --git a/core/src/main/java/com/seattlesolvers/solverslib/hardware/IncrementalEncoder.java b/core/src/main/java/com/seattlesolvers/solverslib/hardware/IncrementalEncoder.java index a0d602c2..448ac241 100644 --- a/core/src/main/java/com/seattlesolvers/solverslib/hardware/IncrementalEncoder.java +++ b/core/src/main/java/com/seattlesolvers/solverslib/hardware/IncrementalEncoder.java @@ -7,9 +7,13 @@ import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +/** + * An extended wrapper class for DcMotor incremental/quadrature encoders. + * + * @author Daniel - 7854 + */ public class IncrementalEncoder extends EncoderBase { private final double cpr; - private int lastPosition; private double lastTimeStamp, veloEstimate, dpp, accel, lastVelo; @@ -77,7 +81,7 @@ public int getPosition() { /** * @return the distance per pulse */ - public double getDpp() { + public double getDistancePerPulse() { return dpp; } diff --git a/core/src/main/java/com/seattlesolvers/solverslib/hardware/motors/Motor.java b/core/src/main/java/com/seattlesolvers/solverslib/hardware/motors/Motor.java index 5d5594d5..f82a8e3b 100644 --- a/core/src/main/java/com/seattlesolvers/solverslib/hardware/motors/Motor.java +++ b/core/src/main/java/com/seattlesolvers/solverslib/hardware/motors/Motor.java @@ -25,7 +25,7 @@ public enum GoBILDA { RPM_117(1425.2, 117), RPM_223(753.2, 223), RPM_312(537.6, 312), RPM_435(383.6, 435), RPM_1150(145.6, 1150), RPM_1620(103.6, 1620), BARE(28, 6000), NONE(0, 0); - final private double cpr, rpm; + private final double cpr, rpm; GoBILDA(double cpr, double rpm) { this.cpr = cpr; @@ -199,14 +199,14 @@ public boolean atTargetPosition() { * Resets the external encoder wrapper value. */ public void resetEncoder() { - encoder.zero(); + encoder.reset(); } /** * Resets the internal position of the motor. */ public void stopAndResetEncoder() { - encoder.resetOffset(); + encoder.zeroOffset(); motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); motor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); } @@ -317,7 +317,7 @@ public double get() { * @param target the target position in ticks */ public void setTargetPosition(int target) { - setTargetDistance(target * encoder.getDpp()); + setTargetDistance(target * encoder.getDistancePerPulse()); } /** diff --git a/core/src/main/java/com/seattlesolvers/solverslib/kinematics/wpilibkinematics/DifferentialDriveOdometry.java b/core/src/main/java/com/seattlesolvers/solverslib/kinematics/wpilibkinematics/DifferentialDriveOdometry.java index b3eb6a79..6ebb4a08 100644 --- a/core/src/main/java/com/seattlesolvers/solverslib/kinematics/wpilibkinematics/DifferentialDriveOdometry.java +++ b/core/src/main/java/com/seattlesolvers/solverslib/kinematics/wpilibkinematics/DifferentialDriveOdometry.java @@ -21,8 +21,8 @@ * path following. Furthermore, odometry can be used for latency compensation * when using computer-vision systems. * - *

It is important that you zero your encoders to zero before using this class. - * Any subsequent pose resets also require the encoders to be zero to zero. + *

It is important that you reset your encoders to zero before using this class. + * Any subsequent pose resets also require the encoders to be reset to zero. */ public class DifferentialDriveOdometry { private Pose2d m_poseMeters; @@ -58,9 +58,9 @@ public DifferentialDriveOdometry(Rotation2d gyroAngle) { /** * Resets the robot's position on the field. * - *

You NEED to zero your encoders (to zero) when calling this method. + *

You NEED to reset your encoders (to zero) when calling this method. * - *

The gyroscope angle does not need to be zero here on the user's robot code. + *

The gyroscope angle does not need to be reset here on the user's robot code. * The library automatically takes care of offsetting the gyro angle. * * @param poseMeters The position on the field that your robot is at. diff --git a/core/src/main/java/com/seattlesolvers/solverslib/kinematics/wpilibkinematics/MecanumDriveOdometry.java b/core/src/main/java/com/seattlesolvers/solverslib/kinematics/wpilibkinematics/MecanumDriveOdometry.java index ae5d1650..bcfa4de4 100644 --- a/core/src/main/java/com/seattlesolvers/solverslib/kinematics/wpilibkinematics/MecanumDriveOdometry.java +++ b/core/src/main/java/com/seattlesolvers/solverslib/kinematics/wpilibkinematics/MecanumDriveOdometry.java @@ -57,7 +57,7 @@ public MecanumDriveOdometry(MecanumDriveKinematics kinematics, Rotation2d gyroAn /** * Resets the robot's position on the field. * - *

The gyroscope angle does not need to be zero here on the user's robot code. + *

The gyroscope angle does not need to be reset here on the user's robot code. * The library automatically takes care of offsetting the gyro angle. * * @param poseMeters The position on the field that your robot is at. diff --git a/core/src/main/java/com/seattlesolvers/solverslib/kinematics/wpilibkinematics/SwerveDriveOdometry.java b/core/src/main/java/com/seattlesolvers/solverslib/kinematics/wpilibkinematics/SwerveDriveOdometry.java index d0e7173a..3f855c0a 100644 --- a/core/src/main/java/com/seattlesolvers/solverslib/kinematics/wpilibkinematics/SwerveDriveOdometry.java +++ b/core/src/main/java/com/seattlesolvers/solverslib/kinematics/wpilibkinematics/SwerveDriveOdometry.java @@ -58,7 +58,7 @@ public SwerveDriveOdometry(SwerveDriveKinematics kinematics, Rotation2d gyroAngl /** * Resets the robot's position on the field. * - *

The gyroscope angle does not need to be zero here on the user's robot code. + *

The gyroscope angle does not need to be reset here on the user's robot code. * The library automatically takes care of offsetting the gyro angle. * * @param pose The position on the field that your robot is at. diff --git a/core/src/main/java/com/seattlesolvers/solverslib/purepursuit/waypoints/GeneralWaypoint.java b/core/src/main/java/com/seattlesolvers/solverslib/purepursuit/waypoints/GeneralWaypoint.java index 56f5efc0..8fa9814e 100644 --- a/core/src/main/java/com/seattlesolvers/solverslib/purepursuit/waypoints/GeneralWaypoint.java +++ b/core/src/main/java/com/seattlesolvers/solverslib/purepursuit/waypoints/GeneralWaypoint.java @@ -253,7 +253,7 @@ public GeneralWaypoint disablePreferredAngle() { * Resets this waypoint. This is called by Path. */ public void reset() { - // GeneralWaypoints don't have anything to zero. + // GeneralWaypoints don't have anything to reset. } /** diff --git a/core/src/main/java/com/seattlesolvers/solverslib/util/Timing.java b/core/src/main/java/com/seattlesolvers/solverslib/util/Timing.java index 0bf95f6f..55b4b3f6 100644 --- a/core/src/main/java/com/seattlesolvers/solverslib/util/Timing.java +++ b/core/src/main/java/com/seattlesolvers/solverslib/util/Timing.java @@ -119,7 +119,7 @@ public boolean isTimerOn() { /** * Very simple class for a refresh rate timer. Can be used to limit hardware writing/reading, - * or other fast-time cases. Only uses milliseconds. Starts counting on creation, can be zero. + * or other fast-time cases. Only uses milliseconds. Starts counting on creation, can be reset. */ public class Rate { diff --git a/examples/src/main/java/org/firstinspires/ftc/teamcode/CommandSample/DriveSubsystem.java b/examples/src/main/java/org/firstinspires/ftc/teamcode/CommandSample/DriveSubsystem.java index 53139c29..3833a745 100644 --- a/examples/src/main/java/org/firstinspires/ftc/teamcode/CommandSample/DriveSubsystem.java +++ b/examples/src/main/java/org/firstinspires/ftc/teamcode/CommandSample/DriveSubsystem.java @@ -61,8 +61,8 @@ public double getRightEncoderDistance() { } public void resetEncoders() { - m_left.zero(); - m_right.zero(); + m_left.reset(); + m_right.reset(); } public double getAverageEncoderDistance() { From 6b839584c7fb722350da4c4a8b3d362af7a8a903 Mon Sep 17 00:00:00 2001 From: "daniel.yang" Date: Mon, 3 Nov 2025 17:36:27 -0800 Subject: [PATCH 16/23] docs: fix "zero" and "reset" errors --- .../solverslib/command/MecanumControllerCommand.java | 4 ++-- .../solverslib/command/ProfiledPIDCommand.java | 4 ++-- .../seattlesolvers/solverslib/command/RepeatCommand.java | 2 +- .../controller/wpilibcontroller/ArmFeedforward.java | 4 ++-- .../controller/wpilibcontroller/ElevatorFeedforward.java | 4 ++-- .../controller/wpilibcontroller/ProfiledPIDController.java | 2 +- .../controller/wpilibcontroller/SimpleMotorFeedforward.java | 4 ++-- .../seattlesolvers/solverslib/geometry/Translation2d.java | 2 +- .../kinematics/wpilibkinematics/ChassisSpeeds.java | 2 +- .../com/seattlesolvers/solverslib/purepursuit/Path.java | 2 +- .../com/seattlesolvers/solverslib/spline/SplineHelper.java | 6 +++--- .../solverslib/trajectory/TrapezoidProfile.java | 2 +- 12 files changed, 19 insertions(+), 19 deletions(-) diff --git a/core/src/main/java/com/seattlesolvers/solverslib/command/MecanumControllerCommand.java b/core/src/main/java/com/seattlesolvers/solverslib/command/MecanumControllerCommand.java index 34f3b512..479175f3 100644 --- a/core/src/main/java/com/seattlesolvers/solverslib/command/MecanumControllerCommand.java +++ b/core/src/main/java/com/seattlesolvers/solverslib/command/MecanumControllerCommand.java @@ -63,7 +63,7 @@ public class MecanumControllerCommand extends CommandBase { * trajectory. PID control and feedforward are handled internally. Outputs are scaled from -12 to * 12 as a voltage output to the motor. * - *

Note: The controllers will *not* set the outputVolts to reset upon completion of the path + *

Note: The controllers will *not* set the outputVolts to zero upon completion of the path * this is left to the user, since it is not appropriate for paths with nonstationary endstates. * *

Note 2: The rotation controller will calculate the rotation based on the final pose in the @@ -145,7 +145,7 @@ public MecanumControllerCommand(Trajectory trajectory, * Constructs a new MecanumControllerCommand that when executed will follow the provided * trajectory. The user should implement a velocity PID on the desired output wheel velocities. * - *

Note: The controllers will *not* set the outputVolts to reset upon completion of the path - + *

Note: The controllers will *not* set the outputVolts to zero upon completion of the path - * this is left to the user, since it is not appropriate for paths with non-stationary end-states. * *

Note2: The rotation controller will calculate the rotation based on the final pose diff --git a/core/src/main/java/com/seattlesolvers/solverslib/command/ProfiledPIDCommand.java b/core/src/main/java/com/seattlesolvers/solverslib/command/ProfiledPIDCommand.java index 6ca6f297..910bba15 100644 --- a/core/src/main/java/com/seattlesolvers/solverslib/command/ProfiledPIDCommand.java +++ b/core/src/main/java/com/seattlesolvers/solverslib/command/ProfiledPIDCommand.java @@ -54,7 +54,7 @@ public ProfiledPIDCommand(@NonNull ProfiledPIDController controller, @NonNull Do /** * Creates a new PIDCommand, which controls the given output with a ProfiledPIDController. - * Goal velocity is implicitly reset. + * Goal velocity is implicitly zero. * * @param controller the controller that controls the output. * @param measurementSource the measurement of the process variable @@ -91,7 +91,7 @@ public ProfiledPIDCommand(ProfiledPIDController controller, DoubleSupplier measu /** * Creates a new PIDCommand, which controls the given output with a ProfiledPIDController. Goal - * velocity is implicitly reset. + * velocity is implicitly zero. * * @param controller the controller that controls the output. * @param measurementSource the measurement of the process variable diff --git a/core/src/main/java/com/seattlesolvers/solverslib/command/RepeatCommand.java b/core/src/main/java/com/seattlesolvers/solverslib/command/RepeatCommand.java index 48a0f7db..0f4e822f 100644 --- a/core/src/main/java/com/seattlesolvers/solverslib/command/RepeatCommand.java +++ b/core/src/main/java/com/seattlesolvers/solverslib/command/RepeatCommand.java @@ -73,7 +73,7 @@ public RepeatCommand(Command command, int maxRepeatTimes) { timesRepeated = 0; if (maxRepeatTimes <= 0) { - throw new IllegalArgumentException("RepeatCommands' maxRepeatTimes cannot be negative or reset!"); + throw new IllegalArgumentException("RepeatCommands' maxRepeatTimes cannot be negative or zero!"); } requireUngrouped(command); diff --git a/core/src/main/java/com/seattlesolvers/solverslib/controller/wpilibcontroller/ArmFeedforward.java b/core/src/main/java/com/seattlesolvers/solverslib/controller/wpilibcontroller/ArmFeedforward.java index f96f0ef6..ec4c7893 100644 --- a/core/src/main/java/com/seattlesolvers/solverslib/controller/wpilibcontroller/ArmFeedforward.java +++ b/core/src/main/java/com/seattlesolvers/solverslib/controller/wpilibcontroller/ArmFeedforward.java @@ -36,7 +36,7 @@ public ArmFeedforward(double ks, double kcos, double kv, double ka) { /** * Creates a new ArmFeedforward with the specified gains. Acceleration gain is - * defaulted to reset. Units of the gain values will dictate units of the computed feedforward. + * defaulted to zero. Units of the gain values will dictate units of the computed feedforward. * * @param ks The static gain. * @param kcos The gravity gain. @@ -62,7 +62,7 @@ public double calculate(double positionRadians, double velocityRadPerSec, /** * Calculates the feedforward from the gains and velocity setpoint (acceleration is assumed to - * be reset). + * be zero). * * @param velocity The velocity setpoint. * @return The computed feedforward. diff --git a/core/src/main/java/com/seattlesolvers/solverslib/controller/wpilibcontroller/ElevatorFeedforward.java b/core/src/main/java/com/seattlesolvers/solverslib/controller/wpilibcontroller/ElevatorFeedforward.java index 11df9aa5..61fb3fcf 100644 --- a/core/src/main/java/com/seattlesolvers/solverslib/controller/wpilibcontroller/ElevatorFeedforward.java +++ b/core/src/main/java/com/seattlesolvers/solverslib/controller/wpilibcontroller/ElevatorFeedforward.java @@ -36,7 +36,7 @@ public ElevatorFeedforward(double ks, double kg, double kv, double ka) { /** * Creates a new ElevatorFeedforward with the specified gains. Acceleration gain is - * defaulted to reset. Units of the gain values will dictate units of the computed feedforward. + * defaulted to zero. Units of the gain values will dictate units of the computed feedforward. * * @param ks The static gain. * @param kg The gravity gain. @@ -59,7 +59,7 @@ public double calculate(double velocity, double acceleration) { /** * Calculates the feedforward from the gains and velocity setpoint (acceleration is assumed to - * be reset). + * be zero). * * @param velocity The velocity setpoint. * @return The computed feedforward. diff --git a/core/src/main/java/com/seattlesolvers/solverslib/controller/wpilibcontroller/ProfiledPIDController.java b/core/src/main/java/com/seattlesolvers/solverslib/controller/wpilibcontroller/ProfiledPIDController.java index fe4c9704..4ef06287 100644 --- a/core/src/main/java/com/seattlesolvers/solverslib/controller/wpilibcontroller/ProfiledPIDController.java +++ b/core/src/main/java/com/seattlesolvers/solverslib/controller/wpilibcontroller/ProfiledPIDController.java @@ -290,7 +290,7 @@ public void reset(double measuredPosition, double measuredVelocity) { * Reset the previous error and the integral term. * * @param measuredPosition The current measured position of the system. The velocity is - * assumed to be reset. + * assumed to be zero. */ public void reset(double measuredPosition) { reset(measuredPosition, 0.0); diff --git a/core/src/main/java/com/seattlesolvers/solverslib/controller/wpilibcontroller/SimpleMotorFeedforward.java b/core/src/main/java/com/seattlesolvers/solverslib/controller/wpilibcontroller/SimpleMotorFeedforward.java index d34ed097..9c9eda1f 100644 --- a/core/src/main/java/com/seattlesolvers/solverslib/controller/wpilibcontroller/SimpleMotorFeedforward.java +++ b/core/src/main/java/com/seattlesolvers/solverslib/controller/wpilibcontroller/SimpleMotorFeedforward.java @@ -32,7 +32,7 @@ public SimpleMotorFeedforward(double ks, double kv, double ka) { /** * Creates a new SimpleMotorFeedforward with the specified gains. Acceleration gain is - * defaulted to reset. Units of the gain values will dictate units of the computed feedforward. + * defaulted to zero. Units of the gain values will dictate units of the computed feedforward. * * @param ks The static gain. * @param kv The velocity gain. @@ -57,7 +57,7 @@ public double calculate(double velocity, double acceleration) { /** * Calculates the feedforward from the gains and velocity setpoint (acceleration is assumed to - * be reset). + * be zero). * * @param velocity The velocity setpoint. * @return The computed feedforward. diff --git a/core/src/main/java/com/seattlesolvers/solverslib/geometry/Translation2d.java b/core/src/main/java/com/seattlesolvers/solverslib/geometry/Translation2d.java index 40437293..88f41e0c 100644 --- a/core/src/main/java/com/seattlesolvers/solverslib/geometry/Translation2d.java +++ b/core/src/main/java/com/seattlesolvers/solverslib/geometry/Translation2d.java @@ -5,7 +5,7 @@ public class Translation2d { private final double m_y; /** - * Constructs a Translation2d with X and Y components equal to reset. + * Constructs a Translation2d with X and Y components equal to zero. */ public Translation2d() { this(0.0, 0.0); diff --git a/core/src/main/java/com/seattlesolvers/solverslib/kinematics/wpilibkinematics/ChassisSpeeds.java b/core/src/main/java/com/seattlesolvers/solverslib/kinematics/wpilibkinematics/ChassisSpeeds.java index 39f5976f..0627c117 100644 --- a/core/src/main/java/com/seattlesolvers/solverslib/kinematics/wpilibkinematics/ChassisSpeeds.java +++ b/core/src/main/java/com/seattlesolvers/solverslib/kinematics/wpilibkinematics/ChassisSpeeds.java @@ -69,7 +69,7 @@ public ChassisSpeeds(double vxMetersPerSecond, double vyMetersPerSecond, * Positive y is to your left when standing behind the alliance wall. * @param omegaRadiansPerSecond The angular rate of the robot. * @param robotAngle The angle of the robot as measured by a gyroscope. The robot's - * angle is considered to be reset when it is facing directly away + * angle is considered to be zero when it is facing directly away * from your alliance station wall. Remember that this should * be CCW positive. * @return ChassisSpeeds object representing the speeds in the robot's frame of reference. diff --git a/core/src/main/java/com/seattlesolvers/solverslib/purepursuit/Path.java b/core/src/main/java/com/seattlesolvers/solverslib/purepursuit/Path.java index c22618f2..41e7c95f 100644 --- a/core/src/main/java/com/seattlesolvers/solverslib/purepursuit/Path.java +++ b/core/src/main/java/com/seattlesolvers/solverslib/purepursuit/Path.java @@ -174,7 +174,7 @@ public boolean followPath(MecanumDrive mecanumDrive, Odometry odometry) { * This is the principle path method. After everything is configured and initiated, this method can be used. * Using the robot's horizontal, y, and rotation, this method calculates the appropriate motor powers for the * robot to follow the path. This method calls all triggered/interrupted actions automatically. If this - * returns reset motor speeds {0, 0, 0} that means the path has either (1) timed out, (2) lost the path and + * returns zero motor speeds {0, 0, 0} that means the path has either (1) timed out, (2) lost the path and * retrace was disabled, or (3) reached the destination. Use isFinished() and timedOut() to troubleshoot. * * @param vPosition Robot's current vertical position. diff --git a/core/src/main/java/com/seattlesolvers/solverslib/spline/SplineHelper.java b/core/src/main/java/com/seattlesolvers/solverslib/spline/SplineHelper.java index bb35aaf5..dc795d7f 100644 --- a/core/src/main/java/com/seattlesolvers/solverslib/spline/SplineHelper.java +++ b/core/src/main/java/com/seattlesolvers/solverslib/spline/SplineHelper.java @@ -114,14 +114,14 @@ public static CubicHermiteSpline[] getCubicSplinesFromControlVectors( https://www.uio.no/studier/emner/matnat/ifi/nedlagte-emner/INF-MAT4350/h08 /undervisningsmateriale/chap7alecture.pdf */ - // Above-diagonal of tridiagonal matrix, reset-padded + // Above-diagonal of tridiagonal matrix, zero-padded final double[] a = new double[newWaypts.length - 2]; // Diagonal of tridiagonal matrix final double[] b = new double[newWaypts.length - 2]; Arrays.fill(b, 4.0); - // Below-diagonal of tridiagonal matrix, reset-padded + // Below-diagonal of tridiagonal matrix, zero-padded final double[] c = new double[newWaypts.length - 2]; // rhs vectors @@ -244,7 +244,7 @@ private static void thomasAlgorithm(double[] a, double[] b, double[] dStar = new double[N]; // This updates the coefficients in the first row - // Note that we should be checking for division by reset here + // Note that we should be checking for division by zero here cStar[0] = c[0] / b[0]; dStar[0] = d[0] / b[0]; diff --git a/core/src/main/java/com/seattlesolvers/solverslib/trajectory/TrapezoidProfile.java b/core/src/main/java/com/seattlesolvers/solverslib/trajectory/TrapezoidProfile.java index 38baef4d..24508a3b 100644 --- a/core/src/main/java/com/seattlesolvers/solverslib/trajectory/TrapezoidProfile.java +++ b/core/src/main/java/com/seattlesolvers/solverslib/trajectory/TrapezoidProfile.java @@ -123,7 +123,7 @@ public TrapezoidProfile(Constraints constraints, State goal, State initial) { // Deal with a possibly truncated motion profile (with nonzero initial or // final velocity) by calculating the parameters as if the profile began and - // ended at reset velocity + // ended at zero velocity double cutoffBegin = m_initial.velocity / m_constraints.maxAcceleration; double cutoffDistBegin = cutoffBegin * cutoffBegin * m_constraints.maxAcceleration / 2.0; From bbe46ed59a1254f79cba0eb1cc24c7fac4328e24 Mon Sep 17 00:00:00 2001 From: "daniel.yang" Date: Mon, 3 Nov 2025 17:58:01 -0800 Subject: [PATCH 17/23] docs: fix "zero" and "reset" errors --- .../com/seattlesolvers/solverslib/command/Command.java | 2 +- .../solverslib/hardware/IncrementalEncoder.java | 9 +++++++++ 2 files changed, 10 insertions(+), 1 deletion(-) diff --git a/core/src/main/java/com/seattlesolvers/solverslib/command/Command.java b/core/src/main/java/com/seattlesolvers/solverslib/command/Command.java index b3db04e6..19788875 100644 --- a/core/src/main/java/com/seattlesolvers/solverslib/command/Command.java +++ b/core/src/main/java/com/seattlesolvers/solverslib/command/Command.java @@ -321,7 +321,7 @@ default boolean runsWhenDisabled() { * Wraps the command in {@link UninterruptibleCommand} internally. * @return the decorated command */ - default Command setUninterruptible() { + default Command uninterruptible() { return new UninterruptibleCommand(this); } diff --git a/core/src/main/java/com/seattlesolvers/solverslib/hardware/IncrementalEncoder.java b/core/src/main/java/com/seattlesolvers/solverslib/hardware/IncrementalEncoder.java index 448ac241..cedf6936 100644 --- a/core/src/main/java/com/seattlesolvers/solverslib/hardware/IncrementalEncoder.java +++ b/core/src/main/java/com/seattlesolvers/solverslib/hardware/IncrementalEncoder.java @@ -153,10 +153,19 @@ public double getCorrectedVelocity() { return real; } + /** + * Convert an angle to encoder ticks, given the CPR + * @param angle Angle to convert + * @return Number of encoder ticks + */ public int angleToTicks(double angle) { return (int) (angle * cpr / MathUtils.returnMaxForAngleUnit(angleUnit)); } + /** + * Gets the angle before normalizing + * @return Unnormalized angle + */ public double getAngleUnnormalized() { return getRevolutions() * MathUtils.returnMaxForAngleUnit(angleUnit); } From 2c3fbb4995ac774a8c4cb6dd5f316dd40243a5a6 Mon Sep 17 00:00:00 2001 From: "daniel.yang" Date: Mon, 3 Nov 2025 23:15:40 -0800 Subject: [PATCH 18/23] fix: CallbackCommand self generics are so bad. Changed to explicit wrapper calling --- .../solverslib/command/CallbackCommand.java | 41 ++++++++++++------- .../solverslib/command/Command.java | 24 +---------- .../solverslib/command/CommandBase.java | 2 +- 3 files changed, 29 insertions(+), 38 deletions(-) diff --git a/core/src/main/java/com/seattlesolvers/solverslib/command/CallbackCommand.java b/core/src/main/java/com/seattlesolvers/solverslib/command/CallbackCommand.java index 9aff64ba..26fc0409 100644 --- a/core/src/main/java/com/seattlesolvers/solverslib/command/CallbackCommand.java +++ b/core/src/main/java/com/seattlesolvers/solverslib/command/CallbackCommand.java @@ -1,8 +1,11 @@ package com.seattlesolvers.solverslib.command; +import java.util.Arrays; import java.util.HashMap; +import java.util.HashSet; import java.util.Iterator; import java.util.Map; +import java.util.Set; import java.util.function.BooleanSupplier; import java.util.function.Function; @@ -10,12 +13,13 @@ * Wrapper to easily add callbacks to a command * @author Daniel - FTC 7854 */ -public class CallbackCommand extends CommandBase { +public class CallbackCommand implements Command { private final Map whenRunnables = new HashMap<>(); private final Map whenCommands = new HashMap<>(); - private final Map, Runnable> whenSelfRunnables = new HashMap<>(); - private final Map, Command> whenSelfCommands = new HashMap<>(); - private final Command command; + private final Map, Runnable> whenSelfRunnables = new HashMap<>(); + private final Map, Command> whenSelfCommands = new HashMap<>(); + protected Set m_requirements = new HashSet<>(); + private final T command; /** * Wrapper for adding custom callbacks to commands. This expects a single command, @@ -24,10 +28,14 @@ public class CallbackCommand extends CommandBase { * {@link SequentialCommandGroup} * {@link ParallelCommandGroup} */ - public CallbackCommand(Command command) { + public CallbackCommand(T command) { this.command = command; } + public final void addRequirements(Subsystem... requirements) { + m_requirements.addAll(Arrays.asList(requirements)); + } + /** * Adds a callback with a boolean supplier * @param condition Runs the runnable the first time this is true @@ -35,7 +43,7 @@ public CallbackCommand(Command command) { * @return Itself for chaining purposes */ @Override - public CallbackCommand when(BooleanSupplier condition, Runnable runnable) { + public CallbackCommand when(BooleanSupplier condition, Runnable runnable) { whenRunnables.put(condition, runnable); return this; } @@ -47,7 +55,7 @@ public CallbackCommand when(BooleanSupplier condition, Runnable runnable) { * @return Itself for chaining purposes */ @Override - public CallbackCommand when(BooleanSupplier condition, Command command) { + public CallbackCommand when(BooleanSupplier condition, Command command) { whenCommands.put(condition, command); return this; } @@ -58,8 +66,7 @@ public CallbackCommand when(BooleanSupplier condition, Command command) { * @param runnable Callback to run * @return Itself for chaining purposes */ - @Override - public CallbackCommand whenSelf(Function condition, Runnable runnable) { + public CallbackCommand whenSelf(Function condition, Runnable runnable) { whenSelfRunnables.put(condition, runnable); return this; } @@ -70,8 +77,7 @@ public CallbackCommand whenSelf(Function condition, Runnable r * @param command Command to schedule * @return Itself for chaining purposes */ - @Override - public CallbackCommand whenSelf(Function condition, Command command) { + public CallbackCommand whenSelf(Function condition, Command command) { whenSelfCommands.put(condition, command); return this; } @@ -100,15 +106,15 @@ public void execute() { } // Self callbacks - for (Iterator, Runnable>> it = whenSelfRunnables.entrySet().iterator(); it.hasNext();) { - Map.Entry, Runnable> action = it.next(); + for (Iterator, Runnable>> it = whenSelfRunnables.entrySet().iterator(); it.hasNext();) { + Map.Entry, Runnable> action = it.next(); if (action.getKey().apply(command)) { action.getValue().run(); it.remove(); } } - for (Iterator, Command>> it = whenSelfCommands.entrySet().iterator(); it.hasNext();) { - Map.Entry, Command> action = it.next(); + for (Iterator, Command>> it = whenSelfCommands.entrySet().iterator(); it.hasNext();) { + Map.Entry, Command> action = it.next(); if (action.getKey().apply(command)) { action.getValue().schedule(); it.remove(); @@ -120,4 +126,9 @@ public void execute() { public boolean isFinished() { return !CommandScheduler.getInstance().isScheduled(command); } + + @Override + public Set getRequirements() { + return m_requirements; + } } diff --git a/core/src/main/java/com/seattlesolvers/solverslib/command/Command.java b/core/src/main/java/com/seattlesolvers/solverslib/command/Command.java index 19788875..83803327 100644 --- a/core/src/main/java/com/seattlesolvers/solverslib/command/Command.java +++ b/core/src/main/java/com/seattlesolvers/solverslib/command/Command.java @@ -332,7 +332,7 @@ default Command uninterruptible() { * @return the decorated command */ default Command when(BooleanSupplier condition, Runnable runnable) { - return new CallbackCommand(this).when(condition, runnable); + return new CallbackCommand<>(this).when(condition, runnable); } /** @@ -342,27 +342,7 @@ default Command when(BooleanSupplier condition, Runnable runnable) { * @return the decorated command */ default Command when(BooleanSupplier condition, Command command) { - return new CallbackCommand(this).when(condition, command); - } - - /** - * Adds a callback with access to the inner command - * @param condition Runs the runnable the first time this is true - * @param runnable Callback to run - * @return the decorated command - */ - default Command whenSelf(Function condition, Runnable runnable) { - return new CallbackCommand(this).whenSelf(condition, runnable); - } - - /** - * Adds a callback with access to the inner command - * @param condition Schedules the command the first time this is true - * @param command Command to schedule - * @return the decorated command - */ - default Command whenSelf(Function condition, Command command) { - return new CallbackCommand(this).whenSelf(condition, command); + return new CallbackCommand<>(this).when(condition, command); } default String getName() { diff --git a/core/src/main/java/com/seattlesolvers/solverslib/command/CommandBase.java b/core/src/main/java/com/seattlesolvers/solverslib/command/CommandBase.java index 5163c324..b30b0958 100644 --- a/core/src/main/java/com/seattlesolvers/solverslib/command/CommandBase.java +++ b/core/src/main/java/com/seattlesolvers/solverslib/command/CommandBase.java @@ -10,6 +10,7 @@ import java.util.Arrays; import java.util.HashSet; import java.util.Set; +import java.util.function.Function; /** * A base class for {@link Command}s. @@ -52,5 +53,4 @@ public String getSubsystem() { public void setSubsystem(String subsystem) { m_subsystem = subsystem; } - } \ No newline at end of file From 924da5b45c799f5f59ca6f28399f5d91dbca6c60 Mon Sep 17 00:00:00 2001 From: "daniel.yang" Date: Tue, 4 Nov 2025 07:02:12 -0800 Subject: [PATCH 19/23] refactor: function to predicate for better semantics --- .../solverslib/command/CallbackCommand.java | 22 +++++++++---------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/core/src/main/java/com/seattlesolvers/solverslib/command/CallbackCommand.java b/core/src/main/java/com/seattlesolvers/solverslib/command/CallbackCommand.java index 26fc0409..7ca45cc0 100644 --- a/core/src/main/java/com/seattlesolvers/solverslib/command/CallbackCommand.java +++ b/core/src/main/java/com/seattlesolvers/solverslib/command/CallbackCommand.java @@ -7,7 +7,7 @@ import java.util.Map; import java.util.Set; import java.util.function.BooleanSupplier; -import java.util.function.Function; +import java.util.function.Predicate; /** * Wrapper to easily add callbacks to a command @@ -16,8 +16,8 @@ public class CallbackCommand implements Command { private final Map whenRunnables = new HashMap<>(); private final Map whenCommands = new HashMap<>(); - private final Map, Runnable> whenSelfRunnables = new HashMap<>(); - private final Map, Command> whenSelfCommands = new HashMap<>(); + private final Map, Runnable> whenSelfRunnables = new HashMap<>(); + private final Map, Command> whenSelfCommands = new HashMap<>(); protected Set m_requirements = new HashSet<>(); private final T command; @@ -66,7 +66,7 @@ public CallbackCommand when(BooleanSupplier condition, Command command) { * @param runnable Callback to run * @return Itself for chaining purposes */ - public CallbackCommand whenSelf(Function condition, Runnable runnable) { + public CallbackCommand whenSelf(Predicate condition, Runnable runnable) { whenSelfRunnables.put(condition, runnable); return this; } @@ -77,7 +77,7 @@ public CallbackCommand whenSelf(Function condition, Runnable runn * @param command Command to schedule * @return Itself for chaining purposes */ - public CallbackCommand whenSelf(Function condition, Command command) { + public CallbackCommand whenSelf(Predicate condition, Command command) { whenSelfCommands.put(condition, command); return this; } @@ -106,16 +106,16 @@ public void execute() { } // Self callbacks - for (Iterator, Runnable>> it = whenSelfRunnables.entrySet().iterator(); it.hasNext();) { - Map.Entry, Runnable> action = it.next(); - if (action.getKey().apply(command)) { + for (Iterator, Runnable>> it = whenSelfRunnables.entrySet().iterator(); it.hasNext();) { + Map.Entry, Runnable> action = it.next(); + if (action.getKey().test(command)) { action.getValue().run(); it.remove(); } } - for (Iterator, Command>> it = whenSelfCommands.entrySet().iterator(); it.hasNext();) { - Map.Entry, Command> action = it.next(); - if (action.getKey().apply(command)) { + for (Iterator, Command>> it = whenSelfCommands.entrySet().iterator(); it.hasNext();) { + Map.Entry, Command> action = it.next(); + if (action.getKey().test(command)) { action.getValue().schedule(); it.remove(); } From a2d0026d850f0fcb9d793ba1d609fe483089b908 Mon Sep 17 00:00:00 2001 From: "daniel.yang" Date: Tue, 4 Nov 2025 07:18:18 -0800 Subject: [PATCH 20/23] feat: add consumers to CallbackCommand --- .../solverslib/command/CallbackCommand.java | 63 +++++++++++++++---- 1 file changed, 51 insertions(+), 12 deletions(-) diff --git a/core/src/main/java/com/seattlesolvers/solverslib/command/CallbackCommand.java b/core/src/main/java/com/seattlesolvers/solverslib/command/CallbackCommand.java index 7ca45cc0..b9032d0a 100644 --- a/core/src/main/java/com/seattlesolvers/solverslib/command/CallbackCommand.java +++ b/core/src/main/java/com/seattlesolvers/solverslib/command/CallbackCommand.java @@ -7,6 +7,7 @@ import java.util.Map; import java.util.Set; import java.util.function.BooleanSupplier; +import java.util.function.Consumer; import java.util.function.Predicate; /** @@ -16,8 +17,10 @@ public class CallbackCommand implements Command { private final Map whenRunnables = new HashMap<>(); private final Map whenCommands = new HashMap<>(); + private final Map> whenConsumers = new HashMap<>(); private final Map, Runnable> whenSelfRunnables = new HashMap<>(); private final Map, Command> whenSelfCommands = new HashMap<>(); + private final Map, Consumer> whenSelfConsumers = new HashMap<>(); protected Set m_requirements = new HashSet<>(); private final T command; @@ -39,46 +42,68 @@ public final void addRequirements(Subsystem... requirements) { /** * Adds a callback with a boolean supplier * @param condition Runs the runnable the first time this is true - * @param runnable Callback to run + * @param action Callback to run * @return Itself for chaining purposes */ @Override - public CallbackCommand when(BooleanSupplier condition, Runnable runnable) { - whenRunnables.put(condition, runnable); + public CallbackCommand when(BooleanSupplier condition, Runnable action) { + whenRunnables.put(condition, action); return this; } /** * Adds a callback with a boolean supplier * @param condition Schedules the command the first time this is true - * @param command Command to schedule + * @param action Command to schedule * @return Itself for chaining purposes */ @Override - public CallbackCommand when(BooleanSupplier condition, Command command) { - whenCommands.put(condition, command); + public CallbackCommand when(BooleanSupplier condition, Command action) { + whenCommands.put(condition, action); + return this; + } + + /** + * Adds a callback with a boolean supplier + * @param condition Schedules the command the first time this is true + * @param action Consumer for using the inner command + * @return Itself for chaining purposes + */ + public CallbackCommand when(BooleanSupplier condition, Consumer action) { + whenConsumers.put(condition, action); return this; } /** * Adds a callback with access to the inner command * @param condition Runs the runnable the first time this is true - * @param runnable Callback to run + * @param action Callback to run + * @return Itself for chaining purposes + */ + public CallbackCommand whenSelf(Predicate condition, Runnable action) { + whenSelfRunnables.put(condition, action); + return this; + } + + /** + * Adds a callback with access to the inner command + * @param condition Schedules the command the first time this is true + * @param action Consumer for using the inner command * @return Itself for chaining purposes */ - public CallbackCommand whenSelf(Predicate condition, Runnable runnable) { - whenSelfRunnables.put(condition, runnable); + public CallbackCommand whenSelf(Predicate condition, Command action) { + whenSelfCommands.put(condition, action); return this; } /** * Adds a callback with access to the inner command * @param condition Schedules the command the first time this is true - * @param command Command to schedule + * @param action Command to schedule * @return Itself for chaining purposes */ - public CallbackCommand whenSelf(Predicate condition, Command command) { - whenSelfCommands.put(condition, command); + public CallbackCommand whenSelf(Predicate condition, Consumer action) { + whenSelfConsumers.put(condition, action); return this; } @@ -104,6 +129,13 @@ public void execute() { it.remove(); } } + for (Iterator>> it = whenConsumers.entrySet().iterator(); it.hasNext();) { + Map.Entry> action = it.next(); + if (action.getKey().getAsBoolean()) { + action.getValue().accept(command); + it.remove(); + } + } // Self callbacks for (Iterator, Runnable>> it = whenSelfRunnables.entrySet().iterator(); it.hasNext();) { @@ -120,6 +152,13 @@ public void execute() { it.remove(); } } + for (Iterator, Consumer>> it = whenSelfConsumers.entrySet().iterator(); it.hasNext();) { + Map.Entry, Consumer> action = it.next(); + if (action.getKey().test(command)) { + action.getValue().accept(command); + it.remove(); + } + } } @Override From 24777c3bbc1bf8f224f9c4969276e510a7dad873 Mon Sep 17 00:00:00 2001 From: "daniel.yang" Date: Wed, 5 Nov 2025 20:15:58 -0800 Subject: [PATCH 21/23] refactor: rename when to whenSelf for Consumer callback --- .../com/seattlesolvers/solverslib/command/CallbackCommand.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/core/src/main/java/com/seattlesolvers/solverslib/command/CallbackCommand.java b/core/src/main/java/com/seattlesolvers/solverslib/command/CallbackCommand.java index b9032d0a..fe194394 100644 --- a/core/src/main/java/com/seattlesolvers/solverslib/command/CallbackCommand.java +++ b/core/src/main/java/com/seattlesolvers/solverslib/command/CallbackCommand.java @@ -69,7 +69,7 @@ public CallbackCommand when(BooleanSupplier condition, Command action) { * @param action Consumer for using the inner command * @return Itself for chaining purposes */ - public CallbackCommand when(BooleanSupplier condition, Consumer action) { + public CallbackCommand whenSelf(BooleanSupplier condition, Consumer action) { whenConsumers.put(condition, action); return this; } From 4ff415a7e8ef6a50464c0cf6ead68dde96680395 Mon Sep 17 00:00:00 2001 From: "daniel.yang" Date: Thu, 6 Nov 2025 10:54:33 -0800 Subject: [PATCH 22/23] feat: CRServoEx now supports any encoder type (e.g. AbsoluteAnalogEncoder, IncrementalEncoder) --- .../swerve/coaxial/CoaxialSwerveModule.java | 4 +- .../solverslib/hardware/motors/CRServoEx.java | 72 +++++++++++++------ 2 files changed, 53 insertions(+), 23 deletions(-) diff --git a/core/src/main/java/com/seattlesolvers/solverslib/drivebase/swerve/coaxial/CoaxialSwerveModule.java b/core/src/main/java/com/seattlesolvers/solverslib/drivebase/swerve/coaxial/CoaxialSwerveModule.java index aeaf75c8..2b386022 100644 --- a/core/src/main/java/com/seattlesolvers/solverslib/drivebase/swerve/coaxial/CoaxialSwerveModule.java +++ b/core/src/main/java/com/seattlesolvers/solverslib/drivebase/swerve/coaxial/CoaxialSwerveModule.java @@ -69,7 +69,7 @@ public void setTargetVelocity(Vector2d velocity) { public void updateModule() { // Wheel flipping optimization (if its quicker to swap motor direction and rotate the pod less, then do that) wheelFlipped = false; - angleError = MathUtils.normalizeRadians(MathUtils.normalizeRadians(targetVelocity.angle(), true) - swervo.getAbsoluteEncoder().getAngle(), false); + angleError = MathUtils.normalizeRadians(MathUtils.normalizeRadians(targetVelocity.angle(), true) - swervo.getEncoder().getAngle(), false); if (Math.abs(angleError) > Math.PI/2) { angleError += Math.PI * -Math.signum(angleError); wheelFlipped = true; @@ -116,7 +116,7 @@ public CoaxialSwerveModule setCachingTolerance(double motorCachingTolerance, dou public String getPowerTelemetry() { return "Motor: " + MathUtils.round(motor.get(), 3) + "; Servo: " + MathUtils.round(swervo.get(), 3) + - "; Absolute Encoder: " + MathUtils.round(swervo.getAbsoluteEncoder().getAngle(), 3); + "; Absolute Encoder: " + MathUtils.round(swervo.getEncoder().getAngle(), 3); } public Vector2d getTargetVelocity() { diff --git a/core/src/main/java/com/seattlesolvers/solverslib/hardware/motors/CRServoEx.java b/core/src/main/java/com/seattlesolvers/solverslib/hardware/motors/CRServoEx.java index c02a869b..126f9ac3 100644 --- a/core/src/main/java/com/seattlesolvers/solverslib/hardware/motors/CRServoEx.java +++ b/core/src/main/java/com/seattlesolvers/solverslib/hardware/motors/CRServoEx.java @@ -6,19 +6,24 @@ import com.qualcomm.robotcore.hardware.ServoControllerEx; import com.seattlesolvers.solverslib.controller.PIDFController; import com.seattlesolvers.solverslib.hardware.AbsoluteAnalogEncoder; +import com.seattlesolvers.solverslib.hardware.Encoder; import com.seattlesolvers.solverslib.util.MathUtils; import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; /** * An extended wrapper class for CRServos with more features - * such as integration with absolute analog encoders for Axon servos - * and their absolute encoders and power caching to reduce loop times. + *

+ * - Integration with absolute analog encoders (Axon) or incremental encoders (regular)
+ * - Power caching to reduce loop times + *

+ * + * @param the encoder type (e.g. AbsoluteAnalogEncoder, IncrementalEncoder) * * @author Saket */ -public class CRServoEx extends CRServo { - private AbsoluteAnalogEncoder absoluteEncoder; +public class CRServoEx> extends CRServo { + private T encoder; private double cachingTolerance = 0.0001; private PIDFController pidf; @@ -40,6 +45,17 @@ public enum RunMode { /** * The constructor for the CR Servo. * + *

Example Usage:

+ * 
{@code
+     * CRServoEx = new CrServoEx<>(
+     *     hardwareMap,
+     *     "servoId",
+     *     "encoderId",
+     *     3.3,
+     *     AngleUnit.RADIANS,
+     *     CRServoEx.RunMode.OptimizedPositionalControl
+     * )}
+ * * @param hwMap hardwareMap * @param id ID of the CR servo as configured * @param encoderID ID of the absolute encoder as configured @@ -47,34 +63,48 @@ public enum RunMode { * @param angleUnit the angle unit for the absolute encoder * @param runmode the runmode of the CR servo */ + @SuppressWarnings("unchecked") public CRServoEx(HardwareMap hwMap, String id, String encoderID, double analogRange, AngleUnit angleUnit, RunMode runmode) { super(hwMap, id); - this.absoluteEncoder = new AbsoluteAnalogEncoder(hwMap, encoderID, analogRange, angleUnit); + this.encoder = (T) new AbsoluteAnalogEncoder(hwMap, encoderID, analogRange, angleUnit); this.runmode = runmode; } /** * The constructor for the CR Servo. * + *

Example Usage:

+ * 
{@code
+     * CRServoEx = new CrServoEx<>(
+     *     hardwareMap,
+     *     "servoId",
+     *     encoder,
+     *     CRServoEx.RunMode.OptimizedPositionalControl
+     * )}
+ * * @param hwMap hardwareMap * @param id ID of the CR servo as configured - * @param absoluteEncoder the object for the absolute encoder to be associated with this servo + * @param encoder the object for the absolute encoder to be associated with this servo * @param runmode the runmode of the CR servo */ - public CRServoEx(HardwareMap hwMap, String id, AbsoluteAnalogEncoder absoluteEncoder, RunMode runmode) { + public CRServoEx(HardwareMap hwMap, String id, T encoder, RunMode runmode) { super(hwMap, id); - this.absoluteEncoder = absoluteEncoder; + this.encoder = encoder; this.runmode = runmode; } /** * A simple constructor for the CR Servo with no absolute encoder. + * + *

Example Usage:

+ * 
{@code CRServoEx = new CrServoEx<>(hardwareMap, "servoId")}
+ * * @param hwMap hardwareMap * @param id ID of the CR servo as configured */ public CRServoEx(HardwareMap hwMap, String id) { super(hwMap, id); - this.absoluteEncoder = null; + this.encoder = null; this.runmode = RunMode.RawPower; } @@ -82,7 +112,7 @@ public CRServoEx(HardwareMap hwMap, String id) { * @param runmode the new runmode to be set * @return this object for chaining purposes */ - public CRServoEx setRunMode(RunMode runmode) { + public CRServoEx setRunMode(RunMode runmode) { this.runmode = runmode; return this; } @@ -92,7 +122,7 @@ public CRServoEx setRunMode(RunMode runmode) { * @param coefficients the coefficients for the PIDF controller * @return this object for chaining purposes */ - public CRServoEx setPIDF(PIDFCoefficients coefficients) { + public CRServoEx setPIDF(PIDFCoefficients coefficients) { this.pidf = new PIDFController(coefficients); return this; } @@ -101,7 +131,7 @@ public CRServoEx setPIDF(PIDFCoefficients coefficients) { * @param cachingTolerance the new caching tolerance between CR servo writes * @return this object for chaining purposes */ - public CRServoEx setCachingTolerance(double cachingTolerance) { + public CRServoEx setCachingTolerance(double cachingTolerance) { this.cachingTolerance = cachingTolerance; return this; } @@ -114,19 +144,19 @@ public double getCachingTolerance() { } /** - * @param absoluteEncoder the new absolute encoder to be associated with the CR servo + * @param encoder the new absolute encoder to be associated with the CR servo * @return this object for chaining purposes */ - public CRServoEx setAbsoluteEncoder(AbsoluteAnalogEncoder absoluteEncoder) { - this.absoluteEncoder = absoluteEncoder; + public CRServoEx setEncoder(T encoder) { + this.encoder = encoder; return this; } /** * @return the absolute encoder object associated with the CR servo */ - public AbsoluteAnalogEncoder getAbsoluteEncoder() { - return absoluteEncoder; + public T getEncoder() { + return encoder; } /** @@ -135,11 +165,11 @@ public AbsoluteAnalogEncoder getAbsoluteEncoder() { @Override public void set(double output) { if (runmode == RunMode.OptimizedPositionalControl) { - if (absoluteEncoder == null) { + if (encoder == null) { throw new IllegalStateException("Must have absolute encoder and PIDF coefficients for CR Servo to be in positional control"); } - double normalizedTarget = MathUtils.normalizeAngle(output, true, absoluteEncoder.getAngleUnit()); - double error = MathUtils.normalizeAngle(normalizedTarget - absoluteEncoder.getAngle(), false, absoluteEncoder.getAngleUnit()); + double normalizedTarget = MathUtils.normalizeAngle(output, true, encoder.getAngleUnit()); + double error = MathUtils.normalizeAngle(normalizedTarget - encoder.getAngle(), false, encoder.getAngleUnit()); double power = pidf.calculate(0, error); setPower(power); } else { @@ -151,7 +181,7 @@ public void set(double output) { * @param pwmRange the PWM range the CR servo should be set to * @return this object for chaining purposes */ - public CRServoEx setPwm(PwmControl.PwmRange pwmRange) { + public CRServoEx setPwm(PwmControl.PwmRange pwmRange) { getController().setServoPwmRange(crServo.getPortNumber(), pwmRange); return this; } From ad6af2dd9f20b09744d4ebd4873d3a3a28d1b592 Mon Sep 17 00:00:00 2001 From: "daniel.yang" Date: Thu, 6 Nov 2025 11:00:47 -0800 Subject: [PATCH 23/23] refactor: deprecate old CRServoEx constructor that only works with AbsoluteAnalogEncoder due to dependency injection --- .../solverslib/hardware/motors/CRServoEx.java | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/core/src/main/java/com/seattlesolvers/solverslib/hardware/motors/CRServoEx.java b/core/src/main/java/com/seattlesolvers/solverslib/hardware/motors/CRServoEx.java index 126f9ac3..358e0615 100644 --- a/core/src/main/java/com/seattlesolvers/solverslib/hardware/motors/CRServoEx.java +++ b/core/src/main/java/com/seattlesolvers/solverslib/hardware/motors/CRServoEx.java @@ -47,7 +47,7 @@ public enum RunMode { * *

Example Usage:

* 
{@code
-     * CRServoEx = new CrServoEx<>(
+     * CRServoEx servo = new CrServoEx<>(
      *     hardwareMap,
      *     "servoId",
      *     "encoderId",
@@ -62,7 +62,11 @@ public enum RunMode {
      * @param analogRange the range of voltage for the absolute encoder
      * @param angleUnit the angle unit for the absolute encoder
      * @param runmode the runmode of the CR servo
+     *
+     * @deprecated Goes against dependency injection.
+     *             Use {@link #CRServoEx(HardwareMap, String, Encoder, CRServoEx.RunMode)} instead.
      */
+    @Deprecated
     @SuppressWarnings("unchecked")
     public CRServoEx(HardwareMap hwMap, String id, String encoderID, double analogRange, AngleUnit angleUnit, RunMode runmode) {
         super(hwMap, id);
@@ -75,7 +79,7 @@ public CRServoEx(HardwareMap hwMap, String id, String encoderID, double analogRa
      *
      * 
 Example Usage: 

      * 
{@code
-     * CRServoEx = new CrServoEx<>(
+     * CRServoEx servo = new CrServoEx<>(
      *     hardwareMap,
      *     "servoId",
      *     encoder,
@@ -97,7 +101,7 @@ public CRServoEx(HardwareMap hwMap, String id, T encoder, RunMode runmode) {
      * A simple constructor for the CR Servo with no absolute encoder.
      *
      * 
 Example Usage: 

-     * 
{@code CRServoEx = new CrServoEx<>(hardwareMap, "servoId")}

+     * 
{@code CRServoEx servo = new CrServoEx<>(hardwareMap, "servoId")}

      *
      * @param hwMap hardwareMap
      * @param id ID of the CR servo as configured