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..fe194394 --- /dev/null +++ b/core/src/main/java/com/seattlesolvers/solverslib/command/CallbackCommand.java @@ -0,0 +1,173 @@ +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.Consumer; +import java.util.function.Predicate; + +/** + * Wrapper to easily add callbacks to a command + * @author Daniel - FTC 7854 + */ +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; + + /** + * 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(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 + * @param action Callback to run + * @return Itself for chaining purposes + */ + @Override + 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 action Command to schedule + * @return Itself for chaining purposes + */ + @Override + 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 whenSelf(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 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, 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 action Command to schedule + * @return Itself for chaining purposes + */ + public CallbackCommand whenSelf(Predicate condition, Consumer action) { + whenSelfConsumers.put(condition, action); + 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(); + } + } + 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();) { + 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().test(command)) { + action.getValue().schedule(); + 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 + 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 236f1912..83803327 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 @@ -315,8 +316,36 @@ default boolean runsWhenDisabled() { return false; } + /** + * Sets this command as uninterruptible. + * Wraps the command in {@link UninterruptibleCommand} internally. + * @return the decorated command + */ + default Command uninterruptible() { + 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); + } + default String getName() { return this.getClass().getSimpleName(); } - } \ No newline at end of file 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 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} */ 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..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().getCurrentPosition(), 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().getCurrentPosition(), 3); + "; Absolute Encoder: " + MathUtils.round(swervo.getEncoder().getAngle(), 3); } public Vector2d getTargetVelocity() { 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..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,15 +10,12 @@ * An extended wrapper class for AnalogInput absolute encoders. * * @author Saket + * @author Daniel - 7854 */ -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 @@ -27,11 +24,10 @@ public class AbsoluteAnalogEncoder implements HardwareDevice { * @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; - reversed = false; } /** @@ -44,72 +40,63 @@ public AbsoluteAnalogEncoder(HardwareMap hwMap, String id) { } /** - * 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 + * @return The raw voltage returned by the encoder */ - public AbsoluteAnalogEncoder zero(double offset) { - this.offset = offset; - return this; + public double getVoltage() { + return encoder.getVoltage(); } - /** - * 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; + public double getRawAngle() { + return getVoltage() / range * MathUtils.returnMaxForAngleUnit(angleUnit); } /** - * Gets whether the encoder is reversed or not - * @return Whether the encoder is reversed + * Alias for {@link #getAngle()} for backwards compatibility + * @return angle of the encoder */ - public boolean getReversed() { - return reversed; + @Deprecated + public double getCurrentPosition() { + return getAngle(); } + /** - * @return The normalized angular position of the encoder in the unit previously specified by the user from 0 to max + * Manually sets the offset. Use setOffset instead + * @param offset The offset angle + * @return The object itself for chaining purposes */ - public double getCurrentPosition() { - return MathUtils.normalizeAngle( - (!reversed ? 1 - getVoltage() / range : getVoltage() / range) * MathUtils.returnMaxForAngleUnit(angleUnit) - offset, - true, - angleUnit - ); + @Deprecated + public AbsoluteAnalogEncoder zero(double offset) { + return setOffset(offset); } /** - * @return The AnalogInput object of the encoder itself + * Manually sets the offset. + * @param offset The offset angle + * @return The object itself for chaining purposes */ - public AnalogInput getEncoder() { - return encoder; + public AbsoluteAnalogEncoder setOffset(double offset) { + this.offset = offset; + return this; } - /** - * @return The raw voltage returned by the encoder - */ - public double getVoltage(){ - return encoder.getVoltage(); + @Override + public AbsoluteAnalogEncoder setAngle(double angle) { + offset += getAngle() - angle; + return null; } @Override - public void disable() { - // "take no action" (encoder.close() call in SDK) + public double getAngle() { + return MathUtils.normalizeAngle( + getDirectionMultiplier() * (getRawAngle() + offset), + true, + angleUnit + ); } @Override 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/Encoder.java b/core/src/main/java/com/seattlesolvers/solverslib/hardware/Encoder.java new file mode 100644 index 00000000..ddd567f3 --- /dev/null +++ b/core/src/main/java/com/seattlesolvers/solverslib/hardware/Encoder.java @@ -0,0 +1,89 @@ +package com.seattlesolvers.solverslib.hardware; + +import com.seattlesolvers.solverslib.util.RotationDirection; + +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) + * + * @author Daniel + */ +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(); + + /** + * @return Direction multiplier: 1 for normal, -1 for reversed + */ + int getDirectionMultiplier(); + + /** + * 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 + */ + 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 + */ + T setReversed(boolean reversed); + + /** + * @return The direction of the encoder + */ + RotationDirection getDirection(); + + /** + * @return Whether or not an encoder is reversed + */ + boolean getReversed(); + + /** + * Resets the encoder to a particular angle. + * @return The object itself for chaining purposes + */ + T setAngle(double angle); + + /** + * Manually sets the offset for any future values returned + */ + T setOffset(double offset); + + /** + * @return The inner encoder + */ + E getEncoder(); + + /** + * Manually zeroes the offset for any future values returned + */ + default T zeroOffset() { + return setOffset(0); + } + + /** + * Resets the encoder to 0 + * @return The object itself for chaining purposes + */ + default T reset() { + 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 new file mode 100644 index 00000000..e663cb11 --- /dev/null +++ b/core/src/main/java/com/seattlesolvers/solverslib/hardware/EncoderBase.java @@ -0,0 +1,66 @@ +package com.seattlesolvers.solverslib.hardware; + +import com.seattlesolvers.solverslib.util.RotationDirection; + +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; + +/** + * A base class for {@link Encoder}. + * @author Daniel - 7854 + */ +public abstract class EncoderBase> implements Encoder { + protected double offset = 0.0; + protected RotationDirection direction = RotationDirection.FORWARD; + protected AngleUnit angleUnit = AngleUnit.RADIANS; + protected final E encoder; + + protected EncoderBase(E encoder) { + this.encoder = encoder; + } + + @Override + public int getDirectionMultiplier() { + return direction.getMultiplier(); + } + + @SuppressWarnings("unchecked") + @Override + public T setDirection(RotationDirection direction) { + this.direction = direction; + return (T) this; + } + + @SuppressWarnings("unchecked") + @Override + public T setReversed(boolean reversed) { + direction = reversed ? RotationDirection.REVERSE : RotationDirection.FORWARD; + return (T) this; + } + + @SuppressWarnings("unchecked") + @Override + public T setOffset(double offset) { + this.offset = offset; + return (T) this; + } + + @Override + public RotationDirection getDirection() { + return direction; + } + + @Override + public boolean getReversed() { + return direction == RotationDirection.REVERSE; + } + + @Override + public AngleUnit getAngleUnit() { + return angleUnit; + } + + @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 new file mode 100644 index 00000000..cedf6936 --- /dev/null +++ b/core/src/main/java/com/seattlesolvers/solverslib/hardware/IncrementalEncoder.java @@ -0,0 +1,197 @@ +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; + +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; + + /** + * 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 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 countsPerRevolution, AngleUnit angleUnit) { + this(hwMap.get(DcMotor.class, id), countsPerRevolution, angleUnit); + } + + /** + * The constructor for incremental encoders, defaults to radians + * @param encoder the DcMotor which encoder is bound to + * @param countsPerRevolution the number of encoder ticks per full revolution, aka cycles per revolution + */ + 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); + } + + /** + * @return the current position of the encoder + */ + public int getPosition() { + int currentPosition = encoder.getCurrentPosition(); + if (currentPosition != lastPosition) { + double currentTime = 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 getDistancePerPulse() { + 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() / cpr; + } + + /** + * @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; + } + + /** + * 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); + } + + @Override + public IncrementalEncoder setAngle(double angle) { + offset = getPosition() - angleToTicks(angle); + return this; + } + + @Override + public DcMotor getEncoder() { + return encoder; + } + + @Override + public double getAngle() { + return MathUtils.normalizeAngle( + getAngleUnnormalized(), + true, + angleUnit + ); + } + + @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/CRServoEx.java b/core/src/main/java/com/seattlesolvers/solverslib/hardware/motors/CRServoEx.java index 112862b3..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 @@ -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,41 +45,70 @@ public enum RunMode { /** * The constructor for the CR Servo. * + *

Example Usage:

+ * 
{@code
+     * CRServoEx servo = 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 * @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); - 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 servo = 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 servo = 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 +116,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 +126,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 +135,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 +148,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 +169,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.getCurrentPosition(), 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 +185,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; } 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..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 @@ -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; + private final double cpr, rpm; GoBILDA(double cpr, double rpm) { this.cpr = cpr; @@ -46,144 +45,6 @@ public double getAchievableMaxTicksPerSecond() { } } - public enum Direction { - FORWARD(1), REVERSE(-1); - - private int val; - - Direction(int multiplier) { - val = multiplier; - } - - public int getMultiplier() { - return val; - } - } - - 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 +69,7 @@ public DcMotor.ZeroPowerBehavior getBehavior() { } public DcMotor motor; - public Encoder encoder; + public IncrementalEncoder encoder; /** * The runmode of the motor @@ -258,7 +119,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 +170,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); } @@ -345,7 +206,7 @@ public void resetEncoder() { * Resets the internal position of the motor. */ public void stopAndResetEncoder() { - encoder.resetVal = 0; + encoder.zeroOffset(); motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); motor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); } @@ -456,7 +317,7 @@ public double get() { * @param target the target position in ticks */ public void setTargetPosition(int target) { - setTargetDistance(target * encoder.dpp); + setTargetDistance(target * encoder.getDistancePerPulse()); } /** @@ -483,7 +344,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/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/CommandSample/DriveSubsystem.java b/examples/src/main/java/org/firstinspires/ftc/teamcode/CommandSample/DriveSubsystem.java index 3b458cc4..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 @@ -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; 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..342743dd 100644 --- a/examples/src/main/java/org/firstinspires/ftc/teamcode/DeadWheelsSample.java +++ b/examples/src/main/java/org/firstinspires/ftc/teamcode/DeadWheelsSample.java @@ -1,13 +1,14 @@ 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; 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 @@ -40,7 +41,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 @@ -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,