TARSReboot: tune pid (should drive straight)

FusionCorps · Dec 6, 2024 · ed4b004 · ed4b004
1 parent 5a0f3fa
commit ed4b004
Show file tree

Hide file tree

Showing 4 changed files with 36 additions and 21 deletions.
diff --git a/TARSReboot/src/main/java/frc/robot/Constants.java b/TARSReboot/src/main/java/frc/robot/Constants.java
@@ -4,7 +4,8 @@
 
 package frc.robot;
 
-import static edu.wpi.first.units.Units.*;
+import static edu.wpi.first.units.Units.Minute;
+import static edu.wpi.first.units.Units.Rotations;
 
 import com.ctre.phoenix6.configs.CANcoderConfiguration;
 import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
@@ -73,7 +74,13 @@ public static class DrivetrainConstants {
     // When using closed-loop control, the drive motor uses the control
     // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput
     private static final Slot0Configs driveGains =
-        new Slot0Configs().withKP(3).withKI(0).withKD(0).withKS(0).withKV(0).withKA(0);
+        new Slot0Configs()
+            .withKP(0.1661)
+            .withKI(0)
+            .withKD(0)
+            .withKS(0.11102)
+            .withKV(0.11088)
+            .withKA(0.010752);
 
     // The closed-loop output type to use for the steer motors;
     // This affects the PID/FF gains for the steer motors
@@ -114,8 +121,8 @@ public static class DrivetrainConstants {
     private static final double kSteerGearRatio = 12.8;
     private static final double kWheelRadiusInches = 2;
 
-    private static final boolean kInvertLeftSide = true;
-    private static final boolean kInvertRightSide = false;
+    private static final boolean kInvertLeftSide = false;
+    private static final boolean kInvertRightSide = true;
 
     private static final String kCANbusName = "";
     private static final int kPigeonId = 0;
@@ -148,7 +155,7 @@ public static class DrivetrainConstants {
             .withDriveInertia(kDriveInertia)
             .withSteerFrictionVoltage(kSteerFrictionVoltage)
             .withDriveFrictionVoltage(kDriveFrictionVoltage)
-            .withFeedbackSource(SteerFeedbackType.FusedCANcoder)
+            .withFeedbackSource(SteerFeedbackType.RemoteCANcoder)
             .withCouplingGearRatio(kCoupleRatio)
             .withDriveMotorInitialConfigs(driveInitialConfigs)
             .withSteerMotorInitialConfigs(steerInitialConfigs)

diff --git a/TARSReboot/src/main/java/frc/robot/RobotContainer.java b/TARSReboot/src/main/java/frc/robot/RobotContainer.java
@@ -45,7 +45,7 @@ private void configureBindings() {
         drivetrain.runSwerveFC(
             () -> -joystick.getLeftY() * MaxSpeed,
             () -> -joystick.getLeftX() * MaxSpeed,
-            () -> joystick.getRightX() * MaxAngularRate));
+            () -> -joystick.getRightX() * MaxAngularRate));
     joystick
         .b()
         .onTrue(

diff --git a/TARSReboot/src/main/java/frc/robot/Telemetry.java b/TARSReboot/src/main/java/frc/robot/Telemetry.java
@@ -4,11 +4,13 @@
 import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrain.SwerveDriveState;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
 import edu.wpi.first.networktables.DoubleArrayPublisher;
 import edu.wpi.first.networktables.DoublePublisher;
 import edu.wpi.first.networktables.NetworkTable;
 import edu.wpi.first.networktables.NetworkTableInstance;
 import edu.wpi.first.networktables.StringPublisher;
+import edu.wpi.first.networktables.StructArrayPublisher;
 import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d;
 import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
@@ -41,6 +43,10 @@ public Telemetry(double maxSpeed) {
   private final DoublePublisher velocityY = driveStats.getDoubleTopic("Velocity Y").publish();
   private final DoublePublisher speed = driveStats.getDoubleTopic("Speed").publish();
   private final DoublePublisher odomPeriod = driveStats.getDoubleTopic("Odometry Period").publish();
+  private final StructArrayPublisher<SwerveModuleState> moduleStates =
+      driveStats.getStructArrayTopic("moduleStates", SwerveModuleState.struct).publish();
+  private final StructArrayPublisher<SwerveModuleState> moduleTargets =
+      driveStats.getStructArrayTopic("moduleTargets", SwerveModuleState.struct).publish();
 
   /* Keep a reference of the last pose to calculate the speeds */
   private Pose2d m_lastPose = new Pose2d();
@@ -113,5 +119,8 @@ public void telemeterize(SwerveDriveState state) {
 
       SmartDashboard.putData("Module " + i, m_moduleMechanisms[i]);
     }
+
+    moduleStates.set(state.ModuleStates);
+    moduleTargets.set(state.ModuleTargets);
   }
 }
diff --git a/TARSReboot/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java b/TARSReboot/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java
@@ -1,6 +1,7 @@
 package frc.robot.subsystems;
 
 import static edu.wpi.first.units.Units.*;
+import static frc.robot.Constants.DrivetrainConstants.kSpeedAt12VoltsMps;
 
 import com.ctre.phoenix6.SignalLogger;
 import com.ctre.phoenix6.Utils;
@@ -17,8 +18,6 @@
 import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest.SysIdSwerveSteerGains;
 import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest.SysIdSwerveTranslation;
 import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.wpilibj.DriverStation;
-import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj.Notifier;
 import edu.wpi.first.wpilibj.RobotController;
 import edu.wpi.first.wpilibj2.command.Command;
@@ -55,8 +54,8 @@ public static enum SysIdMode {
   private FieldCentric fieldCentricRequest =
       new FieldCentric()
           .withSteerRequestType(SteerRequestType.MotionMagic)
-          .withDriveRequestType(DriveRequestType.OpenLoopVoltage)
-          .withDeadband(6 * 0.1)
+          .withDriveRequestType(DriveRequestType.Velocity)
+          .withDeadband(kSpeedAt12VoltsMps * 0.1)
           .withRotationalDeadband(Math.PI * 0.1);
 
   // Brake & point requests
@@ -135,17 +134,17 @@ public void periodic() {
      * This ensures driving behavior doesn't change until an explicit disable event
      * occurs during testing
      */
-    if (!hasAppliedOperatorPerspective || DriverStation.isDisabled()) {
-      DriverStation.getAlliance()
-          .ifPresent(
-              (allianceColor) -> {
-                this.setOperatorPerspectiveForward(
-                    allianceColor == Alliance.Red
-                        ? RedAlliancePerspectiveRotation
-                        : BlueAlliancePerspectiveRotation);
-                hasAppliedOperatorPerspective = true;
-              });
-    }
+    // if (!hasAppliedOperatorPerspective || DriverStation.isDisabled()) {
+    //   DriverStation.getAlliance()
+    //       .ifPresent(
+    //           (allianceColor) -> {
+    //             this.setOperatorPerspectiveForward(
+    //                 allianceColor == Alliance.Red
+    //                     ? RedAlliancePerspectiveRotation
+    //                     : BlueAlliancePerspectiveRotation);
+    //             hasAppliedOperatorPerspective = true;
+    //           });
+    // }
   }
 
   /**