Update CommandSwerveDrivetrain to use the correct module speed and dr…

…ivebase radius
CrossTheRoadElec · Jan 3, 2024 · dbcb25b · dbcb25b
1 parent 61625a4
commit dbcb25b
Show file tree

Hide file tree

Showing 3 changed files with 11 additions and 6 deletions.
diff --git a/java/SwerveWithPathPlanner/src/main/java/frc/robot/CommandSwerveDrivetrain.java b/java/SwerveWithPathPlanner/src/main/java/frc/robot/CommandSwerveDrivetrain.java
@@ -18,6 +18,7 @@
 import edu.wpi.first.wpilibj.RobotController;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Subsystem;
+import frc.robot.generated.TunerConstants;
 
 /**
  * Class that extends the Phoenix SwerveDrivetrain class and implements subsystem
@@ -46,15 +47,20 @@ public CommandSwerveDrivetrain(SwerveDrivetrainConstants driveTrainConstants, Sw
     }
 
     private void configurePathPlanner() {
+        double driveBaseRadius = 0;
+        for (var moduleLocation : m_moduleLocations) {
+            driveBaseRadius = Math.max(driveBaseRadius, moduleLocation.getNorm());
+        }
+
         AutoBuilder.configureHolonomic(
             ()->this.getState().Pose, // Supplier of current robot pose
             this::seedFieldRelative,  // Consumer for seeding pose against auto
             this::getCurrentRobotChassisSpeeds,
             (speeds)->this.setControl(autoRequest.withSpeeds(speeds)), // Consumer of ChassisSpeeds to drive the robot
             new HolonomicPathFollowerConfig(new PIDConstants(10, 0, 0),
                                             new PIDConstants(10, 0, 0),
-                                            1, // TODO: Max module speed. Adjust this for your specific robot
-                                            1, // TODO: Distance from center of robot to furthest module. Adjust this for your specific robot
+                                            TunerConstants.kSpeedAt12VoltsMps,
+                                            driveBaseRadius,
                                             new ReplanningConfig()),
             this); // Subsystem for requirements
     }

diff --git a/java/SwerveWithPathPlanner/src/main/java/frc/robot/RobotContainer.java b/java/SwerveWithPathPlanner/src/main/java/frc/robot/RobotContainer.java
@@ -11,7 +11,6 @@
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import frc.robot.generated.TunerConstants;
 
@@ -32,7 +31,7 @@ public class RobotContainer {
   private final SwerveRequest.PointWheelsAt point = new SwerveRequest.PointWheelsAt();
 
   /* Path follower */
-  // private Command runAuto = drivetrain.getAutoPath("Tests");
+  private Command runAuto = drivetrain.getAutoPath("Tests");
 
   private final Telemetry logger = new Telemetry(MaxSpeed);
 
@@ -66,6 +65,6 @@ public RobotContainer() {
 
   public Command getAutonomousCommand() {
     /* First put the drivetrain into auto run mode, then run the auto */
-    return Commands.runOnce(() -> {});
+    return runAuto;
   }
 }
diff --git a/java/SwerveWithPathPlanner/src/main/java/frc/robot/generated/TunerConstants.java b/java/SwerveWithPathPlanner/src/main/java/frc/robot/generated/TunerConstants.java
@@ -37,7 +37,7 @@ public class TunerConstants {
 
     // Theoretical free speed (m/s) at 12v applied output;
     // This needs to be tuned to your individual robot
-    private static final double kSpeedAt12VoltsMps = 6.0;
+    public static final double kSpeedAt12VoltsMps = 6.0;
 
     // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns;
     // This may need to be tuned to your individual robot