From 1d96a23741a46da680aa08478eb0eed95d4b93b0 Mon Sep 17 00:00:00 2001
From: T Grinch <10247070+thenetworkgrinch@users.noreply.github.com>
Date: Thu, 18 Jan 2024 12:24:19 -0600
Subject: [PATCH] Fix command functions in example subsystems.

Signed-off-by: thenetworkgrinch <thenetworkgrinch@users.noreply.github.com>
---
 .../subsystems/swervedrive/SwerveSubsystem.java  | 16 ++++++++++------
 1 file changed, 10 insertions(+), 6 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java
index a8e764517..6f868e557 100644
--- a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java
@@ -11,7 +11,6 @@
 import com.pathplanner.lib.util.ReplanningConfig;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
@@ -150,10 +149,14 @@ public Command driveCommand(DoubleSupplier translationX, DoubleSupplier translat
                               DoubleSupplier headingY)
   {
     return run(() -> {
+      double xInput = Math.pow(translationX.getAsDouble(), 3); // Smooth controll out
+      double yInput = Math.pow(translationY.getAsDouble(), 3); // Smooth controll out
       // Make the robot move
-      driveFieldOriented(getTargetSpeeds(translationX.getAsDouble(), translationY.getAsDouble(),
-                                         headingX.getAsDouble(),
-                                         headingY.getAsDouble()));
+      driveFieldOriented(swerveDrive.swerveController.getTargetSpeeds(xInput, yInput,
+                                                                      headingX.getAsDouble(),
+                                                                      headingY.getAsDouble(),
+                                                                      swerveDrive.getYaw().getRadians(),
+                                                                      swerveDrive.getMaximumVelocity()));
     });
   }
 
@@ -169,8 +172,9 @@ public Command driveCommand(DoubleSupplier translationX, DoubleSupplier translat
   {
     return run(() -> {
       // Make the robot move
-      swerveDrive.drive(new Translation2d(translationX.getAsDouble() * maximumSpeed, translationY.getAsDouble()),
-                        angularRotationX.getAsDouble() * swerveDrive.swerveController.config.maxAngularVelocity,
+      swerveDrive.drive(new Translation2d(translationX.getAsDouble() * swerveDrive.getMaximumVelocity(),
+                                          translationY.getAsDouble() * swerveDrive.getMaximumVelocity()),
+                        angularRotationX.getAsDouble() * swerveDrive.getMaximumAngularVelocity(),
                         true,
                         false);
     });