Merge branch 'advantagekit-swerve' into advantagekit-swerve-adjustments-#1

Author: Bloodhatam

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

@@ -17,7 +17,7 @@
 import edu.wpi.first.math.system.plant.DCMotor;
 
 public class DriveConstants {
-  public static final double maxSpeedMetersPerSec = 9.72;
+  public static final double maxSpeedMetersPerSec = 4.5;
   public static final double odometryFrequency = 100.0; // Hz
   public static final double trackWidth = 69.5; // w metry
   public static final double wheelBase = 69.5; // w metry

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

@@ -12,6 +12,7 @@
 import static frc.robot.subsystems.drive.DriveConstants.*;
 import static frc.robot.util.SparkUtil.*;
 
+import com.revrobotics.AbsoluteEncoder;
 import com.revrobotics.PersistMode;
 import com.revrobotics.RelativeEncoder;
 import com.revrobotics.ResetMode;
@@ -26,9 +27,10 @@
 import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
 import com.revrobotics.spark.config.SparkMaxConfig;
 import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.filter.Debouncer;
 import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.wpilibj.AnalogEncoder;
+import frc.robot.util.AbsoluteAnalogEncoder;
 import java.util.Queue;
 import java.util.function.DoubleSupplier;
 
@@ -50,6 +52,8 @@ public class ModuleIOSpark implements ModuleIO {
   // Closed loop controllers
   private final SparkClosedLoopController driveController;
   private final SparkClosedLoopController turnController;
+  // RIO-side PID for turn control (we use the RoboRIO analog absolute encoder)
+  private final PIDController turnPid;
 
   // Queue inputs from odometry thread
   private final Queue<Double> timestampQueue;
@@ -107,6 +111,12 @@ public ModuleIOSpark(int module) {
     driveController = driveSpark.getClosedLoopController();
     turnController = turnSpark.getClosedLoopController();
 
+    // RIO-side PID controller for the turn motor. We use the RoboRIO-mounted
+    // analog absolute encoder (turnEncoder) as the measurement and command
+    // voltages to the SPARK via setVoltage(...).
+    turnPid = new PIDController(turnKp, 0.0, turnKd);
+    turnPid.enableContinuousInput(turnPIDMinInput, turnPIDMaxInput);
+
     // Configure drive motor
     var driveConfig = new SparkMaxConfig();
     driveConfig
@@ -157,8 +167,7 @@ public ModuleIOSpark(int module) {
         .closedLoop
         .feedbackSensor(FeedbackSensor.kPrimaryEncoder)
         .positionWrappingEnabled(true)
-        .positionWrappingInputRange(turnPIDMinInput, turnPIDMaxInput)
-        .pid(turnKp, 0.0, turnKd);
+        .positionWrappingInputRange(turnPIDMinInput, turnPIDMaxInput);
     turnConfig
         .signals
         .primaryEncoderPositionAlwaysOn(true)
@@ -183,6 +192,10 @@ public ModuleIOSpark(int module) {
     timestampQueue = SparkOdometryThread.getInstance().makeTimestampQueue();
     drivePositionQueue =
         SparkOdometryThread.getInstance().registerSignal(driveSpark, driveEncoder::getPosition);
+    // The turn encoder is an external RoboRIO-mounted analog encoder (not a Spark signal).
+    // Register it as a generic DoubleSupplier so the odometry thread samples it directly
+    // 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(turnSpark, turnEncoder::getPosition);
     //// MODIFIED
@@ -213,7 +226,8 @@ public void updateInputs(ModuleIOInputs inputs) {
         new DoubleSupplier[] {turnSpark::getAppliedOutput, turnSpark::getBusVoltage},
         (values) -> inputs.turnAppliedVolts = values[0] * values[1]);
     ifOk(turnSpark, turnSpark::getOutputCurrent, (value) -> inputs.turnCurrentAmps = value);
-    inputs.turnConnected = turnConnectedDebounce.calculate(!sparkStickyFault);
+    // Use the analog-read success as our connected indicator for turn.
+    inputs.turnConnected = turnConnectedDebounce.calculate(turnReadOk);
 
     // Update odometry inputs
     inputs.odometryTimestamps =
@@ -255,6 +269,14 @@ public void setTurnPosition(Rotation2d rotation) {
     double setpoint =
         MathUtil.inputModulus(
             rotation.plus(zeroRotation).getRadians(), turnPIDMinInput, turnPIDMaxInput);
-    turnController.setSetpoint(setpoint, ControlType.kPosition);
+
+    // Read the RoboRIO-mounted analog absolute encoder and compute PID output
+    double current = turnEncoder.getPosition();
+    double volts = turnPid.calculate(current, setpoint);
+
+    // Clamp to allowable voltage range
+    volts = MathUtil.clamp(volts, -12.0, 12.0);
+
+    turnSpark.setVoltage(volts);
   }
 }

src/main/java/frc/robot/util/AbsoluteAnalogEncoder.java

@@ -0,0 +1,51 @@
+package frc.robot.util;
+
+import static edu.wpi.first.units.Units.Degrees;
+import static edu.wpi.first.units.Units.Microseconds;
+import static edu.wpi.first.units.Units.Radian;
+import static edu.wpi.first.units.Units.Seconds;
+
+import com.revrobotics.AbsoluteEncoder;
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.wpilibj.AnalogEncoder;
+import org.littletonrobotics.junction.Logger;
+
+public class AbsoluteAnalogEncoder implements AbsoluteEncoder {
+  AnalogEncoder encoder;
+
+  double lastReading;
+  double cachedVelocity;
+  long lastTimestamp;
+
+  public AbsoluteAnalogEncoder(int channel) {
+    encoder = new AnalogEncoder(channel);
+
+    lastTimestamp = Logger.getTimestamp();
+    lastReading = getPosition();
+  }
+
+  public double getRaw() {
+    return encoder.get();
+  }
+
+  public double getPosition() {
+    return Radian.convertFrom(this.getRaw() * 360, Degrees);
+  }
+
+  public double getVelocity() {
+    return calculateVelocity(getPosition(), Logger.getTimestamp());
+  }
+
+  public double calculateVelocity(double currentPositionRad, long currentTimestampMicros) {
+    double deltaRad = MathUtil.angleModulus(currentPositionRad - lastReading);
+
+    double deltaTimeSec = Seconds.convertFrom(currentTimestampMicros - lastTimestamp, Microseconds);
+
+    double velocityRadPerSec = deltaRad / deltaTimeSec;
+
+    lastReading = currentPositionRad;
+    lastTimestamp = currentTimestampMicros;
+
+    return velocityRadPerSec;
+  }
+}