Merge pull request #30 from TitaniumTitans/Steggo-SwervePIDTuning

Set up onboard PID loops on swerve

build.gradle

@@ -116,7 +116,7 @@ sonar {
 // AdvantageKit log replay from the command line. Set the
 // value to "true" to enable the sim GUI by default (this
 // is the standard WPILib behavior).
-wpi.sim.addGui().defaultEnabled = true
+wpi.sim.addGui()
 wpi.sim.addDriverstation()
 
 // Setting up my Jar File. In this case, adding all libraries into the main jar ('fat jar')

networktables.json

@@ -126,5 +126,53 @@
     "properties": {
       "persistent": true
     }
+  },
+  {
+    "name": "/Preferences/Sim Turn PID.mm.kp",
+    "type": "double",
+    "value": 0.1,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/Sim Turn PID.mm.ki",
+    "type": "double",
+    "value": 0.0,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/Sim Turn PID.mm.kd",
+    "type": "double",
+    "value": 0.0,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/Sim Drive PID.mm.kp",
+    "type": "double",
+    "value": 0.05,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/Sim Drive PID.mm.ki",
+    "type": "double",
+    "value": 0.0,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/Sim Drive PID.mm.kd",
+    "type": "double",
+    "value": 0.0,
+    "properties": {
+      "persistent": true
+    }
   }
 ]

networktables.json.bck

@@ -94,5 +94,37 @@
     "properties": {
       "persistent": true
     }
+  },
+  {
+    "name": "/Preferences/Drive/Module3/Drive Pid Property.mm.kp",
+    "type": "double",
+    "value": 0.05,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/Drive/Module3/Turn Pid Property.mm.kp",
+    "type": "double",
+    "value": 7.0,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/Drive/Module3/Turn Pid Property.mm.ki",
+    "type": "double",
+    "value": 0.0,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/Drive/Module3/Turn Pid Property.mm.kd",
+    "type": "double",
+    "value": 0.0,
+    "properties": {
+      "persistent": true
+    }
   }
 ]

src/main/java/frc/robot/Constants.java

@@ -64,7 +64,7 @@ private DriveConstants() {
             DRIVE_FF_GAINS,
             DRIVE_FB_GAINS,
             TURN_FB_GAINS,
-            Units.rotationsToDegrees(-0.017578), // offset 0.457764
+            Units.rotationsToDegrees(0.230713), // offset 0.457764
             true, // inversion
             ModuleConstants.GearRatios.L3
     );
@@ -75,7 +75,7 @@ private DriveConstants() {
             DRIVE_FF_GAINS,
             DRIVE_FB_GAINS,
             TURN_FB_GAINS,
-            Units.rotationsToDegrees(0.201172),
+            Units.rotationsToDegrees(0.203857),
             true,
             ModuleConstants.GearRatios.L3
     );
@@ -86,7 +86,7 @@ private DriveConstants() {
             DRIVE_FF_GAINS,
             DRIVE_FB_GAINS,
             TURN_FB_GAINS,
-            Units.rotationsToDegrees(0.457764),
+            Units.rotationsToDegrees(0.212158),
             true,
             ModuleConstants.GearRatios.L3
     );
@@ -97,7 +97,7 @@ private DriveConstants() {
             DRIVE_FF_GAINS,
             DRIVE_FB_GAINS,
             TURN_FB_GAINS,
-            Units.rotationsToDegrees(-0.263916),
+            Units.rotationsToDegrees(-0.263184),
             true,
             ModuleConstants.GearRatios.L3
     );

src/main/java/frc/robot/subsystems/drive/module/Module.java

@@ -13,8 +13,6 @@
 
 package frc.robot.subsystems.drive.module;
 
