Pass a pose supplier to OdometryHeadingCommand as opposed to Drivetra…

…in subsystem (#59)

src/main/java/frc/robot/commands/OdometryHeadingCommand.java

@@ -1,11 +1,12 @@
 package frc.robot.commands;
 
+import java.util.function.Supplier;
+
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc.robot.Constants;
-import frc.robot.subsystems.Drivetrain;
 import frc.robot.subsystems.Heading;
 
 /**
@@ -16,16 +17,16 @@
 public class OdometryHeadingCommand extends CommandBase {
 
   private Translation2d point;
-  private Drivetrain drivetrain;
+  private Supplier<Pose2d> poseSupplier;
   private Heading heading;
 
-  public OdometryHeadingCommand(Drivetrain drivetrain, Heading heading) {
-    this(Constants.kHub, drivetrain, heading);
+  public OdometryHeadingCommand(Supplier<Pose2d> poseSupplier, Heading heading) {
+    this(Constants.kHub, poseSupplier, heading);
   }
 
-  public OdometryHeadingCommand(Translation2d point, Drivetrain drivetrain, Heading heading) {
+  public OdometryHeadingCommand(Translation2d point, Supplier<Pose2d> poseSupplier, Heading heading) {
     this.point = point;
-    this.drivetrain = drivetrain;
+    this.poseSupplier = poseSupplier;
     this.heading = heading;
 
     addRequirements(heading);
@@ -34,7 +35,7 @@ public OdometryHeadingCommand(Translation2d point, Drivetrain drivetrain, Headin
   @Override
   public void execute() {
     // Might be able to use PolarCoordinate.fromFieldCoordinate here
-    Pose2d pose = drivetrain.getPose();
+    Pose2d pose = poseSupplier.get();
     double x = pose.getX() - point.getX();
     double y = pose.getY() - point.getY();
     double angle = Math.atan2(y, x);  // (-π, π] radians