Merge pull request #1 from SpiceGears/advantagekit-swerve-adjustments-#1

[FIXED, NEEDS TEST] Swerve drive final config- merge into advantagekit-swerve branch

Author: Bloodhatam

src/main/java/frc/robot/Robot.java

@@ -47,7 +47,7 @@ public Robot() {
     switch (Constants.currentMode) {
       case REAL:
         // Running on a real robot, log to a USB stick ("/U/logs")
-        Logger.addDataReceiver(new WPILOGWriter());
+        // Logger.addDataReceiver(new WPILOGWriter());
         Logger.addDataReceiver(new NT4Publisher());
         break;
 

src/main/java/frc/robot/subsystems/drive/DriveConstants.java

@@ -31,10 +31,10 @@ public class DriveConstants {
       };
 
   // Zeroed rotation values for each module, see setup instructions
-  public static final Rotation2d frontLeftZeroRotation = new Rotation2d(0.0);
-  public static final Rotation2d frontRightZeroRotation = new Rotation2d(0.0);
-  public static final Rotation2d backLeftZeroRotation = new Rotation2d(0.0);
-  public static final Rotation2d backRightZeroRotation = new Rotation2d(0.0);
+  public static final Rotation2d frontLeftZeroRotation = new Rotation2d(-0.532);
+  public static final Rotation2d frontRightZeroRotation = new Rotation2d(1.275);
+  public static final Rotation2d backLeftZeroRotation = new Rotation2d(-0.451);
+  public static final Rotation2d backRightZeroRotation = new Rotation2d(1.968);
 
   // Device CAN IDs
   public static final int pigeonCanId = 11;
@@ -49,6 +49,11 @@ public class DriveConstants {
   public static final int frontRightTurnCanId = 5;
   public static final int backRightTurnCanId = 7;
 
+  public static final int frontLeftAbsoluteEncoderChannel = 0;
+  public static final int frontRightAbsoluteEncoderChannel = 1;
+  public static final int backLeftAbsoluteEncoderChannel = 3;
+  public static final int backRightAbsoluteEncoderChannel = 2;
+
   // Drive motor configuration
   public static final int driveMotorCurrentLimit = 40;
   public static final double wheelRadiusMeters = Meter.convertFrom(5, Centimeter);
@@ -77,20 +82,22 @@ public class DriveConstants {
   public static final double driveSimKv = 0.0789;
 
   // Turn motor configuration
-  public static final boolean turnInverted = false;
+  public static final boolean turnInverted = true;
   public static final int turnMotorCurrentLimit = 20;
   public static final double turnMotorReduction = /*9424.0 / 203.0*/ 21.42;
   public static final DCMotor turnGearbox = DCMotor.getNEO(1);
 
   // Turn encoder configuration
-  public static final boolean turnEncoderInverted = true;
-  public static final double turnEncoderPositionFactor = 1;
-  // 2 * Math.PI; // Rotations -> Radians
+  public static final boolean turnEncoderInverted = false;
+  public static final double turnEncoderPositionFactor =
+      2
+          * Math.PI
+          / turnMotorReduction; // Rotations (0-1) * 360deg to convert 0-1 range to 0-360 -> Radians
   public static final double turnEncoderVelocityFactor =
-      1; // (2 * Math.PI) / 60.0; // RPM -> Rad/Sec
+      (2 * Math.PI) / 60.0 / turnMotorReduction; // RPM -> Rad/Sec
 
   // Turn PID configuration
-  public static final double turnKp = 0.1;
+  public static final double turnKp = 0.2;
   public static final double turnKd = 0.0;
   public static final double turnSimP = 8.0;
   public static final double turnSimD = 0.0;

src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java

@@ -7,6 +7,8 @@
 
 package frc.robot.subsystems.drive;
 
+import static edu.wpi.first.units.Units.Degree;
+import static edu.wpi.first.units.Units.Radians;
 import static frc.robot.subsystems.drive.DriveConstants.*;
 import static frc.robot.util.SparkUtil.*;
 
@@ -43,7 +45,9 @@ public class ModuleIOSpark implements ModuleIO {
   private final SparkBase driveSpark;
   private final SparkBase turnSpark;
   private final RelativeEncoder driveEncoder;
-  private final AbsoluteEncoder turnEncoder;
+  private final RelativeEncoder turnEncoder;
+
+  private final AnalogEncoder absoluteEncoder; // MODIFIED
 
   // Closed loop controllers
   private final SparkClosedLoopController driveController;
@@ -92,27 +96,17 @@ public ModuleIOSpark(int module) {
             },
             MotorType.kBrushless);
     driveEncoder = driveSpark.getEncoder();
-    // turnEncoder = turnSpark.getAbsoluteEncoder();
-
-    // # BEGIN CUSTOMIZED CODE:
-    switch (module) {
-      case 0:
-        turnEncoder = new AbsoluteAnalogEncoder(0);
-        break;
-      case 1:
-        turnEncoder = new AbsoluteAnalogEncoder(1);
-        break;
-      case 2:
-        turnEncoder = new AbsoluteAnalogEncoder(3);
-        break;
-      case 3:
-        turnEncoder = new AbsoluteAnalogEncoder(2);
-        break;
-      default:
-        turnEncoder = new AbsoluteAnalogEncoder(5);
-    }
+    turnEncoder = turnSpark.getEncoder();
 
-    // # END CUSTOMIZED CODE.
+    absoluteEncoder =
+        new AnalogEncoder(
+            switch (module) {
+              case 0 -> frontLeftAbsoluteEncoderChannel;
+              case 1 -> frontRightAbsoluteEncoderChannel;
+              case 2 -> backLeftAbsoluteEncoderChannel;
+              case 3 -> backRightAbsoluteEncoderChannel;
+              default -> 0;
+            });
 
     driveController = driveSpark.getClosedLoopController();
     turnController = turnSpark.getClosedLoopController();
@@ -164,25 +158,22 @@ public ModuleIOSpark(int module) {
         .smartCurrentLimit(turnMotorCurrentLimit)
         .voltageCompensation(12.0);
     turnConfig
-        .absoluteEncoder
-        .inverted(turnEncoderInverted)
+        .encoder
+        // .inverted(turnEncoderInverted)
         .positionConversionFactor(turnEncoderPositionFactor)
         .velocityConversionFactor(turnEncoderVelocityFactor)
-        .averageDepth(2);
-    // We do NOT attach the RoboRIO analog absolute encoder to the SPARK as a
-    // closed-loop sensor (the analog encoder is wired to the RoboRIO). Instead
-    // we run the turn PID on the RoboRIO using WPILib's PIDController and
-    // command voltages to the SPARK. Configure only the SPARK parameters here.
+        .uvwAverageDepth(2);
     turnConfig
         .closedLoop
+        .feedbackSensor(FeedbackSensor.kPrimaryEncoder)
         .positionWrappingEnabled(true)
         .positionWrappingInputRange(turnPIDMinInput, turnPIDMaxInput);
     turnConfig
         .signals
-        .absoluteEncoderPositionAlwaysOn(true)
-        .absoluteEncoderPositionPeriodMs((int) (1000.0 / odometryFrequency))
-        .absoluteEncoderVelocityAlwaysOn(true)
-        .absoluteEncoderVelocityPeriodMs(20)
+        .primaryEncoderPositionAlwaysOn(true)
+        .primaryEncoderPositionPeriodMs((int) (1000.0 / odometryFrequency))
+        .primaryEncoderVelocityAlwaysOn(true)
+        .primaryEncoderVelocityPeriodMs(20)
         .appliedOutputPeriodMs(20)
         .busVoltagePeriodMs(20)
         .outputCurrentPeriodMs(20);
@@ -193,6 +184,10 @@ public ModuleIOSpark(int module) {
             turnSpark.configure(
                 turnConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters));
 
+    tryUntilOk(
+        turnSpark,
+        5,
+        () -> turnEncoder.setPosition(Radians.convertFrom(absoluteEncoder.get() * 360, Degree)));
     // Create odometry queues
     timestampQueue = SparkOdometryThread.getInstance().makeTimestampQueue();
     drivePositionQueue =
@@ -202,7 +197,7 @@ public ModuleIOSpark(int module) {
     // instead of going through the Spark-specific sampling/error path which can drop
     // or mis-time samples for a non-Spark sensor and produce jitter in AdvantageScope.
     turnPositionQueue =
-        SparkOdometryThread.getInstance().registerSignal(() -> turnEncoder.getPosition());
+        SparkOdometryThread.getInstance().registerSignal(turnSpark, turnEncoder::getPosition);
     //// MODIFIED
   }
 
@@ -220,30 +215,12 @@ public void updateInputs(ModuleIOInputs inputs) {
     inputs.driveConnected = driveConnectedDebounce.calculate(!sparkStickyFault);
 
     // Update turn inputs
-    // Read the external RoboRIO-mounted analog absolute encoder directly. Avoid using the
-    // Spark-registered sampling path for this signal because the analog encoder is not
-    // attached to the Spark and mixing the two sampling paths can produce missing/erratic
-    // samples in AdvantageScope.
-    boolean turnReadOk = true;
-    double turnPos = 0.0;
-    double turnVel = 0.0;
-    try {
-      turnPos = turnEncoder.getPosition();
-      turnVel = turnEncoder.getVelocity();
-    } catch (Exception e) {
-      // If reading fails, mark read as not-ok and leave defaults.
-      turnReadOk = false;
-    }
-    if (turnReadOk) {
-      inputs.turnPosition = new Rotation2d(turnPos).minus(zeroRotation);
-      inputs.turnVelocityRadPerSec = turnVel;
-    } else {
-      // fallback: keep velocity zero if we couldn't read the analog encoder.
-      inputs.turnVelocityRadPerSec = 0.0;
-    }
-
-    // Still read Spark-side telemetry (applied output, bus voltage, current) via the
-    // Spark-safe helpers.
+    sparkStickyFault = false;
+    ifOk(
+        turnSpark,
+        turnEncoder::getPosition,
+        (value) -> inputs.turnPosition = new Rotation2d(value).minus(zeroRotation));
+    ifOk(turnSpark, turnEncoder::getVelocity, (value) -> inputs.turnVelocityRadPerSec = value);
     ifOk(
         turnSpark,
         new DoubleSupplier[] {turnSpark::getAppliedOutput, turnSpark::getBusVoltage},