diff --git a/src/main/java/org/hyperonline/hyperlib/controller/HYPER_CANSparkMax.java b/src/main/java/org/hyperonline/hyperlib/controller/HYPER_CANSparkMax.java index 9e13ea0..c6472a3 100644 --- a/src/main/java/org/hyperonline/hyperlib/controller/HYPER_CANSparkMax.java +++ b/src/main/java/org/hyperonline/hyperlib/controller/HYPER_CANSparkMax.java @@ -3,13 +3,12 @@ import com.ctre.phoenix.motorcontrol.NeutralMode; import com.revrobotics.*; import edu.wpi.first.util.sendable.SendableBuilder; +import org.hyperonline.hyperlib.controller.meta.RawController; import org.hyperonline.hyperlib.controller.sensor.HYPER_SparkMaxAbsoluteEncoder; import org.hyperonline.hyperlib.controller.sensor.HYPER_SparkMaxAnalogSensor; import org.hyperonline.hyperlib.controller.sensor.HYPER_SparkMaxLimitSwitch; import org.hyperonline.hyperlib.controller.sensor.HYPER_SparkMaxRelativeEncoder; -import java.util.function.DoubleConsumer; - /** * wrapper for added behavior on the {@link CANSparkMax}. * @@ -24,10 +23,7 @@ * * @author Chris McGroarty */ -public class HYPER_CANSparkMax extends CANSparkMax implements SendableMotorController { - - public DoubleConsumer consumeSpeed = speed -> this.set(speed); - +public class HYPER_CANSparkMax extends CANSparkMax implements RawController { /** * Create a new object to control a SPARK MAX motor Controller. * diff --git a/src/main/java/org/hyperonline/hyperlib/controller/HYPER_PWMSparkMax.java b/src/main/java/org/hyperonline/hyperlib/controller/HYPER_PWMSparkMax.java index 564edca..ba44b8b 100644 --- a/src/main/java/org/hyperonline/hyperlib/controller/HYPER_PWMSparkMax.java +++ b/src/main/java/org/hyperonline/hyperlib/controller/HYPER_PWMSparkMax.java @@ -2,8 +2,9 @@ import com.ctre.phoenix.motorcontrol.NeutralMode; import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax; +import org.hyperonline.hyperlib.controller.meta.RawController; -public class HYPER_PWMSparkMax extends PWMSparkMax implements SendableMotorController { +public class HYPER_PWMSparkMax extends PWMSparkMax implements RawController { /** * Common initialization code called by all constructors. * diff --git a/src/main/java/org/hyperonline/hyperlib/controller/HYPER_TalonSRX.java b/src/main/java/org/hyperonline/hyperlib/controller/HYPER_TalonSRX.java index 5e070d8..2bc598e 100644 --- a/src/main/java/org/hyperonline/hyperlib/controller/HYPER_TalonSRX.java +++ b/src/main/java/org/hyperonline/hyperlib/controller/HYPER_TalonSRX.java @@ -2,6 +2,7 @@ import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; import edu.wpi.first.util.sendable.SendableBuilder; +import org.hyperonline.hyperlib.controller.meta.RawController; import java.util.function.DoubleConsumer; @@ -15,7 +16,7 @@ * * @author Chris McGroarty */ -public class HYPER_TalonSRX extends WPI_TalonSRX implements SendableMotorController { +public class HYPER_TalonSRX extends WPI_TalonSRX implements RawController { private final boolean m_useSensor; diff --git a/src/main/java/org/hyperonline/hyperlib/controller/HYPER_VictorSPX.java b/src/main/java/org/hyperonline/hyperlib/controller/HYPER_VictorSPX.java index f23667c..02660b7 100644 --- a/src/main/java/org/hyperonline/hyperlib/controller/HYPER_VictorSPX.java +++ b/src/main/java/org/hyperonline/hyperlib/controller/HYPER_VictorSPX.java @@ -1,6 +1,7 @@ package org.hyperonline.hyperlib.controller; import com.ctre.phoenix.motorcontrol.can.WPI_VictorSPX; +import org.hyperonline.hyperlib.controller.meta.RawController; import java.util.function.DoubleConsumer; @@ -14,7 +15,7 @@ * * @author Chris McGroarty */ -public class HYPER_VictorSPX extends WPI_VictorSPX implements SendableMotorController { +public class HYPER_VictorSPX extends WPI_VictorSPX implements RawController { public DoubleConsumer consumeSpeed = speed -> this.set(speed); /** diff --git a/src/main/java/org/hyperonline/hyperlib/controller/meta/MetaController.java b/src/main/java/org/hyperonline/hyperlib/controller/meta/MetaController.java new file mode 100644 index 0000000..f29efec --- /dev/null +++ b/src/main/java/org/hyperonline/hyperlib/controller/meta/MetaController.java @@ -0,0 +1,48 @@ +package org.hyperonline.hyperlib.controller.meta; + +import org.hyperonline.hyperlib.controller.SendableMotorController; +import org.hyperonline.hyperlib.controller.meta.groups.ControllerGroup; + +/** + * Interface that represents a meta-controller as opposed to a raw controller (see HYPER_* classes) + * @param type of controller this wraps - could also be a MetaController itself + * + * @author Dheeraj Prakash + */ +public interface MetaController extends SendableMotorController { + /** + * Get the controller that this meta-controller wraps + * @return the controller this wraps + */ + T getController(); + + /** + * Travel down the meta-controller chain and obtain the raw controller being wrapped. + * @return the raw controller. + * + * FIXME: make this return not just SendableMotorController + */ + default SendableMotorController getRawController() { + SendableMotorController tmp = this.getController(); + while (true) { + if (tmp instanceof MetaController) tmp = ((MetaController) tmp).getController(); + else if (tmp instanceof RawController) return tmp; + } + } + + /** + * Travel down the meta-controller chain and obtain the {@link ControllerGroup} being used. + * Useful to get the slave controller instead of master at the end of the chain. + * @return ControllerGroup + * + * FIXME: make this return the correct subclass + */ + default ControllerGroup getControllerGroup() { + SendableMotorController tmp = this.getController(); + while (true) { + if (tmp instanceof ControllerGroup) return (ControllerGroup) tmp; + else if (tmp instanceof MetaController) tmp = ((MetaController) tmp).getController(); + else return null; + } + } +} diff --git a/src/main/java/org/hyperonline/hyperlib/controller/meta/RawController.java b/src/main/java/org/hyperonline/hyperlib/controller/meta/RawController.java new file mode 100644 index 0000000..251b138 --- /dev/null +++ b/src/main/java/org/hyperonline/hyperlib/controller/meta/RawController.java @@ -0,0 +1,9 @@ +package org.hyperonline.hyperlib.controller.meta; + +import org.hyperonline.hyperlib.controller.SendableMotorController; + +/** + * Interface to represent a raw (not {@link MetaController}) controller. + * Intentionally has no methods, just serves as an endpoint in the MetaController composition chain. + */ +public interface RawController extends SendableMotorController {} diff --git a/src/main/java/org/hyperonline/hyperlib/controller/meta/groups/CANSparkMaxGroup.java b/src/main/java/org/hyperonline/hyperlib/controller/meta/groups/CANSparkMaxGroup.java new file mode 100644 index 0000000..dfe9526 --- /dev/null +++ b/src/main/java/org/hyperonline/hyperlib/controller/meta/groups/CANSparkMaxGroup.java @@ -0,0 +1,17 @@ +package org.hyperonline.hyperlib.controller.meta.groups; + +import org.hyperonline.hyperlib.controller.HYPER_CANSparkMax; +import org.hyperonline.hyperlib.controller.meta.MetaController; + +/** + * Class that groups together two {@link HYPER_CANSparkMax}es as master and slave. + * Useful to link controllers together to pass as one elsewhere. + * + * @author Dheeraj Prakash + */ +public class CANSparkMaxGroup extends ControllerGroup implements MetaController { + public CANSparkMaxGroup(HYPER_CANSparkMax master, HYPER_CANSparkMax slave, boolean invertSlave) { + super(master, slave); + slave.follow(master, invertSlave); + } +} diff --git a/src/main/java/org/hyperonline/hyperlib/controller/meta/groups/ControllerGroup.java b/src/main/java/org/hyperonline/hyperlib/controller/meta/groups/ControllerGroup.java new file mode 100644 index 0000000..0161323 --- /dev/null +++ b/src/main/java/org/hyperonline/hyperlib/controller/meta/groups/ControllerGroup.java @@ -0,0 +1,88 @@ +package org.hyperonline.hyperlib.controller.meta.groups; + +import com.ctre.phoenix.motorcontrol.NeutralMode; +import edu.wpi.first.util.sendable.SendableBuilder; +import org.hyperonline.hyperlib.controller.meta.MetaController; +import org.hyperonline.hyperlib.controller.SendableMotorController; + +/** + * Base class that facilitates {@link CANSparkMaxGroup} and {@link PhoenixControllerGroup} with common functionality + * (delegating methods to the two controllers involved). + * Intentionally package private - external users don't need to see this. + * @param master motor type + * @param slave motor type + * + * @author Dheeraj Prakash + */ +public abstract class ControllerGroup implements MetaController { + protected final M master; + protected final S slave; + + public ControllerGroup(M master, S slave) { + this.master = master; + this.slave = slave; + } + + @Override + public void setNeutralMode(NeutralMode mode) { + master.setNeutralMode(mode); + slave.setNeutralMode(mode); + } + + @Override + public void resetMotorConfig() { + master.resetMotorConfig(); + slave.resetMotorConfig(); + } + + @Override + public void initSendable(SendableBuilder builder) { + master.initSendable(builder); + slave.initSendable(builder); + } + + @Override + public void set(double speed) { + master.set(speed); + } + + @Override + public double get() { + return master.get(); + } + + @Override + public M getController() { + return master; + } + + @Override + public void setInverted(boolean isInverted) { + throw new UnsupportedOperationException("Cannot invert " + getClass().getSimpleName() + ". Invert individual controllers instead."); + } + + @Override + public boolean getInverted() { + return false; + } + + @Override + public void disable() { + master.disable(); + slave.disable(); + } + + @Override + public void stopMotor() { + master.stopMotor(); + slave.stopMotor(); + } + + public M getMaster() { + return master; + } + + public S getSlave() { + return slave; + } +} diff --git a/src/main/java/org/hyperonline/hyperlib/controller/meta/groups/PhoenixControllerGroup.java b/src/main/java/org/hyperonline/hyperlib/controller/meta/groups/PhoenixControllerGroup.java new file mode 100644 index 0000000..9a37d42 --- /dev/null +++ b/src/main/java/org/hyperonline/hyperlib/controller/meta/groups/PhoenixControllerGroup.java @@ -0,0 +1,25 @@ +package org.hyperonline.hyperlib.controller.meta.groups; + +import com.ctre.phoenix.motorcontrol.IMotorController; +import org.hyperonline.hyperlib.controller.meta.MetaController; +import org.hyperonline.hyperlib.controller.SendableMotorController; + +/** + * Class that groups together two Phoenix speed controllers (TalonSRX/VictorSPX) as master and slave. + * Useful to link controllers together to pass as one elsewhere. + * + * @param type of master controller used + * @param type of slave controller used + * + * @author Dheeraj Prakash + */ +public class PhoenixControllerGroup< + M extends SendableMotorController & IMotorController, + S extends SendableMotorController & IMotorController + > extends ControllerGroup implements MetaController { + public PhoenixControllerGroup(M master, S slave, boolean invertSlave) { + super(master, slave); + slave.follow(master); + slave.setInverted(invertSlave); + } +} diff --git a/src/main/java/org/hyperonline/hyperlib/controller/meta/limits/DualLimitedController.java b/src/main/java/org/hyperonline/hyperlib/controller/meta/limits/DualLimitedController.java new file mode 100644 index 0000000..7274030 --- /dev/null +++ b/src/main/java/org/hyperonline/hyperlib/controller/meta/limits/DualLimitedController.java @@ -0,0 +1,219 @@ +package org.hyperonline.hyperlib.controller.meta.limits; + +import com.ctre.phoenix.motorcontrol.NeutralMode; +import edu.wpi.first.util.sendable.SendableBuilder; +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Subsystem; +import org.hyperonline.hyperlib.controller.meta.MetaController; +import org.hyperonline.hyperlib.controller.SendableMotorController; +import org.hyperonline.hyperlib.pref.DoublePreference; + +import java.util.function.BooleanSupplier; +import java.util.function.DoubleSupplier; + +/** + * Simple wrapper over {@link ForwardLimitedController} and {@link ReverseLimitedController} in one class. + * Offers marginal functionality over having both individually. + * + * @author Dheeraj Prakash + */ +public class DualLimitedController implements MetaController { + private final T controller; + private final ForwardLimitedController forwardController; + private final ReverseLimitedController reverseController; + + public DualLimitedController(T controller, DigitalInput forwardLimit, DigitalInput reverseLimit, DoublePreference forwardSpeed, DoublePreference reverseSpeed) { + this.controller = controller; + this.forwardController = new ForwardLimitedController<>(controller, forwardLimit, forwardSpeed); + this.reverseController = new ReverseLimitedController<>(controller, reverseLimit, reverseSpeed); + } + + public void setSubsystem(Subsystem subsystem) { + forwardController.setSubsystem(subsystem); + reverseController.setSubsystem(subsystem); + } + + public boolean canMove(double speed) { + if (speed > 0) return canMoveForward(); + if (speed < 0) return canMoveReverse(); + return true; + } + + public boolean canMove(DoubleSupplier speed) { + return this.canMove(speed.getAsDouble()); + } + + public void move(DoubleSupplier speed) { + forwardController.move(speed); + } + + // delegate methods to controller classes + + public void move(double speed) { + if (speed > 0 && canMoveForward()) forwardController.move(speed); + if (speed < 0 && canMoveReverse()) reverseController.move(speed); + } + + public void stop() { + forwardController.stop(); + } + + public void forward(double multiplier) { + forwardController.forward(multiplier); + } + + public void forward(DoubleSupplier multiplier) { + forwardController.forward(multiplier); + } + + public void forward() { + forwardController.forward(); + } + + public Command forwardCmd(double multiplier) { + return forwardController.forwardCmd(multiplier); + } + + public Command forwardCmd(DoubleSupplier multiplier) { + return forwardController.forwardCmd(multiplier); + } + + public Command forwardCmd() { + return forwardController.forwardCmd(); + } + + public Command conditionalForwardCmd(BooleanSupplier condition, double multiplier) { + return forwardController.conditionalForwardCmd(condition, multiplier); + } + + public Command conditionalForwardCmd(BooleanSupplier condition, DoubleSupplier multiplier) { + return forwardController.conditionalForwardCmd(condition, multiplier); + } + + public Command conditionalForwardCmd(BooleanSupplier condition) { + return forwardController.conditionalForwardCmd(condition); + } + + public boolean atForwardLimit() { + return forwardController.atForwardLimit(); + } + + public boolean canMoveForward() { + return forwardController.canMoveForward(); + } + + public void reverse(double multiplier) { + reverseController.reverse(multiplier); + } + + public void reverse(DoubleSupplier multiplier) { + reverseController.reverse(multiplier); + } + + public void reverse() { + reverseController.reverse(); + } + + public Command reverseCmd(double multiplier) { + return reverseController.reverseCmd(multiplier); + } + + public Command reverseCmd(DoubleSupplier multiplier) { + return reverseController.reverseCmd(multiplier); + } + + public Command reverseCmd() { + return reverseController.reverseCmd(); + } + + public Command conditionalReverseCmd(BooleanSupplier condition, double multiplier) { + return reverseController.conditionalReverseCmd(condition, multiplier); + } + + public Command conditionalReverseCmd(BooleanSupplier condition, DoubleSupplier multiplier) { + return reverseController.conditionalReverseCmd(condition, multiplier); + } + + public Command conditionalReverseCmd(BooleanSupplier condition) { + return reverseController.conditionalReverseCmd(condition); + } + + public boolean atReverseLimit() { + return reverseController.atReverseLimit(); + } + + public boolean canMoveReverse() { + return reverseController.canMoveReverse(); + } + + public DigitalInput getForwardLimit() { + return forwardController.getLimit(); + } + + public DigitalInput getReverseLimit() { + return reverseController.getLimit(); + } + + public DoublePreference getForwardSpeed() { + return forwardController.getForwardSpeed(); + } + + public DoublePreference getReverseSpeed() { + return reverseController.getReverseSpeed(); + } + + public Subsystem getSubsystem() { + return forwardController.getSubsystem(); + } + + @Override + public void setNeutralMode(NeutralMode mode) { + controller.setNeutralMode(mode); + } + + @Override + public void resetMotorConfig() { + controller.resetMotorConfig(); + } + + @Override + public void initSendable(SendableBuilder builder) { + controller.initSendable(builder); + } + + @Override + public void set(double speed) { + controller.set(speed); + } + + @Override + public double get() { + return controller.get(); + } + + @Override + public void setInverted(boolean isInverted) { + controller.setInverted(isInverted); + } + + @Override + public boolean getInverted() { + return controller.getInverted(); + } + + @Override + public void disable() { + controller.disable(); + } + + @Override + public void stopMotor() { + controller.stopMotor(); + } + + @Override + public T getController() { + return forwardController.getController(); + } +} diff --git a/src/main/java/org/hyperonline/hyperlib/controller/meta/limits/ForwardLimitedController.java b/src/main/java/org/hyperonline/hyperlib/controller/meta/limits/ForwardLimitedController.java new file mode 100644 index 0000000..3bbfb00 --- /dev/null +++ b/src/main/java/org/hyperonline/hyperlib/controller/meta/limits/ForwardLimitedController.java @@ -0,0 +1,271 @@ +package org.hyperonline.hyperlib.controller.meta.limits; + +import com.ctre.phoenix.motorcontrol.NeutralMode; +import edu.wpi.first.util.sendable.SendableBuilder; +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.FunctionalCommand; +import edu.wpi.first.wpilibj2.command.RunCommand; +import edu.wpi.first.wpilibj2.command.Subsystem; +import org.hyperonline.hyperlib.controller.meta.MetaController; +import org.hyperonline.hyperlib.controller.SendableMotorController; +import org.hyperonline.hyperlib.pref.DoublePreference; + +import java.util.function.BooleanSupplier; +import java.util.function.DoubleSupplier; + +/** + * Class that limits a {@link SendableMotorController} to only run until a forward limit is reached. + * Useful for running a subsystem until a limit switch is tripped. + * + * @author Dheeraj Prakash + */ +public class ForwardLimitedController implements MetaController { + + /** + * The controller being used + */ + private final T controller; + + /** + * The limit switch to stop at + */ + private final DigitalInput limit; + + /** + * The speed preference for the controller to be set to + */ + private final DoublePreference forwardSpeed; + + /** + * The subsystem the controller is being attached to + */ + private Subsystem subsystem; + + + /** + * Construct a new {@link ForwardLimitedController} from the given information. + * @param controller the controller to limit + * @param limit the forward limit switch + * @param forwardSpeed the speed preference to move at + */ + public ForwardLimitedController(T controller, DigitalInput limit, DoublePreference forwardSpeed) { + this.controller = controller; + this.limit = limit; + this.forwardSpeed = forwardSpeed; + } + + public void setSubsystem(Subsystem subsystem) { + if (this.subsystem != null) throw new UnsupportedOperationException("Cannot replace already set subsystem."); + this.subsystem = subsystem; + } + + /** + * Manually move at a given speed. + * @param speed the speed to move at + */ + public void move(double speed) { + if (this.canMoveForward()) controller.set(speed); + } + + /** + * Manually move at a given speed + * @param speed function returning the speed to move at + */ + public void move(DoubleSupplier speed) { + this.move(speed.getAsDouble()); + } + + /** + * Stop the motor. + * Sets the power to 0. + */ + public void stop() { + this.move(0); + } + + /** + * Move forward at a speed proportional to the preference + * @param multiplier speed multiplier for the preference + */ + public void forward(double multiplier) { + this.move(forwardSpeed.get() * multiplier); + } + + /** + * Move forward at a speed proportional to the preference + * @param multiplier function returning speed multiplier for the preference + */ + public void forward(DoubleSupplier multiplier) { + this.forward(multiplier.getAsDouble()); + } + + /** + * Move forward at the speed preference + */ + public void forward() { + this.forward(1); + } + + /** + * Command wrapping {@link #forward(double)}. + * Will end when limit is reached. + * @param multiplier speed multiplier for the preference + * @return Command + */ + public Command forwardCmd(double multiplier) { + return new RunCommand(() -> this.forward(multiplier), subsystem).until(this::atForwardLimit); + } + + /** + * Command wrapping {@link #forward(DoubleSupplier)}. + * Will end when limit is reached. + * @param multiplier function teturning speed multiplier for the preference + * @return Command + */ + public Command forwardCmd(DoubleSupplier multiplier) { + return this.forwardCmd(multiplier.getAsDouble()); + } + + /** + * Command wrapping {@link #forward()}. + * Will end when limit is reached. + * @return Command + */ + public Command forwardCmd() { + return this.forwardCmd(1); + } + + /** + * Only move forward when the given predicate returns true. + * @param condition should move forward? + * @param multiplier speed multiplier for the preference + * @return Command + */ + public Command conditionalForwardCmd(BooleanSupplier condition, double multiplier) { + return new FunctionalCommand( + () -> {}, + () -> { + if (condition.getAsBoolean()) this.forward(multiplier); + else this.stop(); + }, + (interrupted) -> this.stop(), + this::atForwardLimit, + subsystem + ); + } + + /** + * Only move forward when the given predicate returns true. + * @param condition should move forward? + * @param multiplier function returning speed multiplier for the preference + * @return Command + */ + public Command conditionalForwardCmd(BooleanSupplier condition, DoubleSupplier multiplier) { + return this.conditionalForwardCmd(condition, multiplier.getAsDouble()); + } + + /** + * Only move forward when the given predicate returns true. + * @param condition should move forward? + * @return Command + */ + public Command conditionalForwardCmd(BooleanSupplier condition) { + return this.conditionalForwardCmd(condition, 1); + } + + /** + * Has the limit been reached? + * @return true if the limit has been reached + */ + public boolean atForwardLimit() { + return !limit.get(); + } + + /** + * Can we move forward? + * Inversion of {@link #atForwardLimit()} + * @return true if we can move forward + */ + public boolean canMoveForward() { + return !this.atForwardLimit(); + } + + /** + * Get limit source. + * @return DigitalInput of the limit switch + */ + public DigitalInput getLimit() { + return limit; + } + + /** + * Get the forward speed preference + * @return the preference + */ + public DoublePreference getForwardSpeed() { + return forwardSpeed; + } + + /** + * Get the subsystem the controller belongs to. + * @return Subsystem + */ + public Subsystem getSubsystem() { + return subsystem; + } + + @Override + public void setNeutralMode(NeutralMode mode) { + controller.setNeutralMode(mode); + } + + @Override + public void resetMotorConfig() { + controller.resetMotorConfig(); + } + + @Override + public void initSendable(SendableBuilder builder) { + controller.initSendable(builder); + } + + @Override + public void set(double speed) { + controller.set(speed); + } + + @Override + public double get() { + return controller.get(); + } + + @Override + public void setInverted(boolean isInverted) { + controller.setInverted(isInverted); + } + + @Override + public boolean getInverted() { + return controller.getInverted(); + } + + @Override + public void disable() { + controller.disable(); + } + + @Override + public void stopMotor() { + controller.stopMotor(); + } + + /** + * Get motor controller being used. + * @return the controller + */ + @Override + public T getController() { + return controller; + } +} diff --git a/src/main/java/org/hyperonline/hyperlib/controller/meta/limits/ReverseLimitedController.java b/src/main/java/org/hyperonline/hyperlib/controller/meta/limits/ReverseLimitedController.java new file mode 100644 index 0000000..8260636 --- /dev/null +++ b/src/main/java/org/hyperonline/hyperlib/controller/meta/limits/ReverseLimitedController.java @@ -0,0 +1,168 @@ +package org.hyperonline.hyperlib.controller.meta.limits; + +import com.ctre.phoenix.motorcontrol.NeutralMode; +import edu.wpi.first.util.sendable.SendableBuilder; +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.FunctionalCommand; +import edu.wpi.first.wpilibj2.command.RunCommand; +import edu.wpi.first.wpilibj2.command.Subsystem; +import org.hyperonline.hyperlib.controller.meta.MetaController; +import org.hyperonline.hyperlib.controller.SendableMotorController; +import org.hyperonline.hyperlib.pref.DoublePreference; + +import java.util.function.BooleanSupplier; +import java.util.function.DoubleSupplier; + +/** + * Class that limits a {@link SendableMotorController} to only run until a reverse limit is reached. + * Useful for running a subsystem until a limit switch is tripped. + * + * @author Dheeraj Prakash + * + * TODO: write docs for methods (copy from ForwardLimitedController) + */ +public class ReverseLimitedController implements MetaController { + private final T controller; + private final DigitalInput limit; + private final DoublePreference reverseSpeed; + private Subsystem subsystem; + + public ReverseLimitedController(T controller, DigitalInput limit, DoublePreference forwardSpeed) { + this.controller = controller; + this.limit = limit; + this.reverseSpeed = forwardSpeed; + } + + public void setSubsystem(Subsystem subsystem) { + if (this.subsystem != null) throw new UnsupportedOperationException("Cannot replace already set subsystem."); + this.subsystem = subsystem; + } + + public void move(double speed) { + if (this.canMoveReverse()) controller.set(speed); + } + + public void move(DoubleSupplier speed) { + this.move(speed.getAsDouble()); + } + + public void stop() { + this.move(0); + } + + public void reverse(double multiplier) { + this.move(reverseSpeed.get() * multiplier); + } + + public void reverse(DoubleSupplier multiplier) { + this.reverse(multiplier.getAsDouble()); + } + + public void reverse() { + this.reverse(1); + } + + public Command reverseCmd(double multiplier) { + return new RunCommand(() -> this.reverse(multiplier), subsystem).until(this::atReverseLimit); + } + + public Command reverseCmd(DoubleSupplier multiplier) { + return this.reverseCmd(multiplier.getAsDouble()); + } + + public Command reverseCmd() { + return this.reverseCmd(1); + } + + public Command conditionalReverseCmd(BooleanSupplier condition, double multiplier) { + return new FunctionalCommand( + () -> {}, + () -> { + if (condition.getAsBoolean()) this.reverse(multiplier); + else this.stop(); + }, + (interrupted) -> this.stop(), + this::atReverseLimit, + subsystem + ); + } + + public Command conditionalReverseCmd(BooleanSupplier condition, DoubleSupplier multiplier) { + return this.conditionalReverseCmd(condition, multiplier.getAsDouble()); + } + + public Command conditionalReverseCmd(BooleanSupplier condition) { + return this.conditionalReverseCmd(condition, 1); + } + + public boolean atReverseLimit() { + return !limit.get(); + } + + public boolean canMoveReverse() { + return !this.atReverseLimit(); + } + + public DigitalInput getLimit() { + return limit; + } + + public DoublePreference getReverseSpeed() { + return reverseSpeed; + } + + public Subsystem getSubsystem() { + return subsystem; + } + + @Override + public void setNeutralMode(NeutralMode mode) { + controller.setNeutralMode(mode); + } + + @Override + public void resetMotorConfig() { + controller.resetMotorConfig(); + } + + @Override + public void initSendable(SendableBuilder builder) { + controller.initSendable(builder); + } + + @Override + public void set(double speed) { + controller.set(speed); + } + + @Override + public double get() { + return controller.get(); + } + + @Override + public void setInverted(boolean isInverted) { + controller.setInverted(isInverted); + } + + @Override + public boolean getInverted() { + return controller.getInverted(); + } + + @Override + public void disable() { + controller.disable(); + } + + @Override + public void stopMotor() { + controller.stopMotor(); + } + + @Override + public T getController() { + return controller; + } +} diff --git a/src/main/java/org/hyperonline/hyperlib/subsystem/PrefControllerSlaveSubsystem.java b/src/main/java/org/hyperonline/hyperlib/subsystem/PrefControllerSlaveSubsystem.java deleted file mode 100644 index b37a2aa..0000000 --- a/src/main/java/org/hyperonline/hyperlib/subsystem/PrefControllerSlaveSubsystem.java +++ /dev/null @@ -1,86 +0,0 @@ -package org.hyperonline.hyperlib.subsystem; - -import com.ctre.phoenix.motorcontrol.NeutralMode; -import edu.wpi.first.util.sendable.Sendable; -import org.hyperonline.hyperlib.controller.SendableMotorController; - -import java.util.stream.Stream; - -/** - * @author Chris McGroarty - */ -public abstract class PrefControllerSlaveSubsystem< - MasterMotorType extends SendableMotorController, - SlaveMotorType extends SendableMotorController> - extends PreferenceControllerSubsystem { - protected SlaveMotorType m_slaveMotor; - protected boolean m_invertedSlave; - - /** - * - * @param masterMotor the motor to use in the subsystem - * @param slaveMotor the motor to follow the master motor in the subsystem - * @param inverted is the slaveMotor inverted compared to the masterMotor - */ - protected PrefControllerSlaveSubsystem( - MasterMotorType masterMotor, SlaveMotorType slaveMotor, boolean inverted) { - this(null, masterMotor, slaveMotor, inverted); - } - - /** - * {@inheritDoc} - */ - protected PrefControllerSlaveSubsystem(MasterMotorType masterMotor, SlaveMotorType slaveMotor) { - this(masterMotor, slaveMotor, false); - } - - - - /** - * @param name string to use as the Subsystem's name - * @param masterMotor the motor to use in the subsystem - * @param slaveMotor the motor to follow the master motor in the subsystem - * @param inverted is the slaveMotor inverted compared to the masterMotor - * - */ - protected PrefControllerSlaveSubsystem( - String name, MasterMotorType masterMotor, SlaveMotorType slaveMotor, boolean inverted) { - super(name, masterMotor); - m_slaveMotor = slaveMotor; - this.addChild("Slave Motor", m_slaveMotor); - m_invertedSlave = inverted; - } - - /** - * @deprecated SubsystemBase uses class.getSimpleName as default name - * @param name string to use as the Subsystem's name - * @param masterMotor the motor to use in the subsystem - * @param slaveMotor the motor to follow the master motor in the subsystem - */ - @Deprecated - protected PrefControllerSlaveSubsystem( - String name, MasterMotorType masterMotor, SlaveMotorType slaveMotor) { - this(masterMotor, slaveMotor); - } - - protected abstract void setFollowing(boolean inverted); - - @Override - public void setNeutralMode(NeutralMode mode) { - super.setNeutralMode(mode); - m_slaveMotor.setNeutralMode(mode); - } - - @Override - protected void configMotor() { - super.configMotor(); - m_slaveMotor.resetMotorConfig(); - this.setFollowing(m_invertedSlave); - } - - @Override - protected Stream getSendables() { - return Stream.concat( - super.getSendables(), Stream.of(m_slaveMotor)); - } -} diff --git a/src/main/java/org/hyperonline/hyperlib/subsystem/PrefControllerSlaveSubsystemPhoenix.java b/src/main/java/org/hyperonline/hyperlib/subsystem/PrefControllerSlaveSubsystemPhoenix.java deleted file mode 100644 index 8bb9015..0000000 --- a/src/main/java/org/hyperonline/hyperlib/subsystem/PrefControllerSlaveSubsystemPhoenix.java +++ /dev/null @@ -1,44 +0,0 @@ -package org.hyperonline.hyperlib.subsystem; - -import com.ctre.phoenix.motorcontrol.IMotorController; -import org.hyperonline.hyperlib.controller.SendableMotorController; -/** - * @author Chris McGroarty - */ -public abstract class PrefControllerSlaveSubsystemPhoenix< - MasterMotorType extends SendableMotorController & IMotorController, - SlaveMotorType extends SendableMotorController & IMotorController> - extends PrefControllerSlaveSubsystem { - /** {@inheritDoc} */ - protected PrefControllerSlaveSubsystemPhoenix( - MasterMotorType masterMotor, SlaveMotorType slaveMotor, boolean inverted) { - this( null, masterMotor, slaveMotor, inverted); - } - /** {@inheritDoc} */ - protected PrefControllerSlaveSubsystemPhoenix(MasterMotorType masterMotor, SlaveMotorType slaveMotor) { - super(masterMotor, slaveMotor); - } - - /** - * - * @param name - * @param masterMotor - * @param slaveMotor - * @param inverted - */ - protected PrefControllerSlaveSubsystemPhoenix( - String name, MasterMotorType masterMotor, SlaveMotorType slaveMotor, boolean inverted) { - super(name, masterMotor, slaveMotor, inverted); - } - - @Override - protected void setFollowing(boolean inverted) { - m_slaveMotor.follow(m_motor); - m_slaveMotor.setInverted(inverted); - } - - @Override - protected void configMotor() { - super.configMotor(); - } -} diff --git a/src/main/java/org/hyperonline/hyperlib/subsystem/PrefControllerSlaveSubsystemSparkMax.java b/src/main/java/org/hyperonline/hyperlib/subsystem/PrefControllerSlaveSubsystemSparkMax.java deleted file mode 100644 index ea1f538..0000000 --- a/src/main/java/org/hyperonline/hyperlib/subsystem/PrefControllerSlaveSubsystemSparkMax.java +++ /dev/null @@ -1,58 +0,0 @@ -package org.hyperonline.hyperlib.subsystem; - -import com.revrobotics.CANSparkMaxLowLevel; -import org.hyperonline.hyperlib.controller.HYPER_CANSparkMax; - -/** - * @author Chris McGroarty - */ -public abstract class PrefControllerSlaveSubsystemSparkMax - extends PrefControllerSlaveSubsystem { - /** - * {@inheritDoc} - */ - protected PrefControllerSlaveSubsystemSparkMax( - HYPER_CANSparkMax masterMotor, HYPER_CANSparkMax slaveMotor, boolean inverted) { - this(null, masterMotor, slaveMotor, inverted); - } - /** - * {@inheritDoc} - */ - protected PrefControllerSlaveSubsystemSparkMax( - HYPER_CANSparkMax masterMotor, HYPER_CANSparkMax slaveMotor) { - super(masterMotor, slaveMotor); - } - - /** - * {@inheritDoc} - */ - protected PrefControllerSlaveSubsystemSparkMax( - String name, HYPER_CANSparkMax masterMotor, HYPER_CANSparkMax slaveMotor, boolean inverted) { - super(name, masterMotor, slaveMotor, inverted); - } - /** - * @deprecated SubsystemBase uses class.getSimpleName as default name - * {@inheritDoc} - */ - @Deprecated - protected PrefControllerSlaveSubsystemSparkMax( - String name, HYPER_CANSparkMax masterMotor, HYPER_CANSparkMax slaveMotor) { - this(masterMotor, slaveMotor); - } - - @Override - protected void setFollowing(boolean inverted) { - m_slaveMotor.follow(m_motor, inverted); - } - - @Override - protected void configMotor() { - super.configMotor(); - - m_motor.setPeriodicFramePeriod(CANSparkMaxLowLevel.PeriodicFrame.kStatus1, 500); - m_motor.setPeriodicFramePeriod(CANSparkMaxLowLevel.PeriodicFrame.kStatus2, 500); - m_slaveMotor.setPeriodicFramePeriod(CANSparkMaxLowLevel.PeriodicFrame.kStatus0, 10); - m_slaveMotor.setPeriodicFramePeriod(CANSparkMaxLowLevel.PeriodicFrame.kStatus1, 500); - m_slaveMotor.setPeriodicFramePeriod(CANSparkMaxLowLevel.PeriodicFrame.kStatus2, 500); - } -} diff --git a/src/main/java/org/hyperonline/hyperlib/subsystem/PreferenceControllerSubsystem.java b/src/main/java/org/hyperonline/hyperlib/subsystem/PreferenceControllerSubsystem.java index 8c75f94..886d331 100644 --- a/src/main/java/org/hyperonline/hyperlib/subsystem/PreferenceControllerSubsystem.java +++ b/src/main/java/org/hyperonline/hyperlib/subsystem/PreferenceControllerSubsystem.java @@ -275,44 +275,4 @@ protected void configSensors() { @Override protected void configPID() { } - - public boolean canMoveForward(DoubleSupplier speed) { - return speed.getAsDouble() >= 0 && canMoveForward(); - } - - public boolean canMoveReverse(DoubleSupplier speed) { - return speed.getAsDouble() <= 0 && canMoveReverse(); - } - - - public boolean canMove(DoubleSupplier speed) { - if (speed.getAsDouble() > 0) { - return canMoveForward(); - } - - if (speed.getAsDouble() < 0) { - return canMoveReverse(); - } - - return true; - - } - - @Override - public Command resetPositionSensorAtReverseLimitCmd() { - return new RunCommand(() -> { - if (isAtReverseLimit() && m_motor.get() <= 0) { - this.resetPositionSensor(); - } - }); - } - - @Override - public Command resetPositionSensorAtForwardLimitCmd() { - return new RunCommand(() -> { - if (isAtForwardLimit() && m_motor.get() >= 0) { - this.resetPositionSensor(); - } - }); - } } diff --git a/src/main/java/org/hyperonline/hyperlib/subsystem/PreferenceMotorSubsystem.java b/src/main/java/org/hyperonline/hyperlib/subsystem/PreferenceMotorSubsystem.java index 8784cda..561f9ad 100644 --- a/src/main/java/org/hyperonline/hyperlib/subsystem/PreferenceMotorSubsystem.java +++ b/src/main/java/org/hyperonline/hyperlib/subsystem/PreferenceMotorSubsystem.java @@ -129,41 +129,4 @@ public void resetPositionSensor() { public Command resetPositionSensorCmd() { return new InstantCommand(this::resetPositionSensor); } - - public abstract boolean isAtForwardLimit(); - - - public boolean canMoveForward() { - return !isAtForwardLimit(); - } - - public boolean canMoveReverse() { - return !isAtReverseLimit(); - } - - public abstract boolean isAtReverseLimit(); - - public Command resetPositionSensorAtReverseLimitCmd() { - return new RunCommand(() -> { - if (isAtReverseLimit()) { - this.resetPositionSensor(); - } - }); - } - - public Command resetPositionSensorAtForwardLimitCmd() { - return new RunCommand(() -> { - if (isAtForwardLimit()) { - this.resetPositionSensor(); - } - }); - } - - public Command moveAllWayReverseCmd() { - return this.reverseCmd().until(this::isAtReverseLimit); - } - - public Command moveAllWayForwardCmd() { - return this.forwardCmd().until(this::isAtForwardLimit); - } }