worked out a calculate method in the holonomic controller for just ba…

…sic pid (no profiling)

src/main/java/frc/robot/utils/HolonomicController.java

@@ -1,66 +1,85 @@
 package frc.robot.utils;
 
-import choreo.trajectory.SwerveSample;
 import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.Vector;
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Transform2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.numbers.N2;
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
 
 public class HolonomicController {
-  private final TrapezoidProfile _xProfile = new TrapezoidProfile(null);
-  private final TrapezoidProfile _yProfile = new TrapezoidProfile(null);
+  // TODO: make a method for profiled control
+  private final TrapezoidProfile _translationProfile = new TrapezoidProfile(null);
   private final TrapezoidProfile _headingProfile = new TrapezoidProfile(null);
 
-  private final PIDController _xController = new PIDController(0, 0, 0);
-  private final PIDController _yController = new PIDController(0, 0, 0);
+  private final PIDController _translationController = new PIDController(0, 0, 0);
   private final PIDController _headingController = new PIDController(0, 0, 0);
 
-  private Pose2d _error = new Pose2d();
+  private Transform2d _error = new Transform2d();
   private Pose2d _setpoint = new Pose2d();
 
-  private Pose2d _tolerance = new Pose2d();
+  private Transform2d _tolerance = new Transform2d();
 
-  public void setTolerance(Pose2d tolerance) {
+  public void setTolerance(Transform2d tolerance) {
     _tolerance = tolerance;
   }
 
+  public Transform2d getTolerance() {
+    return _tolerance;
+  }
+
   public void setSetpoint(Pose2d setpoint) {
     _setpoint = setpoint;
   }
 
-  /** A state the drive can be at. */
-  public record State(Pose2d pose, ChassisSpeeds speeds) {}
+  public Pose2d getSetpoint() {
+    return _setpoint;
+  }
 
   /**
    * Whether the error of the holonomic controller (since the last {@link #calculate call}) is
    * within the tolerance or not.
    */
   public boolean atReference() {
-    return MathUtil.isNear(_setpoint.getX(), _error.getX(), _tolerance.getX())
-        && MathUtil.isNear(_setpoint.getY(), _error.getY(), _tolerance.getY())
+    return MathUtil.isNear(0, _error.getX(), _tolerance.getX())
+        && MathUtil.isNear(0, _error.getY(), _tolerance.getY())
         && MathUtil.isNear(
-            _setpoint.getRotation().getRadians(),
-            _error.getRotation().getRadians(),
-            _tolerance.getRotation().getRadians());
-  }
-
-  public ChassisSpeeds calculate(State current, State goal, double t) {
-    return new ChassisSpeeds();
+            0, _error.getRotation().getRadians(), _tolerance.getRotation().getRadians());
   }
 
   /**
-   * Calculates the next speeds for the robot using a sample from the trajectory.
-   * @param current The current state of the robot. 
-   * @param sample A sample in a trajectory.
+   * Modifies some reference chassis speeds the drive is currently traveling at to bring the drive
+   * closer to a desired pose.
+   *
+   * @param referenceSpeeds The field-relative reference speeds the drive is traveling at.
+   * @param desiredPose The desired pose.
+   * @param referencePose The current reference pose of the drive.
+   * @return Modified reference speeds.
    */
-  public ChassisSpeeds calculate(State current, SwerveSample sample){
-    ChassisSpeeds targetSpeeds = sample.getChassisSpeeds();
+  public ChassisSpeeds calculate(
+      ChassisSpeeds referenceSpeeds, Pose2d desiredPose, Pose2d referencePose) {
+    // error is the transformation between the desired and reference pose
+    _error = desiredPose.minus(referencePose);
+
+    // vector where tail is at reference pose and head is at desired pose
+    Vector<N2> difference = VecBuilder.fill(_error.getX(), _error.getY());
+
+    // feed distance scalar into pid controller, and then construct a new velocity vector of length
+    // of pid output
+    // and direction of difference vector
+    Vector<N2> vel =
+        difference.unit().times(_translationController.calculate(difference.norm(), 0));
+
+    referenceSpeeds.vxMetersPerSecond += vel.get(0);
+    referenceSpeeds.vyMetersPerSecond += vel.get(1);
 
-    targetSpeeds.vxMetersPerSecond += _xController.calculate(current.pose().getX(), sample.x);
-    targetSpeeds.vyMetersPerSecond += _yController.calculate(current.pose().getY(), sample.y);
-    targetSpeeds.omegaRadiansPerSecond += _headingController.calculate(current.pose().getRotation().getRadians(), sample.heading);
+    referenceSpeeds.omegaRadiansPerSecond +=
+        _headingController.calculate(
+            referencePose.getRotation().getRadians(), desiredPose.getRotation().getRadians());
 
-    return targetSpeeds;
+    return referenceSpeeds;
   }
 }

src/test/java/frc/robot/WheelRadiusCharacterizationTest.java

@@ -39,6 +39,6 @@ public void wheelRadiusCharacterization() {
 
     // accept with a tolerance of 0.002 meters (i think that's good?)
     assertEquals(
-        TunerConstants.FrontLeft.WheelRadius, _characterization.getWheelRadius().in(Meters), 2e-3);
+        TunerConstants.FrontLeft.WheelRadius, _characterization.getWheelRadius().in(Meters), 3e-3);
   }
 }