-import edu.wpi.first.math.controller.PIDController;
-import edu.wpi.first.math.controller.SimpleMotorFeedforward;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
@@ -28,12 +26,8 @@ public class Module {
 
   private final ModuleIO m_io;
   private final ModuleIOInputsAutoLogged m_inputs = new ModuleIOInputsAutoLogged();
-  private final ModuleConstants m_moduleConstants;
   private final int m_index;
 
-  private final SimpleMotorFeedforward m_driveFeedforward;
-  private final PIDController m_driveFeedback;
-  private final PIDController m_turnFeedback;
   private Rotation2d m_angleSetpoint = null; // Setpoint for closed loop control, null for open loop
   private Double m_speedSetpoint = null; // Setpoint for closed loop control, null for open loop
   private Rotation2d m_turnRelativeOffset = null; // Relative + Offset = Absolute
@@ -42,32 +36,11 @@ public class Module {
 
   public Module(ModuleIO io) {
     this.m_io = io;
-    this.m_moduleConstants = m_io.getModuleConstants();
-    this.m_index = m_moduleConstants.MODULE_INDEX();
+    ModuleConstants moduleConstants = m_io.getModuleConstants();
+    this.m_index = moduleConstants.MODULE_INDEX();
     // delay to initialize all hardware
     Timer.delay(0.5);
 
-    // Switch constants based on mode (the physics simulator is treated as a
-    // separate robot with different tuning)
-    switch (Constants.currentMode) {
-        case REAL, REPLAY -> {
-          m_driveFeedforward = new SimpleMotorFeedforward(0.1, 0.13);
-          m_driveFeedback = new PIDController(0.05, 0.0, 0.0);
-          m_turnFeedback = new PIDController(0.1, 0.0, 0.0);
-        }
-        case SIM -> {
-          m_driveFeedforward = new SimpleMotorFeedforward(0.0, 0.13);
-          m_driveFeedback = new PIDController(0.1, 0.0, 0.0);
-          m_turnFeedback = new PIDController(10.0, 0.0, 0.0);
-        }
-        default -> {
-          m_driveFeedforward = new SimpleMotorFeedforward(0.0, 0.0);
-          m_driveFeedback = new PIDController(0.0, 0.0, 0.0);
-          m_turnFeedback = new PIDController(0.0, 0.0, 0.0);
-        }
-      }
-
-    m_turnFeedback.enableContinuousInput(-Math.PI, Math.PI);
     setBrakeMode(true);
   }
 
@@ -90,23 +63,13 @@ public void periodic() {
 
     // Run closed loop turn control
     if (m_angleSetpoint != null) {
-      m_io.setTurnPositionDegs(m_angleSetpoint.minus(m_turnRelativeOffset).getRotations());
+      m_io.setTurnPositionRots(m_angleSetpoint.minus(m_turnRelativeOffset).getRotations());
 
       // Run closed loop drive control
       // Only allowed if closed loop turn control is running
       if (m_speedSetpoint != null) {
-        // Scale velocity based on turn error
-        //
-        // When the error is 90°, the velocity setpoint should be 0. As the wheel turns
-        // towards the setpoint, its velocity should increase. This is achieved by
-        // taking the component of the velocity in the direction of the setpoint.
-        double adjustSpeedSetpoint = m_speedSetpoint * Math.cos(m_turnFeedback.getPositionError());
-
         // Run drive controller
-        m_io.setDriveVelocityMPS(adjustSpeedSetpoint);
-//        m_io.setDriveVelocityMPS(
-//            m_driveFeedforward.calculate(velocityRadPerSec)
-//                + m_driveFeedback.calculate(m_inputs.getDriveVelocityRadPerSec(), velocityRadPerSec));
+        m_io.setDriveVelocityMPS(m_speedSetpoint);
       }
     }
 
@@ -149,7 +112,7 @@ public void runCharacterization(double volts) {
 
   /** Disables all outputs to motors. */
   public void stop() {
-    m_io.setTurnPositionDegs(0.0);
+    m_io.setTurnPositionRots(0.0);
     m_io.setDriveVelocityMPS(0.0);
 
     // Disable closed loop control for turn and drive

src/main/java/frc/robot/subsystems/drive/module/ModuleIO.java

@@ -129,7 +129,7 @@ default void updateInputs(ModuleIOInputs inputs) {}
   default void setDriveVelocityMPS(double mps) {}
 
   /** Run the turn motor at the specified voltage. */
-  default void setTurnPositionDegs(double degrees) {}
+  default void setTurnPositionRots(double degrees) {}
 
   /** Enable or disable brake mode on the drive motor. */
   default void setDriveBrakeMode(boolean enable) {}

src/main/java/frc/robot/subsystems/drive/module/ModuleIOSim.java

@@ -104,7 +104,7 @@ public void setDriveVelocityMPS(double mps) {
   }
 
   @Override
-  public void setTurnPositionDegs(double degrees) {
+  public void setTurnPositionRots(double degrees) {
     double rots = degrees / 360;
     double volts = m_turnPid.calculate(m_turnSim.getAngularPositionRotations(), rots);
     turnAppliedVolts = MathUtil.clamp(volts, -12.0, 12.0);

src/main/java/frc/robot/subsystems/drive/module/ModuleIOTalonFX.java

@@ -28,10 +28,7 @@
 import com.ctre.phoenix6.signals.SensorDirectionValue;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.util.Units;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import lib.properties.phoenix6.Phoenix6PidPropertyBuilder;
-import lib.properties.phoenix6.PidPropertyPublic;
-import org.littletonrobotics.junction.Logger;
 
 import java.util.Queue;
 
@@ -79,6 +76,7 @@ public class ModuleIOTalonFX implements ModuleIO {
   public ModuleIOTalonFX(ModuleConstants moduleConstants) {
     m_driveTalon = new TalonFX(moduleConstants.DRIVE_MOTOR_ID());
     m_turnTalon = new TalonFX(moduleConstants.TURN_MOTOR_ID());
+
     /*
     * this is technically the proper way of using any class that
     * implements the "Closeable" or "AutoClosable", typically things
@@ -87,8 +85,8 @@ public ModuleIOTalonFX(ModuleConstants moduleConstants) {
       // run factory default on cancoder
       var encoderConfig = new CANcoderConfiguration();
       encoderConfig.MagnetSensor.SensorDirection =
-              moduleConstants.ENCODER_INVERTED() ? SensorDirectionValue.Clockwise_Positive
-                      : SensorDirectionValue.CounterClockwise_Positive;
+          moduleConstants.ENCODER_INVERTED() ? SensorDirectionValue.Clockwise_Positive
+              : SensorDirectionValue.CounterClockwise_Positive;
       encoderConfig.MagnetSensor.AbsoluteSensorRange = AbsoluteSensorRangeValue.Signed_PlusMinusHalf;
       cancoder.getConfigurator().apply(encoderConfig);
 
@@ -142,6 +140,7 @@ public ModuleIOTalonFX(ModuleConstants moduleConstants) {
 
     m_posRequest = new PositionVoltage(0, 0, false, 0, 0, false, false, false);
 
+
     // Fancy multithreaded odometry update stuff
     // setup drive values
     m_driveTalon.setPosition(0.0);
@@ -205,9 +204,9 @@ public void updateInputs(ModuleIOInputs inputs) {
     m_turnPid.updateIfChanged();
 
     inputs.drivePositionRad =
-        Units.rotationsToRadians(m_drivePosition.getValueAsDouble()) / m_moduleConstants.DRIVE_GEAR_RATIO();
+        Units.rotationsToRadians(m_drivePosition.getValueAsDouble());
     inputs.driveVelocityRadPerSec =
-        Units.rotationsToRadians(m_driveVelocity.getValueAsDouble()) / m_moduleConstants.DRIVE_GEAR_RATIO();
+        Units.rotationsToRadians(m_driveVelocity.getValueAsDouble());
     inputs.driveAppliedVolts = m_driveAppliedVolts.getValueAsDouble();
     inputs.driveCurrentAmps = new double[] {m_driveCurrent.getValueAsDouble()};
 
@@ -216,28 +215,19 @@ public void updateInputs(ModuleIOInputs inputs) {
     inputs.setTurnPosition(Rotation2d.fromRotations((
             m_turnPosition.getValueAsDouble())));
     inputs.setTurnVelocityRadPerSec(
-            Units.rotationsToRadians(m_turnVelocity.getValueAsDouble()) / m_moduleConstants.TURNING_GEAR_RATIO());
+            Units.rotationsToRadians(m_turnVelocity.getValueAsDouble()));
     inputs.setTurnAppliedVolts(m_turnAppliedVolts.getValueAsDouble());
     inputs.setTurnCurrentAmps(new double[] {m_turnCurrent.getValueAsDouble()});
 
     inputs.setOdometryDrivePositionsRad(m_drivePositionQueue.stream()
-        .mapToDouble((Double value) -> Units.rotationsToRadians(value) / m_moduleConstants.DRIVE_GEAR_RATIO())
+        .mapToDouble(Units::rotationsToRadians)
         .toArray());
     inputs.setOdometryTurnPositions(m_turnPositionQueue.stream()
-        .map((Double value) -> Rotation2d.fromRotations(value / m_moduleConstants.DRIVE_GEAR_RATIO()))
+        .map(Rotation2d::fromRotations)
         .toArray(Rotation2d[]::new));
+
     m_drivePositionQueue.clear();
     m_turnPositionQueue.clear();
-
-    SmartDashboard.putNumber("Module " + m_moduleConstants.MODULE_INDEX() +
-            "/Closed Loop Output", m_turnClosedLoopOutput.getValueAsDouble());
-    SmartDashboard.putNumber("Module " + m_moduleConstants.MODULE_INDEX() +
-            "/Closed Loop Error", m_turnClosedLoopError.getValueAsDouble());
-    SmartDashboard.putNumber("Module " + m_moduleConstants.MODULE_INDEX() +
-            "/Closed Loop Reference", m_turnClosedLoopReference.getValueAsDouble());
-
-    SmartDashboard.putNumber("Module " + m_moduleConstants.MODULE_INDEX() +
-            "/Recorded kP", m_turnPid.getSlotConfigs().kP);
   }
 
   @Override
@@ -248,9 +238,8 @@ public void setDriveVelocityMPS(double mps) {
   }
 
   @Override
-  public void setTurnPositionDegs(double degrees) {
-//    double rots = degrees / 360;
-    m_turnTalon.setControl(m_posRequest.withPosition(degrees));
+  public void setTurnPositionRots(double rotations) {
+    m_turnTalon.setControl(m_posRequest.withPosition(rotations));
   }
 
   @Override

src/main/java/frc/robot/subsystems/drive/module/PhoenixOdometryThread.java

@@ -95,6 +95,7 @@ public void run() {
         }
       } catch (InterruptedException e) {
         DriverStation.reportError(e.toString(), true);
+        Thread.currentThread().interrupt();
       } finally {
         signalsLock.unlock();
       }

src/main/java/lib/properties/phoenix6/Phoenix6PidPropertyBuilder.java

@@ -6,7 +6,6 @@
 import com.ctre.phoenix6.configs.SlotConfigs;
 import com.ctre.phoenix6.hardware.core.CoreTalonFX;
 import com.ctre.phoenix6.signals.GravityTypeValue;
-import com.gos.lib.properties.BaseHeavyProperty;
 import com.gos.lib.properties.GosDoubleProperty;
 import com.gos.lib.properties.HeavyDoubleProperty;
 
@@ -15,7 +14,7 @@
 import java.util.function.DoubleConsumer;
 
 public final class Phoenix6PidPropertyBuilder {
-   private final CoreTalonFX m_motor;
+  private final CoreTalonFX m_motor;
   private final SlotConfigs m_slot;
   private final boolean m_isConstant;
   private final String m_baseName;
@@ -42,7 +41,9 @@ public Phoenix6PidPropertyBuilder(String baseName, boolean isConstant, CoreTalon
     }
 }
 
-  private HeavyDoubleProperty createDoubleProperty(String propertyNameSuffix, double defaultValue, DoubleConsumer setter) {
+  private HeavyDoubleProperty createDoubleProperty(String propertyNameSuffix,
+                                                   double defaultValue,
+                                                   DoubleConsumer setter) {
     String propertyName = m_baseName + ".mm." + propertyNameSuffix;
     GosDoubleProperty prop = new GosDoubleProperty(m_isConstant, propertyName, defaultValue);