Fix command functions in example subsystems.

Signed-off-by: thenetworkgrinch <thenetworkgrinch@users.noreply.github.com>
BroncBotz3481 · Jan 18, 2024 · 1d96a23 · 1d96a23
1 parent 76bf2dd
commit 1d96a23
Showing 1 changed file with 10 additions and 6 deletions.
diff --git 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);
     });