Use delegation pattern for registering with the AutoDrive subsystem t…

…o negotiate joystick values

src/main/java/frc/robot/commands/auto/Top3Ball.java

@@ -25,9 +25,4 @@ public Top3Ball(Drivetrain drivetrain, Heading heading, AutoDrive autoDrive) {
     );
   }
 
-  @Override
-  public State calculate(double forward, double strafe, boolean isFieldOriented) {
-    return m_currentCommand.calculate(forward, strafe, isFieldOriented);
-  }
-
 }

src/main/java/frc/robot/commands/teleop/PointToPointCommand.java

@@ -49,6 +49,7 @@ public PointToPointCommand(PolarCoordinate target, Supplier<Pose2d> poseSupplier
   @Override
   public void initialize() {
     heading.enableMaintainHeading();
+    autoDrive.registerAutoDrivableCommand(this);
   }
 
   @Override

src/main/java/frc/robot/subsystems/AutoDrive.java

@@ -1,13 +1,15 @@
 package frc.robot.subsystems;
 
+import java.lang.ref.WeakReference;
+
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.CommandBase;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.commands.AutoDrivableCommand;
 
 public class AutoDrive extends SubsystemBase {
 
+  private WeakReference<AutoDrivableCommand> commandReference;
+
   public static class State {
     public double forward;
     public double strafe;
@@ -20,6 +22,19 @@ public State(double forward, double strafe, boolean isFieldOriented) {
     }
   }
 
+  /**
+   * Registered an AutoDrivableCommand to be called when an output is needed for
+   * the SwerveDriveCommand. Note: Only one AutoDrivableCommand can be registered
+   * at a time. Commands calling registerAutoDrivableCommand should require
+   * the AutoDrive subsystem in order to prevent other commands from
+   * registering for callbacks.
+   *
+   * @param command
+   */
+  public void registerAutoDrivableCommand(AutoDrivableCommand command) {
+    this.commandReference = new WeakReference<AutoDrivableCommand>(command);
+  }
+
   /**
    * Calculate forward/strafe values using the current command. May return null if
    * there is no auto drive command scheduled or the currently auto drive command
@@ -31,28 +46,21 @@ public State(double forward, double strafe, boolean isFieldOriented) {
    * @return - A negotiated forward/strafe from the auto driveable command
    */
   public State calculate(double forward, double strafe, boolean isFieldOriented) {
-    AutoDrivableCommand command = getAutoDrivableCommand();
+    AutoDrivableCommand command = commandReference.get();
     if (command == null) {
       return null;
     }
     return command.calculate(forward, strafe, isFieldOriented);
   }
 
-  /**
-   * Gets the current Command for the subsystem if it's an AutoDrivableCommand.
-   * May return null if the subsystem has no Command or the Command does
-   * not conform to AutoDrivableCommand.
-   */
-  private AutoDrivableCommand getAutoDrivableCommand() {
-    Command command = getCurrentCommand();
+  @Override
+  public void periodic() {
+    AutoDrivableCommand command = commandReference.get();
     if (command != null) {
-      SmartDashboard.putData("AutoDrive Current Command", (CommandBase) command);
-      SmartDashboard.putBoolean("Is AutoDrivableCommand", command instanceof AutoDrivableCommand);
-    }
-    if (command != null && command instanceof AutoDrivableCommand) {
-      return (AutoDrivableCommand) command;
+      SmartDashboard.putString("AutoDrivable Command", command.toString());
+    } else {
+      SmartDashboard.putString("AutoDrivable Command", "N/A");
     }
-    return null;
   }
 
 }