PID pretty tuned

TheHighlanders · Apr 1, 2024 · 745169c · 745169c
1 parent ceb75ea
commit 745169c
Show file tree

Hide file tree

Showing 4 changed files with 11 additions and 12 deletions.
diff --git a/logs/Log_24-04-01_14-47-21.wpilog b/logs/Log_24-04-01_14-47-21.wpilog
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -13,12 +13,8 @@
 
 package frc.robot;
 
-import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
-import org.littletonrobotics.junction.networktables.LoggedDashboardNumber;
-
 import com.pathplanner.lib.auto.AutoBuilder;
 import com.pathplanner.lib.auto.NamedCommands;
-
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.wpilibj.GenericHID;
@@ -38,6 +34,8 @@
 import frc.robot.subsystems.flywheel.FlywheelIO;
 import frc.robot.subsystems.flywheel.FlywheelIOSim;
 import frc.robot.subsystems.flywheel.FlywheelIOSparkMax;
+import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
+import org.littletonrobotics.junction.networktables.LoggedDashboardNumber;
 
 /**
  * This class is where the bulk of the robot should be declared. Since Command-based is a
@@ -135,13 +133,12 @@ public RobotContainer() {
     autoChooser.addOption(
         "Flywheel SysId (Dynamic Reverse)", flywheel.sysIdDynamic(SysIdRoutine.Direction.kReverse));
     autoChooser.addOption(
-        "Drive SysID (Quad Test)", Commands.sequence(
+        "Drive SysID (Quad Test)",
+        Commands.sequence(
             drive.sysIdQuasistatic(SysIdRoutine.Direction.kForward),
             drive.sysIdQuasistatic(SysIdRoutine.Direction.kReverse),
             drive.sysIdDynamic(SysIdRoutine.Direction.kForward),
-            drive.sysIdDynamic(SysIdRoutine.Direction.kReverse))
-            );
-
+            drive.sysIdDynamic(SysIdRoutine.Direction.kReverse)));
     // Configure the button bindings
     configureButtonBindings();
   }

diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java
@@ -111,7 +111,7 @@ MAX_LINEAR_SPEED, DRIVE_BASE_RADIUS, new ReplanningConfig()),
         new SysIdRoutine(
             new SysIdRoutine.Config(
                 null,
-                null,
+                Volt.of(2.5),
                 Seconds.of(5),
                 (state) -> Logger.recordOutput("Drive/SysIdState", state.toString())),
             new SysIdRoutine.Mechanism(
@@ -296,6 +296,8 @@ public void setPose(Pose2d pose) {
     poseEstimator.resetPosition(rawGyroRotation, getModulePositions(), pose);
   }
 
+  /** Resets Gyro Angle for Driving */
+
   /**
    * Adds a vision measurement to the pose estimator.
    *

diff --git a/src/main/java/frc/robot/subsystems/drive/Module.java b/src/main/java/frc/robot/subsystems/drive/Module.java
@@ -47,9 +47,9 @@ public Module(ModuleIO io, int index) {
     switch (Constants.currentMode) {
       case REAL:
       case REPLAY:
-        driveFeedforward = new SimpleMotorFeedforward(0.1, 0.13);
-        driveFeedback = new PIDController(0.05, 0.0, 0.0);
-        turnFeedback = new PIDController(7.0, 0.0, 0.0);
+        driveFeedforward = new SimpleMotorFeedforward(0.0011966, 0.15661, 0.018981);
+        driveFeedback = new PIDController(0.024181, 0.0, 0.0);
+        turnFeedback = new PIDController(7.5, 0.0, 0.013);
         break;
       case SIM:
         driveFeedforward = new SimpleMotorFeedforward(0.0, 0.13);