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