Fixed crashing by removing phase correction. Added units to driveToPose.

Signed-off-by: thenetworkgrinch <[email protected]>
BroncBotz3481 · Feb 22, 2025 · 3e6db97 · 3e6db97
1 parent 8efd746
commit 3e6db97
Show file tree

Hide file tree

Showing 5 changed files with 25 additions and 14 deletions.
diff --git a/simgui.json b/simgui.json
@@ -60,6 +60,7 @@
       "/LiveWindow/Ungrouped/PIDController[1]": "PIDController",
       "/LiveWindow/Ungrouped/Pigeon 2 [13]": "Gyro",
       "/LiveWindow/Ungrouped/Scheduler": "Scheduler",
+      "/SmartDashboard/Alerts": "Alerts",
       "/SmartDashboard/Encoders": "Alerts",
       "/SmartDashboard/Field": "Field2d",
       "/SmartDashboard/IMU": "Alerts",

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -5,9 +5,11 @@
 package frc.robot;
 
 import com.pathplanner.lib.auto.NamedCommands;
+import edu.wpi.first.math.controller.ProfiledPIDController;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.Filesystem;
 import edu.wpi.first.wpilibj.RobotBase;
@@ -104,7 +106,6 @@ public RobotContainer()
    */
   private void configureBindings()
   {
-
     Command driveFieldOrientedDirectAngle      = drivebase.driveFieldOriented(driveDirectAngle);
     Command driveFieldOrientedAnglularVelocity = drivebase.driveFieldOriented(driveAngularVelocity);
     Command driveRobotOrientedAngularVelocity  = drivebase.driveFieldOriented(driveRobotOriented);
@@ -125,8 +126,25 @@ private void configureBindings()
 
     if (Robot.isSimulation())
     {
+      driveDirectAngleKeyboard.driveToPose(() -> new Pose2d(new Translation2d(9, 3),
+                                                            Rotation2d.fromDegrees(90)),
+                                           new ProfiledPIDController(5,
+                                                                     0,
+                                                                     0,
+                                                                     new Constraints(5,
+                                                                                     3)),
+                                           new ProfiledPIDController(5,
+                                                                     0,
+                                                                     0,
+                                                                     new Constraints(
+                                                                         Math.toRadians(
+                                                                             360),
+                                                                         Math.toRadians(
+                                                                             90))));
       driverXbox.start().onTrue(Commands.runOnce(() -> drivebase.resetOdometry(new Pose2d(3, 3, new Rotation2d()))));
       driverXbox.button(1).whileTrue(drivebase.sysIdDriveMotorCommand());
+      driverXbox.button(2).whileTrue(Commands.runEnd(() -> driveDirectAngleKeyboard.driveToPoseEnabled(true),
+                                                     () -> driveDirectAngleKeyboard.driveToPoseEnabled(false)));
 
     }
     if (DriverStation.isTest())

diff --git a/src/main/java/swervelib/SwerveInputStream.java b/src/main/java/swervelib/SwerveInputStream.java
@@ -273,9 +273,9 @@ public SwerveInputStream robotRelative(boolean enabled)
    * Drive to a given pose with the provided {@link ProfiledPIDController}s
    *
    * @param pose               {@link Supplier<Pose2d>} for ease of use.
-   * @param xPIDController     PID controller for the X axis.
-   * @param yPIDController     PID controller for the Y axis.
-   * @param omegaPIDController PID Controller for rotational axis.
+   * @param xPIDController     PID controller for the X axis, units are m/s.
+   * @param yPIDController     PID controller for the Y axis, units are m/s.
+   * @param omegaPIDController PID Controller for rotational axis, units are rad/s.
    * @return self
    */
   public SwerveInputStream driveToPose(Supplier<Pose2d> pose, ProfiledPIDController xPIDController,
@@ -292,8 +292,8 @@ public SwerveInputStream driveToPose(Supplier<Pose2d> pose, ProfiledPIDControlle
    * Drive to a given pose with the provided {@link ProfiledPIDController}s
    *
    * @param pose        {@link Supplier<Pose2d>} for ease of use.
-   * @param translation PID controller for the X and Y axis.
-   * @param rotation    PID Controller for rotational axis.
+   * @param translation PID controller for the X and Y axis, units are m/s.
+   * @param rotation    PID Controller for rotational axis, units are rad/s.
    * @return self
    */
   public SwerveInputStream driveToPose(Supplier<Pose2d> pose, ProfiledPIDController translation,

diff --git a/src/main/java/swervelib/motors/SparkFlexSwerve.java b/src/main/java/swervelib/motors/SparkFlexSwerve.java
@@ -384,10 +384,6 @@ public void setMotorBrake(boolean isBrakeMode)
   public void setInverted(boolean inverted)
   {
     cfg.inverted(inverted);
-    if (isDriveMotor)
-    {
-      cfg.encoder.inverted(inverted);
-    }
   }
 
   /**

diff --git a/src/main/java/swervelib/motors/SparkMaxSwerve.java b/src/main/java/swervelib/motors/SparkMaxSwerve.java
@@ -383,10 +383,6 @@ public void setMotorBrake(boolean isBrakeMode)
   public void setInverted(boolean inverted)
   {
     cfg.inverted(inverted);
-    if (isDriveMotor)
-    {
-      cfg.encoder.inverted(inverted);
-    }
   }
 
   /**