From 1cd6746bfbb65172ef2d82488085182c2d6a13e9 Mon Sep 17 00:00:00 2001
From: Dheeraj Prakash <dheerajpv16@gmail.com>
Date: Tue, 28 Mar 2023 22:59:27 -0400
Subject: [PATCH 01/13] add directional limit class (and combined wrapper)

---
 .../limits/ForwardLimitedController.java      | 219 ++++++++++++++++++
 .../controller/limits/LimitedController.java  | 163 +++++++++++++
 .../limits/ReverseLimitedController.java      | 115 +++++++++
 3 files changed, 497 insertions(+)
 create mode 100644 src/main/java/org/hyperonline/hyperlib/controller/limits/ForwardLimitedController.java
 create mode 100644 src/main/java/org/hyperonline/hyperlib/controller/limits/LimitedController.java
 create mode 100644 src/main/java/org/hyperonline/hyperlib/controller/limits/ReverseLimitedController.java

diff --git a/src/main/java/org/hyperonline/hyperlib/controller/limits/ForwardLimitedController.java b/src/main/java/org/hyperonline/hyperlib/controller/limits/ForwardLimitedController.java
new file mode 100644
index 0000000..b56a7bd
--- /dev/null
+++ b/src/main/java/org/hyperonline/hyperlib/controller/limits/ForwardLimitedController.java
@@ -0,0 +1,219 @@
+package org.hyperonline.hyperlib.controller.limits;
+
+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.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 {
+
+    /**
+     * The controller being used
+     */
+    private final SendableMotorController 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 final 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
+     * @param subsystem the subsystem the controller belongs to (to set requirement for commands)
+     */
+    public ForwardLimitedController(SendableMotorController controller, DigitalInput limit, DoublePreference forwardSpeed, Subsystem subsystem) {
+        this.controller = controller;
+        this.limit = limit;
+        this.forwardSpeed = forwardSpeed;
+        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 motor controller being used.
+     * @return the controller
+     */
+    public SendableMotorController getController() {
+        return controller;
+    }
+
+    /**
+     * 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;
+    }
+}
diff --git a/src/main/java/org/hyperonline/hyperlib/controller/limits/LimitedController.java b/src/main/java/org/hyperonline/hyperlib/controller/limits/LimitedController.java
new file mode 100644
index 0000000..e5c8414
--- /dev/null
+++ b/src/main/java/org/hyperonline/hyperlib/controller/limits/LimitedController.java
@@ -0,0 +1,163 @@
+package org.hyperonline.hyperlib.controller.limits;
+
+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.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 LimitedController {
+    private final ForwardLimitedController forwardController;
+    private final ReverseLimitedController reverseController;
+
+    public LimitedController(SendableMotorController controller, DigitalInput forwardLimit, DigitalInput reverseLimit, DoublePreference forwardSpeed, DoublePreference reverseSpeed, Subsystem subsystem) {
+
+        this.forwardController = new ForwardLimitedController(controller, forwardLimit, forwardSpeed, subsystem);
+        this.reverseController = new ReverseLimitedController(controller, reverseLimit, reverseSpeed, 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());
+    }
+
+    // delegate methods to controller classes
+
+    public void move(double speed) {
+        forwardController.move(speed);
+    }
+
+    public void move(DoubleSupplier speed) {
+        forwardController.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 SendableMotorController getController() {
+        return forwardController.getController();
+    }
+
+    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();
+    }
+}
diff --git a/src/main/java/org/hyperonline/hyperlib/controller/limits/ReverseLimitedController.java b/src/main/java/org/hyperonline/hyperlib/controller/limits/ReverseLimitedController.java
new file mode 100644
index 0000000..1c3922b
--- /dev/null
+++ b/src/main/java/org/hyperonline/hyperlib/controller/limits/ReverseLimitedController.java
@@ -0,0 +1,115 @@
+package org.hyperonline.hyperlib.controller.limits;
+
+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.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 {
+    private final SendableMotorController controller;
+    private final DigitalInput limit;
+    private final DoublePreference reverseSpeed;
+    private final Subsystem subsystem;
+
+    public ReverseLimitedController(SendableMotorController controller, DigitalInput limit, DoublePreference forwardSpeed, Subsystem subsystem) {
+        this.controller = controller;
+        this.limit = limit;
+        this.reverseSpeed = forwardSpeed;
+        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 SendableMotorController getController() {
+        return controller;
+    }
+
+    public DigitalInput getLimit() {
+        return limit;
+    }
+
+    public DoublePreference getReverseSpeed() {
+        return reverseSpeed;
+    }
+
+    public Subsystem getSubsystem() {
+        return subsystem;
+    }
+}

From bfee6485272afdcaaf5d43f0e29cb99e83999cd0 Mon Sep 17 00:00:00 2001
From: Dheeraj Prakash <dheerajpv16@gmail.com>
Date: Tue, 28 Mar 2023 23:05:08 -0400
Subject: [PATCH 02/13] remove limits from PreferenceMotorSubsystem

---
 .../subsystem/PreferenceMotorSubsystem.java   | 37 -------------------
 1 file changed, 37 deletions(-)

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);
-    }
 }

From 34e0794bd1aa363bff048d3fcf2dbccb6c2cd8e4 Mon Sep 17 00:00:00 2001
From: Dheeraj Prakash <dheerajpv16@gmail.com>
Date: Wed, 29 Mar 2023 21:40:33 -0400
Subject: [PATCH 03/13] implement SendableMotorController for limited
 controller classes

---
 ...roller.java => DualLimitedController.java} | 54 +++++++++++++++++--
 .../limits/ForwardLimitedController.java      | 49 ++++++++++++++++-
 .../limits/ReverseLimitedController.java      | 49 ++++++++++++++++-
 3 files changed, 147 insertions(+), 5 deletions(-)
 rename src/main/java/org/hyperonline/hyperlib/controller/limits/{LimitedController.java => DualLimitedController.java} (77%)

diff --git a/src/main/java/org/hyperonline/hyperlib/controller/limits/LimitedController.java b/src/main/java/org/hyperonline/hyperlib/controller/limits/DualLimitedController.java
similarity index 77%
rename from src/main/java/org/hyperonline/hyperlib/controller/limits/LimitedController.java
rename to src/main/java/org/hyperonline/hyperlib/controller/limits/DualLimitedController.java
index e5c8414..09b977e 100644
--- a/src/main/java/org/hyperonline/hyperlib/controller/limits/LimitedController.java
+++ b/src/main/java/org/hyperonline/hyperlib/controller/limits/DualLimitedController.java
@@ -1,5 +1,7 @@
 package org.hyperonline.hyperlib.controller.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;
@@ -15,12 +17,13 @@
  *
  * @author Dheeraj Prakash
  */
-public class LimitedController {
+public class DualLimitedController implements SendableMotorController {
+    private final SendableMotorController controller;
     private final ForwardLimitedController forwardController;
     private final ReverseLimitedController reverseController;
 
-    public LimitedController(SendableMotorController controller, DigitalInput forwardLimit, DigitalInput reverseLimit, DoublePreference forwardSpeed, DoublePreference reverseSpeed, Subsystem subsystem) {
-
+    public DualLimitedController(SendableMotorController controller, DigitalInput forwardLimit, DigitalInput reverseLimit, DoublePreference forwardSpeed, DoublePreference reverseSpeed, Subsystem subsystem) {
+        this.controller = controller;
         this.forwardController = new ForwardLimitedController(controller, forwardLimit, forwardSpeed, subsystem);
         this.reverseController = new ReverseLimitedController(controller, reverseLimit, reverseSpeed, subsystem);
     }
@@ -160,4 +163,49 @@ public DoublePreference 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();
+    }
 }
diff --git a/src/main/java/org/hyperonline/hyperlib/controller/limits/ForwardLimitedController.java b/src/main/java/org/hyperonline/hyperlib/controller/limits/ForwardLimitedController.java
index b56a7bd..415eba2 100644
--- a/src/main/java/org/hyperonline/hyperlib/controller/limits/ForwardLimitedController.java
+++ b/src/main/java/org/hyperonline/hyperlib/controller/limits/ForwardLimitedController.java
@@ -1,5 +1,7 @@
 package org.hyperonline.hyperlib.controller.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;
@@ -17,7 +19,7 @@
  *
  * @author Dheeraj Prakash
  */
-public class ForwardLimitedController {
+public class ForwardLimitedController implements SendableMotorController {
 
     /**
      * The controller being used
@@ -216,4 +218,49 @@ public DoublePreference getForwardSpeed() {
     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();
+    }
 }
diff --git a/src/main/java/org/hyperonline/hyperlib/controller/limits/ReverseLimitedController.java b/src/main/java/org/hyperonline/hyperlib/controller/limits/ReverseLimitedController.java
index 1c3922b..3c04fa6 100644
--- a/src/main/java/org/hyperonline/hyperlib/controller/limits/ReverseLimitedController.java
+++ b/src/main/java/org/hyperonline/hyperlib/controller/limits/ReverseLimitedController.java
@@ -1,5 +1,7 @@
 package org.hyperonline.hyperlib.controller.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;
@@ -19,7 +21,7 @@
  *
  * TODO: write docs for methods (copy from ForwardLimitedController)
  */
-public class ReverseLimitedController {
+public class ReverseLimitedController implements SendableMotorController {
     private final SendableMotorController controller;
     private final DigitalInput limit;
     private final DoublePreference reverseSpeed;
@@ -112,4 +114,49 @@ public DoublePreference getReverseSpeed() {
     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();
+    }
 }

From 517c8fe1c4ba8d985653b541ecbb80b916793268 Mon Sep 17 00:00:00 2001
From: Dheeraj Prakash <dheerajpv16@gmail.com>
Date: Wed, 29 Mar 2023 22:01:09 -0400
Subject: [PATCH 04/13] remove all directional limit code from subsystem

---
 .../PreferenceControllerSubsystem.java        | 40 -------------------
 1 file changed, 40 deletions(-)

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();
-            }
-        });
-    }
 }

From 5aa58a272156ab6f4e9f0337ac4a28482b0efb84 Mon Sep 17 00:00:00 2001
From: Dheeraj Prakash <dheerajpv16@gmail.com>
Date: Wed, 29 Mar 2023 22:39:48 -0400
Subject: [PATCH 05/13] make master/slave controller groups

---
 .../controller/groups/CANSparkMaxGroup.java   | 17 ++++
 .../controller/groups/ControllerGroup.java    | 82 +++++++++++++++++++
 .../groups/PhoenixControllerGroup.java        | 24 ++++++
 3 files changed, 123 insertions(+)
 create mode 100644 src/main/java/org/hyperonline/hyperlib/controller/groups/CANSparkMaxGroup.java
 create mode 100644 src/main/java/org/hyperonline/hyperlib/controller/groups/ControllerGroup.java
 create mode 100644 src/main/java/org/hyperonline/hyperlib/controller/groups/PhoenixControllerGroup.java

diff --git a/src/main/java/org/hyperonline/hyperlib/controller/groups/CANSparkMaxGroup.java b/src/main/java/org/hyperonline/hyperlib/controller/groups/CANSparkMaxGroup.java
new file mode 100644
index 0000000..f66f67e
--- /dev/null
+++ b/src/main/java/org/hyperonline/hyperlib/controller/groups/CANSparkMaxGroup.java
@@ -0,0 +1,17 @@
+package org.hyperonline.hyperlib.controller.groups;
+
+import org.hyperonline.hyperlib.controller.HYPER_CANSparkMax;
+import org.hyperonline.hyperlib.controller.SendableMotorController;
+
+/**
+ * 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<HYPER_CANSparkMax, HYPER_CANSparkMax> implements SendableMotorController {
+    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/groups/ControllerGroup.java b/src/main/java/org/hyperonline/hyperlib/controller/groups/ControllerGroup.java
new file mode 100644
index 0000000..b39bde6
--- /dev/null
+++ b/src/main/java/org/hyperonline/hyperlib/controller/groups/ControllerGroup.java
@@ -0,0 +1,82 @@
+package org.hyperonline.hyperlib.controller.groups;
+
+import com.ctre.phoenix.motorcontrol.NeutralMode;
+import edu.wpi.first.util.sendable.SendableBuilder;
+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 <M> master motor type
+ * @param <S> slave motor type
+ *
+ * @author Dheeraj Prakash
+ */
+abstract class ControllerGroup<M extends SendableMotorController, S extends SendableMotorController> implements SendableMotorController {
+    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 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/groups/PhoenixControllerGroup.java b/src/main/java/org/hyperonline/hyperlib/controller/groups/PhoenixControllerGroup.java
new file mode 100644
index 0000000..0f6ead5
--- /dev/null
+++ b/src/main/java/org/hyperonline/hyperlib/controller/groups/PhoenixControllerGroup.java
@@ -0,0 +1,24 @@
+package org.hyperonline.hyperlib.controller.groups;
+
+import com.ctre.phoenix.motorcontrol.IMotorController;
+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 <M> type of master controller used
+ * @param <S> type of slave controller used
+ *
+ * @author Dheeraj Prakash
+ */
+public class PhoenixControllerGroup<
+        M extends SendableMotorController & IMotorController,
+        S extends SendableMotorController & IMotorController
+        > extends ControllerGroup<M, S> implements SendableMotorController {
+    public PhoenixControllerGroup(M master, S slave, boolean invertSlave) {
+        super(master, slave);
+        slave.follow(master);
+        slave.setInverted(invertSlave);
+    }
+}

From 30ce7d7eb9997d55a0732898b6894009cbfbc1cc Mon Sep 17 00:00:00 2001
From: Dheeraj Prakash <dheerajpv16@gmail.com>
Date: Wed, 29 Mar 2023 22:45:47 -0400
Subject: [PATCH 06/13] fix DualLimitedController possibly not obeying reverse
 limit under manual move

---
 .../controller/limits/DualLimitedController.java      | 11 ++++++-----
 1 file changed, 6 insertions(+), 5 deletions(-)

diff --git a/src/main/java/org/hyperonline/hyperlib/controller/limits/DualLimitedController.java b/src/main/java/org/hyperonline/hyperlib/controller/limits/DualLimitedController.java
index 09b977e..c0db6e3 100644
--- a/src/main/java/org/hyperonline/hyperlib/controller/limits/DualLimitedController.java
+++ b/src/main/java/org/hyperonline/hyperlib/controller/limits/DualLimitedController.java
@@ -38,14 +38,15 @@ public boolean canMove(DoubleSupplier speed) {
         return this.canMove(speed.getAsDouble());
     }
 
-    // delegate methods to controller classes
-
-    public void move(double speed) {
+    public void move(DoubleSupplier speed) {
         forwardController.move(speed);
     }
 
-    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() {

From 2d6cbd239f03bcd2abb8aed0d348704e88258dba Mon Sep 17 00:00:00 2001
From: Dheeraj Prakash <dheerajpv16@gmail.com>
Date: Thu, 30 Mar 2023 21:28:10 -0400
Subject: [PATCH 07/13] remove specific slave subsystem classes the same
 functionality can be used with controller groups.

---
 .../PrefControllerSlaveSubsystem.java         | 86 -------------------
 .../PrefControllerSlaveSubsystemPhoenix.java  | 44 ----------
 .../PrefControllerSlaveSubsystemSparkMax.java | 58 -------------
 3 files changed, 188 deletions(-)
 delete mode 100644 src/main/java/org/hyperonline/hyperlib/subsystem/PrefControllerSlaveSubsystem.java
 delete mode 100644 src/main/java/org/hyperonline/hyperlib/subsystem/PrefControllerSlaveSubsystemPhoenix.java
 delete mode 100644 src/main/java/org/hyperonline/hyperlib/subsystem/PrefControllerSlaveSubsystemSparkMax.java

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<MasterMotorType> {
-  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<Sendable> 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<MasterMotorType, SlaveMotorType> {
-  /** {@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<HYPER_CANSparkMax, HYPER_CANSparkMax> {
-  /**
-   * {@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);
-  }
-}

From 8e1a9a4804be75c5d1b882f55f5f88afdf57b274 Mon Sep 17 00:00:00 2001
From: Dheeraj Prakash <dheerajpv16@gmail.com>
Date: Sat, 1 Apr 2023 14:16:57 -0400
Subject: [PATCH 08/13] remove subsystem parameter - can't use this in super()

---
 .../controller/limits/DualLimitedController.java         | 9 +++++++--
 .../controller/limits/ForwardLimitedController.java      | 9 ++++++---
 .../controller/limits/ReverseLimitedController.java      | 8 ++++++--
 3 files changed, 19 insertions(+), 7 deletions(-)

diff --git a/src/main/java/org/hyperonline/hyperlib/controller/limits/DualLimitedController.java b/src/main/java/org/hyperonline/hyperlib/controller/limits/DualLimitedController.java
index c0db6e3..0296a69 100644
--- a/src/main/java/org/hyperonline/hyperlib/controller/limits/DualLimitedController.java
+++ b/src/main/java/org/hyperonline/hyperlib/controller/limits/DualLimitedController.java
@@ -24,8 +24,13 @@ public class DualLimitedController implements SendableMotorController {
 
     public DualLimitedController(SendableMotorController controller, DigitalInput forwardLimit, DigitalInput reverseLimit, DoublePreference forwardSpeed, DoublePreference reverseSpeed, Subsystem subsystem) {
         this.controller = controller;
-        this.forwardController = new ForwardLimitedController(controller, forwardLimit, forwardSpeed, subsystem);
-        this.reverseController = new ReverseLimitedController(controller, reverseLimit, reverseSpeed, subsystem);
+        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) {
diff --git a/src/main/java/org/hyperonline/hyperlib/controller/limits/ForwardLimitedController.java b/src/main/java/org/hyperonline/hyperlib/controller/limits/ForwardLimitedController.java
index 415eba2..a26e2af 100644
--- a/src/main/java/org/hyperonline/hyperlib/controller/limits/ForwardLimitedController.java
+++ b/src/main/java/org/hyperonline/hyperlib/controller/limits/ForwardLimitedController.java
@@ -39,7 +39,7 @@ public class ForwardLimitedController implements SendableMotorController {
     /**
      * The subsystem the controller is being attached to
      */
-    private final Subsystem subsystem;
+    private Subsystem subsystem;
 
 
     /**
@@ -47,12 +47,15 @@ public class ForwardLimitedController implements SendableMotorController {
      * @param controller the controller to limit
      * @param limit the forward limit switch
      * @param forwardSpeed the speed preference to move at
-     * @param subsystem the subsystem the controller belongs to (to set requirement for commands)
      */
-    public ForwardLimitedController(SendableMotorController controller, DigitalInput limit, DoublePreference forwardSpeed, Subsystem subsystem) {
+    public ForwardLimitedController(SendableMotorController 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;
     }
 
diff --git a/src/main/java/org/hyperonline/hyperlib/controller/limits/ReverseLimitedController.java b/src/main/java/org/hyperonline/hyperlib/controller/limits/ReverseLimitedController.java
index 3c04fa6..079c14a 100644
--- a/src/main/java/org/hyperonline/hyperlib/controller/limits/ReverseLimitedController.java
+++ b/src/main/java/org/hyperonline/hyperlib/controller/limits/ReverseLimitedController.java
@@ -25,12 +25,16 @@ public class ReverseLimitedController implements SendableMotorController {
     private final SendableMotorController controller;
     private final DigitalInput limit;
     private final DoublePreference reverseSpeed;
-    private final Subsystem subsystem;
+    private Subsystem subsystem;
 
-    public ReverseLimitedController(SendableMotorController controller, DigitalInput limit, DoublePreference forwardSpeed, Subsystem subsystem) {
+    public ReverseLimitedController(SendableMotorController 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;
     }
 

From e0c5cf750c0ebd1a2890f0b70f2c79bd5eaceaa5 Mon Sep 17 00:00:00 2001
From: Dheeraj Prakash <dheerajpv16@gmail.com>
Date: Sat, 1 Apr 2023 15:22:51 -0400
Subject: [PATCH 09/13] genericize controllers to preserve raw HYPER_* class

---
 .../controller/limits/DualLimitedController.java | 16 ++++++++--------
 .../limits/ForwardLimitedController.java         |  8 ++++----
 .../limits/ReverseLimitedController.java         |  8 ++++----
 3 files changed, 16 insertions(+), 16 deletions(-)

diff --git a/src/main/java/org/hyperonline/hyperlib/controller/limits/DualLimitedController.java b/src/main/java/org/hyperonline/hyperlib/controller/limits/DualLimitedController.java
index 0296a69..1a2047d 100644
--- a/src/main/java/org/hyperonline/hyperlib/controller/limits/DualLimitedController.java
+++ b/src/main/java/org/hyperonline/hyperlib/controller/limits/DualLimitedController.java
@@ -17,15 +17,15 @@
  *
  * @author Dheeraj Prakash
  */
-public class DualLimitedController implements SendableMotorController {
-    private final SendableMotorController controller;
-    private final ForwardLimitedController forwardController;
-    private final ReverseLimitedController reverseController;
+public class DualLimitedController<T extends SendableMotorController> implements SendableMotorController {
+    private final T controller;
+    private final ForwardLimitedController<T> forwardController;
+    private final ReverseLimitedController<T> reverseController;
 
-    public DualLimitedController(SendableMotorController controller, DigitalInput forwardLimit, DigitalInput reverseLimit, DoublePreference forwardSpeed, DoublePreference reverseSpeed, Subsystem subsystem) {
+    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);
+        this.forwardController = new ForwardLimitedController<>(controller, forwardLimit, forwardSpeed);
+        this.reverseController = new ReverseLimitedController<>(controller, reverseLimit, reverseSpeed);
     }
 
     public void setSubsystem(Subsystem subsystem) {
@@ -146,7 +146,7 @@ public boolean canMoveReverse() {
         return reverseController.canMoveReverse();
     }
 
-    public SendableMotorController getController() {
+    public T getController() {
         return forwardController.getController();
     }
 
diff --git a/src/main/java/org/hyperonline/hyperlib/controller/limits/ForwardLimitedController.java b/src/main/java/org/hyperonline/hyperlib/controller/limits/ForwardLimitedController.java
index a26e2af..91fdbd9 100644
--- a/src/main/java/org/hyperonline/hyperlib/controller/limits/ForwardLimitedController.java
+++ b/src/main/java/org/hyperonline/hyperlib/controller/limits/ForwardLimitedController.java
@@ -19,12 +19,12 @@
  *
  * @author Dheeraj Prakash
  */
-public class ForwardLimitedController implements SendableMotorController {
+public class ForwardLimitedController<T extends SendableMotorController> implements SendableMotorController {
 
     /**
      * The controller being used
      */
-    private final SendableMotorController controller;
+    private final T controller;
 
     /**
      * The limit switch to stop at
@@ -48,7 +48,7 @@ public class ForwardLimitedController implements SendableMotorController {
      * @param limit the forward limit switch
      * @param forwardSpeed the speed preference to move at
      */
-    public ForwardLimitedController(SendableMotorController controller, DigitalInput limit, DoublePreference forwardSpeed) {
+    public ForwardLimitedController(T controller, DigitalInput limit, DoublePreference forwardSpeed) {
         this.controller = controller;
         this.limit = limit;
         this.forwardSpeed = forwardSpeed;
@@ -194,7 +194,7 @@ public boolean canMoveForward() {
      * Get motor controller being used.
      * @return the controller
      */
-    public SendableMotorController getController() {
+    public T getController() {
         return controller;
     }
 
diff --git a/src/main/java/org/hyperonline/hyperlib/controller/limits/ReverseLimitedController.java b/src/main/java/org/hyperonline/hyperlib/controller/limits/ReverseLimitedController.java
index 079c14a..660f603 100644
--- a/src/main/java/org/hyperonline/hyperlib/controller/limits/ReverseLimitedController.java
+++ b/src/main/java/org/hyperonline/hyperlib/controller/limits/ReverseLimitedController.java
@@ -21,13 +21,13 @@
  *
  * TODO: write docs for methods (copy from ForwardLimitedController)
  */
-public class ReverseLimitedController implements SendableMotorController {
-    private final SendableMotorController controller;
+public class ReverseLimitedController<T extends SendableMotorController> implements SendableMotorController {
+    private final T controller;
     private final DigitalInput limit;
     private final DoublePreference reverseSpeed;
     private Subsystem subsystem;
 
-    public ReverseLimitedController(SendableMotorController controller, DigitalInput limit, DoublePreference forwardSpeed) {
+    public ReverseLimitedController(T controller, DigitalInput limit, DoublePreference forwardSpeed) {
         this.controller = controller;
         this.limit = limit;
         this.reverseSpeed = forwardSpeed;
@@ -103,7 +103,7 @@ public boolean canMoveReverse() {
         return !this.atReverseLimit();
     }
 
-    public SendableMotorController getController() {
+    public T getController() {
         return controller;
     }
 

From 60f8e6cb02147d6f77841408e0a39ca25e28379b Mon Sep 17 00:00:00 2001
From: Dheeraj Prakash <dheerajpv16@gmail.com>
Date: Sat, 1 Apr 2023 17:12:07 -0400
Subject: [PATCH 10/13] create MetaController and RawController interfaces

---
 .../controller/HYPER_CANSparkMax.java         |  5 +---
 .../controller/HYPER_PWMSparkMax.java         |  2 +-
 .../hyperlib/controller/HYPER_TalonSRX.java   |  2 +-
 .../hyperlib/controller/HYPER_VictorSPX.java  |  2 +-
 .../hyperlib/controller/MetaController.java   | 29 +++++++++++++++++++
 .../hyperlib/controller/RawController.java    |  7 +++++
 .../controller/groups/CANSparkMaxGroup.java   |  3 +-
 .../controller/groups/ControllerGroup.java    |  8 ++++-
 .../groups/PhoenixControllerGroup.java        |  3 +-
 .../limits/DualLimitedController.java         | 13 +++++----
 .../limits/ForwardLimitedController.java      | 20 +++++++------
 .../limits/ReverseLimitedController.java      | 12 ++++----
 12 files changed, 77 insertions(+), 29 deletions(-)
 create mode 100644 src/main/java/org/hyperonline/hyperlib/controller/MetaController.java
 create mode 100644 src/main/java/org/hyperonline/hyperlib/controller/RawController.java

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..d6d8fb4 100644
--- a/src/main/java/org/hyperonline/hyperlib/controller/HYPER_CANSparkMax.java
+++ b/src/main/java/org/hyperonline/hyperlib/controller/HYPER_CANSparkMax.java
@@ -24,10 +24,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..7b1aaad 100644
--- a/src/main/java/org/hyperonline/hyperlib/controller/HYPER_PWMSparkMax.java
+++ b/src/main/java/org/hyperonline/hyperlib/controller/HYPER_PWMSparkMax.java
@@ -3,7 +3,7 @@
 import com.ctre.phoenix.motorcontrol.NeutralMode;
 import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
 
-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..a4618a4 100644
--- a/src/main/java/org/hyperonline/hyperlib/controller/HYPER_TalonSRX.java
+++ b/src/main/java/org/hyperonline/hyperlib/controller/HYPER_TalonSRX.java
@@ -15,7 +15,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..37eac7c 100644
--- a/src/main/java/org/hyperonline/hyperlib/controller/HYPER_VictorSPX.java
+++ b/src/main/java/org/hyperonline/hyperlib/controller/HYPER_VictorSPX.java
@@ -14,7 +14,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/MetaController.java b/src/main/java/org/hyperonline/hyperlib/controller/MetaController.java
new file mode 100644
index 0000000..1d7053e
--- /dev/null
+++ b/src/main/java/org/hyperonline/hyperlib/controller/MetaController.java
@@ -0,0 +1,29 @@
+package org.hyperonline.hyperlib.controller;
+
+/**
+ * Interface that represents a meta-controller as opposed to a raw controller (see HYPER_* classes)
+ * @param <T> type of controller this wraps - could also be a MetaController itself
+ *
+ * @author Dheeraj Prakash
+ */
+public interface MetaController<T extends SendableMotorController> 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;
+        }
+    }
+}
diff --git a/src/main/java/org/hyperonline/hyperlib/controller/RawController.java b/src/main/java/org/hyperonline/hyperlib/controller/RawController.java
new file mode 100644
index 0000000..849fa38
--- /dev/null
+++ b/src/main/java/org/hyperonline/hyperlib/controller/RawController.java
@@ -0,0 +1,7 @@
+package org.hyperonline.hyperlib.controller;
+
+/**
+ * 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/groups/CANSparkMaxGroup.java b/src/main/java/org/hyperonline/hyperlib/controller/groups/CANSparkMaxGroup.java
index f66f67e..8577910 100644
--- a/src/main/java/org/hyperonline/hyperlib/controller/groups/CANSparkMaxGroup.java
+++ b/src/main/java/org/hyperonline/hyperlib/controller/groups/CANSparkMaxGroup.java
@@ -1,6 +1,7 @@
 package org.hyperonline.hyperlib.controller.groups;
 
 import org.hyperonline.hyperlib.controller.HYPER_CANSparkMax;
+import org.hyperonline.hyperlib.controller.MetaController;
 import org.hyperonline.hyperlib.controller.SendableMotorController;
 
 /**
@@ -9,7 +10,7 @@
  *
  * @author Dheeraj Prakash
  */
-public class CANSparkMaxGroup extends ControllerGroup<HYPER_CANSparkMax, HYPER_CANSparkMax> implements SendableMotorController {
+public class CANSparkMaxGroup extends ControllerGroup<HYPER_CANSparkMax, HYPER_CANSparkMax> implements MetaController<HYPER_CANSparkMax> {
     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/groups/ControllerGroup.java b/src/main/java/org/hyperonline/hyperlib/controller/groups/ControllerGroup.java
index b39bde6..a9d2f4e 100644
--- a/src/main/java/org/hyperonline/hyperlib/controller/groups/ControllerGroup.java
+++ b/src/main/java/org/hyperonline/hyperlib/controller/groups/ControllerGroup.java
@@ -2,6 +2,7 @@
 
 import com.ctre.phoenix.motorcontrol.NeutralMode;
 import edu.wpi.first.util.sendable.SendableBuilder;
+import org.hyperonline.hyperlib.controller.MetaController;
 import org.hyperonline.hyperlib.controller.SendableMotorController;
 
 /**
@@ -13,7 +14,7 @@
  *
  * @author Dheeraj Prakash
  */
-abstract class ControllerGroup<M extends SendableMotorController, S extends SendableMotorController> implements SendableMotorController {
+abstract class ControllerGroup<M extends SendableMotorController, S extends SendableMotorController> implements MetaController<M> {
     protected final M master;
     protected final S slave;
 
@@ -50,6 +51,11 @@ 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.");
diff --git a/src/main/java/org/hyperonline/hyperlib/controller/groups/PhoenixControllerGroup.java b/src/main/java/org/hyperonline/hyperlib/controller/groups/PhoenixControllerGroup.java
index 0f6ead5..39ef451 100644
--- a/src/main/java/org/hyperonline/hyperlib/controller/groups/PhoenixControllerGroup.java
+++ b/src/main/java/org/hyperonline/hyperlib/controller/groups/PhoenixControllerGroup.java
@@ -1,6 +1,7 @@
 package org.hyperonline.hyperlib.controller.groups;
 
 import com.ctre.phoenix.motorcontrol.IMotorController;
+import org.hyperonline.hyperlib.controller.MetaController;
 import org.hyperonline.hyperlib.controller.SendableMotorController;
 
 /**
@@ -15,7 +16,7 @@
 public class PhoenixControllerGroup<
         M extends SendableMotorController & IMotorController,
         S extends SendableMotorController & IMotorController
-        > extends ControllerGroup<M, S> implements SendableMotorController {
+        > extends ControllerGroup<M, S> implements MetaController<M> {
     public PhoenixControllerGroup(M master, S slave, boolean invertSlave) {
         super(master, slave);
         slave.follow(master);
diff --git a/src/main/java/org/hyperonline/hyperlib/controller/limits/DualLimitedController.java b/src/main/java/org/hyperonline/hyperlib/controller/limits/DualLimitedController.java
index 1a2047d..35600a5 100644
--- a/src/main/java/org/hyperonline/hyperlib/controller/limits/DualLimitedController.java
+++ b/src/main/java/org/hyperonline/hyperlib/controller/limits/DualLimitedController.java
@@ -5,9 +5,11 @@
 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.MetaController;
 import org.hyperonline.hyperlib.controller.SendableMotorController;
 import org.hyperonline.hyperlib.pref.DoublePreference;
 
+import java.util.Map;
 import java.util.function.BooleanSupplier;
 import java.util.function.DoubleSupplier;
 
@@ -17,7 +19,7 @@
  *
  * @author Dheeraj Prakash
  */
-public class DualLimitedController<T extends SendableMotorController> implements SendableMotorController {
+public class DualLimitedController<T extends SendableMotorController> implements MetaController<T> {
     private final T controller;
     private final ForwardLimitedController<T> forwardController;
     private final ReverseLimitedController<T> reverseController;
@@ -146,10 +148,6 @@ public boolean canMoveReverse() {
         return reverseController.canMoveReverse();
     }
 
-    public T getController() {
-        return forwardController.getController();
-    }
-
     public DigitalInput getForwardLimit() {
         return forwardController.getLimit();
     }
@@ -214,4 +212,9 @@ public void disable() {
     public void stopMotor() {
         controller.stopMotor();
     }
+
+    @Override
+    public T getController() {
+        return forwardController.getController();
+    }
 }
diff --git a/src/main/java/org/hyperonline/hyperlib/controller/limits/ForwardLimitedController.java b/src/main/java/org/hyperonline/hyperlib/controller/limits/ForwardLimitedController.java
index 91fdbd9..2f0d2a4 100644
--- a/src/main/java/org/hyperonline/hyperlib/controller/limits/ForwardLimitedController.java
+++ b/src/main/java/org/hyperonline/hyperlib/controller/limits/ForwardLimitedController.java
@@ -7,6 +7,7 @@
 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.MetaController;
 import org.hyperonline.hyperlib.controller.SendableMotorController;
 import org.hyperonline.hyperlib.pref.DoublePreference;
 
@@ -19,7 +20,7 @@
  *
  * @author Dheeraj Prakash
  */
-public class ForwardLimitedController<T extends SendableMotorController> implements SendableMotorController {
+public class ForwardLimitedController<T extends SendableMotorController> implements MetaController<T> {
 
     /**
      * The controller being used
@@ -190,14 +191,6 @@ public boolean canMoveForward() {
         return !this.atForwardLimit();
     }
 
-    /**
-     * Get motor controller being used.
-     * @return the controller
-     */
-    public T getController() {
-        return controller;
-    }
-
     /**
      * Get limit source.
      * @return DigitalInput of the limit switch
@@ -266,4 +259,13 @@ public void disable() {
     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/limits/ReverseLimitedController.java b/src/main/java/org/hyperonline/hyperlib/controller/limits/ReverseLimitedController.java
index 660f603..cce5186 100644
--- a/src/main/java/org/hyperonline/hyperlib/controller/limits/ReverseLimitedController.java
+++ b/src/main/java/org/hyperonline/hyperlib/controller/limits/ReverseLimitedController.java
@@ -7,6 +7,7 @@
 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.MetaController;
 import org.hyperonline.hyperlib.controller.SendableMotorController;
 import org.hyperonline.hyperlib.pref.DoublePreference;
 
@@ -21,7 +22,7 @@
  *
  * TODO: write docs for methods (copy from ForwardLimitedController)
  */
-public class ReverseLimitedController<T extends SendableMotorController> implements SendableMotorController {
+public class ReverseLimitedController<T extends SendableMotorController> implements MetaController<T> {
     private final T controller;
     private final DigitalInput limit;
     private final DoublePreference reverseSpeed;
@@ -103,10 +104,6 @@ public boolean canMoveReverse() {
         return !this.atReverseLimit();
     }
 
-    public T getController() {
-        return controller;
-    }
-
     public DigitalInput getLimit() {
         return limit;
     }
@@ -163,4 +160,9 @@ public void disable() {
     public void stopMotor() {
         controller.stopMotor();
     }
+
+    @Override
+    public T getController() {
+        return controller;
+    }
 }

From 0ca34098c66c0b496e95a6afd82a27a918f6879d Mon Sep 17 00:00:00 2001
From: Dheeraj Prakash <dheerajpv16@gmail.com>
Date: Sat, 1 Apr 2023 17:17:51 -0400
Subject: [PATCH 11/13] make ControllerGroup public

---
 .../hyperonline/hyperlib/controller/groups/ControllerGroup.java | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/java/org/hyperonline/hyperlib/controller/groups/ControllerGroup.java b/src/main/java/org/hyperonline/hyperlib/controller/groups/ControllerGroup.java
index a9d2f4e..d3d5a07 100644
--- a/src/main/java/org/hyperonline/hyperlib/controller/groups/ControllerGroup.java
+++ b/src/main/java/org/hyperonline/hyperlib/controller/groups/ControllerGroup.java
@@ -14,7 +14,7 @@
  *
  * @author Dheeraj Prakash
  */
-abstract class ControllerGroup<M extends SendableMotorController, S extends SendableMotorController> implements MetaController<M> {
+public abstract class ControllerGroup<M extends SendableMotorController, S extends SendableMotorController> implements MetaController<M> {
     protected final M master;
     protected final S slave;
 

From 385275c774e174cc0f9f8b38f785724b29487aa1 Mon Sep 17 00:00:00 2001
From: Dheeraj Prakash <dheerajpv16@gmail.com>
Date: Sat, 1 Apr 2023 17:23:17 -0400
Subject: [PATCH 12/13] getControllerGroup() to stop hitting the bottom earlier

---
 .../hyperlib/controller/MetaController.java    | 18 ++++++++++++++++++
 1 file changed, 18 insertions(+)

diff --git a/src/main/java/org/hyperonline/hyperlib/controller/MetaController.java b/src/main/java/org/hyperonline/hyperlib/controller/MetaController.java
index 1d7053e..09a09ec 100644
--- a/src/main/java/org/hyperonline/hyperlib/controller/MetaController.java
+++ b/src/main/java/org/hyperonline/hyperlib/controller/MetaController.java
@@ -1,5 +1,7 @@
 package org.hyperonline.hyperlib.controller;
 
+import org.hyperonline.hyperlib.controller.groups.ControllerGroup;
+
 /**
  * Interface that represents a meta-controller as opposed to a raw controller (see HYPER_* classes)
  * @param <T> type of controller this wraps - could also be a MetaController itself
@@ -26,4 +28,20 @@ default SendableMotorController getRawController() {
             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;
+        }
+    }
 }

From cc92336e521fe848d0da89d40917c6eb61da254b Mon Sep 17 00:00:00 2001
From: Dheeraj Prakash <dheerajpv16@gmail.com>
Date: Sat, 1 Apr 2023 17:25:04 -0400
Subject: [PATCH 13/13] move into controllers.meta package

---
 .../hyperonline/hyperlib/controller/HYPER_CANSparkMax.java   | 3 +--
 .../hyperonline/hyperlib/controller/HYPER_PWMSparkMax.java   | 1 +
 .../org/hyperonline/hyperlib/controller/HYPER_TalonSRX.java  | 1 +
 .../org/hyperonline/hyperlib/controller/HYPER_VictorSPX.java | 1 +
 .../hyperlib/controller/{ => meta}/MetaController.java       | 5 +++--
 .../hyperlib/controller/{ => meta}/RawController.java        | 4 +++-
 .../controller/{ => meta}/groups/CANSparkMaxGroup.java       | 5 ++---
 .../controller/{ => meta}/groups/ControllerGroup.java        | 4 ++--
 .../controller/{ => meta}/groups/PhoenixControllerGroup.java | 4 ++--
 .../controller/{ => meta}/limits/DualLimitedController.java  | 5 ++---
 .../{ => meta}/limits/ForwardLimitedController.java          | 4 ++--
 .../{ => meta}/limits/ReverseLimitedController.java          | 4 ++--
 12 files changed, 22 insertions(+), 19 deletions(-)
 rename src/main/java/org/hyperonline/hyperlib/controller/{ => meta}/MetaController.java (89%)
 rename src/main/java/org/hyperonline/hyperlib/controller/{ => meta}/RawController.java (67%)
 rename src/main/java/org/hyperonline/hyperlib/controller/{ => meta}/groups/CANSparkMaxGroup.java (76%)
 rename src/main/java/org/hyperonline/hyperlib/controller/{ => meta}/groups/ControllerGroup.java (94%)
 rename src/main/java/org/hyperonline/hyperlib/controller/{ => meta}/groups/PhoenixControllerGroup.java (87%)
 rename src/main/java/org/hyperonline/hyperlib/controller/{ => meta}/limits/DualLimitedController.java (97%)
 rename src/main/java/org/hyperonline/hyperlib/controller/{ => meta}/limits/ForwardLimitedController.java (98%)
 rename src/main/java/org/hyperonline/hyperlib/controller/{ => meta}/limits/ReverseLimitedController.java (97%)

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 d6d8fb4..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}.
  *
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 7b1aaad..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,6 +2,7 @@
 
 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 RawController {
     /**
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 a4618a4..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;
 
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 37eac7c..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;
 
diff --git a/src/main/java/org/hyperonline/hyperlib/controller/MetaController.java b/src/main/java/org/hyperonline/hyperlib/controller/meta/MetaController.java
similarity index 89%
rename from src/main/java/org/hyperonline/hyperlib/controller/MetaController.java
rename to src/main/java/org/hyperonline/hyperlib/controller/meta/MetaController.java
index 09a09ec..f29efec 100644
--- a/src/main/java/org/hyperonline/hyperlib/controller/MetaController.java
+++ b/src/main/java/org/hyperonline/hyperlib/controller/meta/MetaController.java
@@ -1,6 +1,7 @@
-package org.hyperonline.hyperlib.controller;
+package org.hyperonline.hyperlib.controller.meta;
 
-import org.hyperonline.hyperlib.controller.groups.ControllerGroup;
+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)
diff --git a/src/main/java/org/hyperonline/hyperlib/controller/RawController.java b/src/main/java/org/hyperonline/hyperlib/controller/meta/RawController.java
similarity index 67%
rename from src/main/java/org/hyperonline/hyperlib/controller/RawController.java
rename to src/main/java/org/hyperonline/hyperlib/controller/meta/RawController.java
index 849fa38..251b138 100644
--- a/src/main/java/org/hyperonline/hyperlib/controller/RawController.java
+++ b/src/main/java/org/hyperonline/hyperlib/controller/meta/RawController.java
@@ -1,4 +1,6 @@
-package org.hyperonline.hyperlib.controller;
+package org.hyperonline.hyperlib.controller.meta;
+
+import org.hyperonline.hyperlib.controller.SendableMotorController;
 
 /**
  * Interface to represent a raw (not {@link MetaController}) controller.
diff --git a/src/main/java/org/hyperonline/hyperlib/controller/groups/CANSparkMaxGroup.java b/src/main/java/org/hyperonline/hyperlib/controller/meta/groups/CANSparkMaxGroup.java
similarity index 76%
rename from src/main/java/org/hyperonline/hyperlib/controller/groups/CANSparkMaxGroup.java
rename to src/main/java/org/hyperonline/hyperlib/controller/meta/groups/CANSparkMaxGroup.java
index 8577910..dfe9526 100644
--- a/src/main/java/org/hyperonline/hyperlib/controller/groups/CANSparkMaxGroup.java
+++ b/src/main/java/org/hyperonline/hyperlib/controller/meta/groups/CANSparkMaxGroup.java
@@ -1,8 +1,7 @@
-package org.hyperonline.hyperlib.controller.groups;
+package org.hyperonline.hyperlib.controller.meta.groups;
 
 import org.hyperonline.hyperlib.controller.HYPER_CANSparkMax;
-import org.hyperonline.hyperlib.controller.MetaController;
-import org.hyperonline.hyperlib.controller.SendableMotorController;
+import org.hyperonline.hyperlib.controller.meta.MetaController;
 
 /**
  * Class that groups together two {@link HYPER_CANSparkMax}es as master and slave.
diff --git a/src/main/java/org/hyperonline/hyperlib/controller/groups/ControllerGroup.java b/src/main/java/org/hyperonline/hyperlib/controller/meta/groups/ControllerGroup.java
similarity index 94%
rename from src/main/java/org/hyperonline/hyperlib/controller/groups/ControllerGroup.java
rename to src/main/java/org/hyperonline/hyperlib/controller/meta/groups/ControllerGroup.java
index d3d5a07..0161323 100644
--- a/src/main/java/org/hyperonline/hyperlib/controller/groups/ControllerGroup.java
+++ b/src/main/java/org/hyperonline/hyperlib/controller/meta/groups/ControllerGroup.java
@@ -1,8 +1,8 @@
-package org.hyperonline.hyperlib.controller.groups;
+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.MetaController;
+import org.hyperonline.hyperlib.controller.meta.MetaController;
 import org.hyperonline.hyperlib.controller.SendableMotorController;
 
 /**
diff --git a/src/main/java/org/hyperonline/hyperlib/controller/groups/PhoenixControllerGroup.java b/src/main/java/org/hyperonline/hyperlib/controller/meta/groups/PhoenixControllerGroup.java
similarity index 87%
rename from src/main/java/org/hyperonline/hyperlib/controller/groups/PhoenixControllerGroup.java
rename to src/main/java/org/hyperonline/hyperlib/controller/meta/groups/PhoenixControllerGroup.java
index 39ef451..9a37d42 100644
--- a/src/main/java/org/hyperonline/hyperlib/controller/groups/PhoenixControllerGroup.java
+++ b/src/main/java/org/hyperonline/hyperlib/controller/meta/groups/PhoenixControllerGroup.java
@@ -1,7 +1,7 @@
-package org.hyperonline.hyperlib.controller.groups;
+package org.hyperonline.hyperlib.controller.meta.groups;
 
 import com.ctre.phoenix.motorcontrol.IMotorController;
-import org.hyperonline.hyperlib.controller.MetaController;
+import org.hyperonline.hyperlib.controller.meta.MetaController;
 import org.hyperonline.hyperlib.controller.SendableMotorController;
 
 /**
diff --git a/src/main/java/org/hyperonline/hyperlib/controller/limits/DualLimitedController.java b/src/main/java/org/hyperonline/hyperlib/controller/meta/limits/DualLimitedController.java
similarity index 97%
rename from src/main/java/org/hyperonline/hyperlib/controller/limits/DualLimitedController.java
rename to src/main/java/org/hyperonline/hyperlib/controller/meta/limits/DualLimitedController.java
index 35600a5..7274030 100644
--- a/src/main/java/org/hyperonline/hyperlib/controller/limits/DualLimitedController.java
+++ b/src/main/java/org/hyperonline/hyperlib/controller/meta/limits/DualLimitedController.java
@@ -1,15 +1,14 @@
-package org.hyperonline.hyperlib.controller.limits;
+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.MetaController;
+import org.hyperonline.hyperlib.controller.meta.MetaController;
 import org.hyperonline.hyperlib.controller.SendableMotorController;
 import org.hyperonline.hyperlib.pref.DoublePreference;
 
-import java.util.Map;
 import java.util.function.BooleanSupplier;
 import java.util.function.DoubleSupplier;
 
diff --git a/src/main/java/org/hyperonline/hyperlib/controller/limits/ForwardLimitedController.java b/src/main/java/org/hyperonline/hyperlib/controller/meta/limits/ForwardLimitedController.java
similarity index 98%
rename from src/main/java/org/hyperonline/hyperlib/controller/limits/ForwardLimitedController.java
rename to src/main/java/org/hyperonline/hyperlib/controller/meta/limits/ForwardLimitedController.java
index 2f0d2a4..3bbfb00 100644
--- a/src/main/java/org/hyperonline/hyperlib/controller/limits/ForwardLimitedController.java
+++ b/src/main/java/org/hyperonline/hyperlib/controller/meta/limits/ForwardLimitedController.java
@@ -1,4 +1,4 @@
-package org.hyperonline.hyperlib.controller.limits;
+package org.hyperonline.hyperlib.controller.meta.limits;
 
 import com.ctre.phoenix.motorcontrol.NeutralMode;
 import edu.wpi.first.util.sendable.SendableBuilder;
@@ -7,7 +7,7 @@
 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.MetaController;
+import org.hyperonline.hyperlib.controller.meta.MetaController;
 import org.hyperonline.hyperlib.controller.SendableMotorController;
 import org.hyperonline.hyperlib.pref.DoublePreference;
 
diff --git a/src/main/java/org/hyperonline/hyperlib/controller/limits/ReverseLimitedController.java b/src/main/java/org/hyperonline/hyperlib/controller/meta/limits/ReverseLimitedController.java
similarity index 97%
rename from src/main/java/org/hyperonline/hyperlib/controller/limits/ReverseLimitedController.java
rename to src/main/java/org/hyperonline/hyperlib/controller/meta/limits/ReverseLimitedController.java
index cce5186..8260636 100644
--- a/src/main/java/org/hyperonline/hyperlib/controller/limits/ReverseLimitedController.java
+++ b/src/main/java/org/hyperonline/hyperlib/controller/meta/limits/ReverseLimitedController.java
@@ -1,4 +1,4 @@
-package org.hyperonline.hyperlib.controller.limits;
+package org.hyperonline.hyperlib.controller.meta.limits;
 
 import com.ctre.phoenix.motorcontrol.NeutralMode;
 import edu.wpi.first.util.sendable.SendableBuilder;
@@ -7,7 +7,7 @@
 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.MetaController;
+import org.hyperonline.hyperlib.controller.meta.MetaController;
 import org.hyperonline.hyperlib.controller.SendableMotorController;
 import org.hyperonline.hyperlib.pref.DoublePreference;