From c70cb3d14cca3cc65a1d7b8eaee54e70bea638c4 Mon Sep 17 00:00:00 2001
From: Elvis Osmanov <gamerprotime777@gmail.com>
Date: Wed, 8 Jan 2025 23:16:25 -0500
Subject: [PATCH] holonomic controller for following a trajectory

---
 .../frc/robot/utils/HolonomicController.java     | 16 ++++++++++++++++
 1 file changed, 16 insertions(+)

diff --git a/src/main/java/frc/robot/utils/HolonomicController.java b/src/main/java/frc/robot/utils/HolonomicController.java
index 87c88c8..e6dfc82 100644
--- a/src/main/java/frc/robot/utils/HolonomicController.java
+++ b/src/main/java/frc/robot/utils/HolonomicController.java
@@ -1,5 +1,6 @@
 package frc.robot.utils;
 
+import choreo.trajectory.SwerveSample;
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.geometry.Pose2d;
@@ -47,4 +48,19 @@ public boolean atReference() {
   public ChassisSpeeds calculate(State current, State goal, double t) {
     return new ChassisSpeeds();
   }
+
+  /**
+   * 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.
+   */
+  public ChassisSpeeds calculate(State current, SwerveSample sample){
+    ChassisSpeeds targetSpeeds = sample.getChassisSpeeds();
+
+    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);
+
+    return targetSpeeds;
+  }
 }