Feature/swerve tuning

src/main/deploy/pathplanner/settings.json

@@ -28,7 +28,7 @@
     "BAD AUTONS"
   ],
   "defaultMaxVel": 3.0,
-  "defaultMaxAccel": 3.0,
+  "defaultMaxAccel": 5.0,
   "defaultMaxAngVel": 540.0,
   "defaultMaxAngAccel": 720.0,
   "defaultNominalVoltage": 12.0,

src/main/java/frc/robot/constants/RobotConstants.java

@@ -49,6 +49,7 @@ public class RobotTypes {
         intakeSubsystem).addAprilTagCamera(CameraConstants.primeLeftCamera)
             .addAprilTagCamera(CameraConstants.primeRightCamera)
             .addAprilTagCamera(CameraConstants.primeBackCamera);
+
     public static final RobotType deux26 = new RobotType("Deux26", driveSubsystem); // , kickerSubsystem,
     // spindexerSubsystem, intakeSubsystem, intakeRollerSubsystem, turretSubsystem,
     // shooterSubsystem,
@@ -189,7 +190,7 @@ public static class WarningThresholdConstants {
 
   public class AutopilotConstants {
     private static final APConstraints kConstraints = new APConstraints()
-        .withAcceleration(DriveConstants.accelLimit).withVelocity(DriveConstants.maxSpeed).withJerk(2.0);
+        .withAcceleration(DriveConstants.moduleAccelLimit).withVelocity(DriveConstants.maxSpeed).withJerk(2.0);
 
     private static final APProfile kProfile = new APProfile(kConstraints).withErrorXY(Centimeters.of(2))
         .withErrorTheta(Degrees.of(0.5)).withBeelineRadius(Centimeters.of(8));

src/main/java/frc/robot/constants/RobotPIDConstants.java

@@ -24,7 +24,7 @@ public class RobotPIDConstants {
   public static final PIDConstants drivePid = new PIDConstants(0.17189, 0.0, 0);
   public static final FeedForwardConstants driveFF = new FeedForwardConstants(0.12506, 2, 0.27879);
   public static final PIDConstants steerPid = new PIDConstants(0.725, 0.0, 0.005);
-  // TODO compare results across modules, run one by one
+  // TODO deux
   public static final FeedForwardConstants[] steerFFs = new FeedForwardConstants[]{
       new FeedForwardConstants(0.43147, 0.46232, 0.072917), new FeedForwardConstants(0.52027, 0.44734, 0.048812),
       new FeedForwardConstants(0.33446, 0.44561, 0.071012), new FeedForwardConstants(0.53387, 0.46702, 0.063417)};

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

@@ -1,5 +1,7 @@
 package frc.robot.subsystems.drive;
 
+import static frc.robot.subsystems.drive.DriveConstants.useTunedSwerve;
+
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
 import edu.wpi.first.math.util.Units;
@@ -43,15 +45,26 @@ public static enum PivotId {
   public static final double maxSpeed = 4.25;
   public static final double maxNorm = DriveSubsystem.computeMaxNorm(DriveConstants.positions, new Translation2d());
   public static final double maxOmega = (maxSpeed / maxNorm);
-  public static final double accelLimit = 20;
-  public static final double deaccelLimit = 11;
+  public static final double maxTranslationalDecel = 10;
+  public static final double maxRotationalDecel = Double.POSITIVE_INFINITY;
+
+  public static final double moduleAccelLimit = 20;
+  public static final double moduleDecelLimit = 11;
+
+  public static final double steerSetpointContinuityDeltaRadPerSec = Double.POSITIVE_INFINITY; // (Math.PI / 8) /
+  // 0.02;
+  public static final boolean useTunedSwerve = true;
+
+  public static final boolean useSteerVelocitySetpoint = useTunedSwerve;
+
+  public static final double defaultDreamLevel = DriveConstants.useTunedSwerve ? 1 : 3;
 
   public static final double wheelRadius = Units.inchesToMeters(2);
 
   public static final double initalSlope = 3.125;
   public static final double finalSlope = 4.375;
 
-  public static final double maxSteerSpeed = 50; // rad per second
+  public static final double maxSteerSpeed = Double.POSITIVE_INFINITY; // rad per second
 
   public static final SwerveDriveKinematics kinematics = new SwerveDriveKinematics(frontLeftLocation,
       frontRightLocation, backLeftLocation, backRightLocation);

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

@@ -123,7 +123,7 @@ public void configureBLine() {
     FollowPath.setTranslationListLoggingConsumer((pair) -> Logger.recordOutput(pair.getFirst(), pair.getSecond()));
 
     this.pathBuilder = new FollowPath.Builder((SubsystemBase) this, () -> RobotOdometry.instance.getPose("Main"),
-        this::getChassisSpeeds, (speeds) -> runVelocity(speeds, false, 3, () -> false),
+        this::getChassisSpeeds, (speeds) -> runVelocity(speeds, false, () -> false),
         new PIDController(5.0, 0.0, 2.0), new PIDController(6, 0.0, 1.3), new PIDController(4, 0.0, 1))
             .withDefaultShouldFlip().withPoseReset((pose) -> {
               CommandScheduler.getInstance().schedule(new InstantCommand(() -> {
@@ -220,6 +220,10 @@ public SwerveModulePosition[] getModulePositions() {
     return states;
   }
 
+  public void runVelocity(ChassisSpeeds speeds, boolean fieldCentric, BooleanSupplier limitSpeeds) {
+    runVelocity(speeds, fieldCentric, DriveConstants.defaultDreamLevel, limitSpeeds);
+  }
+
   public void runVelocity(ChassisSpeeds speeds, boolean fieldCentric, double dreamLevel,
       BooleanSupplier limitSpeeds) {
 
@@ -241,7 +245,14 @@ public void runVelocity(ChassisSpeeds speeds, boolean fieldCentric, double dream
             doubleCone.vyMetersPerSecond * DriveConstants.maxSpeed * scale,
             doubleCone.omegaRadiansPerSecond * DriveConstants.maxOmega * scale);
 
-    previousSetpoint = setpointGenerator.generateSetpoint(previousSetpoint, speedsOptimized, 0.02);
+    previousSetpoint = setpointGenerator.generateSetpoint(previousSetpoint, // The previous setpoint
+        speedsOptimized, // The desired target speeds
+        0.02 // The loop time of the robot code, in seconds
+    );
+    // replace above w/ below to bypass generator
+    // previousSetpoint = new SwerveSetpoint(speedsOptimized,
+    // DriveConstants.kinematics.toSwerveModuleStates(speedsOptimized),
+    // DriveFeedforwards.zeros(4));
 
     Logger.recordOutput("Drive/SwerveStates/SetpointStates", previousSetpoint.moduleStates());
 
@@ -250,6 +261,9 @@ public void runVelocity(ChassisSpeeds speeds, boolean fieldCentric, double dream
     Logger.recordOutput("Drive/SwerveStates/DoubleCone",
         DriveConstants.kinematics.toSwerveModuleStates(speedsOptimized));
 
+    Logger.recordOutput("Drive/SwerveStates/InputStates", DriveConstants.kinematics.toSwerveModuleStates(speeds));
+    // speedsOptimized = ChassisSpeeds.discretize(speedsOptimized, 0.02);
+
     SwerveModuleState[] directStates = DriveConstants.kinematics.toSwerveModuleStates(speedsOptimized);
 
     Logger.recordOutput("Drive/SwerveStates/DirectStates", directStates);
@@ -342,11 +356,11 @@ public Command sysIdDynamicSteer(SysIdRoutine.Direction direction) {
   }
 
   public Command runVelocityCommand(Supplier<ChassisSpeeds> speeds, BooleanSupplier limitSpeeds) {
-    return new RunCommand(() -> runVelocity(speeds.get(), true, 3, limitSpeeds), this).finallyDo(() -> stop());
+    return new RunCommand(() -> runVelocity(speeds.get(), true, limitSpeeds), this).finallyDo(() -> stop());
   }
 
   public Consumer<ChassisSpeeds> runVelocityConsumer() {
-    return (speeds) -> runVelocity(speeds, true, 3, () -> false);
+    return (speeds) -> runVelocity(speeds, true, () -> false);
   }
 
   public static ModuleIO getIOByMode(ModuleInfo modInfo) {

src/main/java/frc/robot/subsystems/drive/weights/JoystickDriveWeight.java

@@ -68,8 +68,8 @@ public ChassisSpeeds getSpeeds() {
       linearVelocity = linearVelocity.div(linearVelocity.getNorm());
     }
     omega = MathUtil.clamp(omega, -1, 1);
-    double xyMult = 0.90;
-    double omegaMult = 0.7;
+    double xyMult = DriveConstants.useTunedSwerve ? 1 : 0.9;
+    double omegaMult = DriveConstants.useTunedSwerve ? 1 : 0.7;
     if (slowMode.getAsBoolean()) {
       xyMult = 0.3;
       omegaMult = 0.4;

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

@@ -20,8 +20,8 @@ public class Module {
   PivotId id;
   ModuleIOInputsAutoLogged inputs = new ModuleIOInputsAutoLogged();
 
-  SlewRateLimiter accelLimiter = new SlewRateLimiter(DriveConstants.accelLimit);
-  SlewRateLimiter deaccelLimiter = new SlewRateLimiter(DriveConstants.deaccelLimit);
+  SlewRateLimiter accelLimiter = new SlewRateLimiter(DriveConstants.moduleAccelLimit);
+  SlewRateLimiter deaccelLimiter = new SlewRateLimiter(DriveConstants.moduleDecelLimit);
 
   public Module(ModuleIO io, PivotId id) {
     this.io = io;
@@ -48,10 +48,13 @@ public Module(ModuleIO io, PivotId id) {
   public void periodic() {
     io.updateInputs(inputs);
     Logger.processInputs("Drive/Modules/" + id, inputs);
-    Logger.recordOutput("Subsystems/Module/" + id + "/setpointVel", io.velocitySetpoint(), Units.MetersPerSecond);
+    Logger.recordOutput("Subsystems/Module/" + id + "/setpointVel", io.driveVelocitySetpoint(),
+        Units.MetersPerSecond);
     Logger.recordOutput("Subsystems/Module/" + id + "/currentVel", getVelocity(), Units.MetersPerSecond);
     Logger.recordOutput("Subsystems/Module/" + id + "/velocityError",
-        Math.abs(io.velocitySetpoint() - getVelocity()), Units.MetersPerSecond);
+        Math.abs(io.driveVelocitySetpoint() - getVelocity()), Units.MetersPerSecond);
+    Logger.recordOutput("Subsystems/Module/" + id + "/steerSetpointVel", io.steerVelocitySetpoint(),
+        Units.RadiansPerSecond);
   }
 
   public void setDesiredStateMetersPerSecond(SwerveModuleState state) {

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

@@ -52,7 +52,11 @@ public default void setSteerPosition(Rotation2d angle, ModuleIOInputs inputs) {
   default void close() {
   }
 
-  public default double velocitySetpoint() {
+  public default double driveVelocitySetpoint() {
+    return 0.0;
+  }
+
+  public default double steerVelocitySetpoint() {
     return 0.0;
   }
 

src/main/java/frc/robot/subsystems/module/ModuleIOReal.java

@@ -2,6 +2,8 @@
 
 import java.util.Queue;
 
+import org.littletonrobotics.junction.Logger;
+
 import com.revrobotics.RelativeEncoder;
 import com.revrobotics.spark.SparkFlex;
 import com.revrobotics.spark.SparkMax;
@@ -10,15 +12,19 @@
 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.units.Units;
 import frc.robot.constants.RobotPIDConstants;
 import frc.robot.sensors.odometry.SparkOdometryThread;
 import frc.robot.sensors.resolvers.ResolverPWM;
 import frc.robot.subsystems.drive.DriveConstants;
 import frc.robot.util.spark.SparkConfigurer;
 import frc.robot.util.spark.SparkConstants;
 
+import static frc.robot.subsystems.drive.DriveConstants.useSteerVelocitySetpoint;
+
 public class ModuleIOReal implements ModuleIO {
-  private double velocitySetpoint = 0;
+  private double driveVelocitySetpoint = 0;
+  private double steerVelocitySetpoint = 0;
   private final Queue<Double> timestampQueue;
   private final Queue<Double> drivePositionQueue;
   private final Queue<Double> turnPositionQueue;
@@ -36,16 +42,23 @@ public class ModuleIOReal implements ModuleIO {
   private final PIDController steerPID;
   private final SimpleMotorFeedforward steerFF;
 
+  private final double maxSteerRateRadiansPerSecond;
+
+  private Rotation2d lastSteerSetpoint = new Rotation2d();
+  private int lastSteerSetpointCounter = 0;
+
+  private final String logPath;
+
   public ModuleIOReal(ModuleInfo id) {
     drivePID = RobotPIDConstants.constructPID(RobotPIDConstants.drivePid, "drivePID" + id.id.toString());
     driveFF = RobotPIDConstants.constructFFSimpleMotor(RobotPIDConstants.driveFF, "driveFF" + id.id.toString());
     steerPID = RobotPIDConstants.constructPID(RobotPIDConstants.steerPid, "steerPID" + id.id.toString());
+    steerFF = RobotPIDConstants.constructFFSimpleMotor(id.steerFF);
     driveSpark = SparkConfigurer.configSparkFlex(id.driveID, SparkConstants.driveConfig);
     steerSpark = SparkConfigurer.configSparkMax(id.steerID, SparkConstants.steerConfig);
     timestampQueue = SparkOdometryThread.getInstance().makeTimestampQueue();
     drivePositionQueue = SparkOdometryThread.getInstance().registerSignal(driveSpark,
         () -> driveSpark.getEncoder().getPosition());
-    steerFF = RobotPIDConstants.constructFFSimpleMotor(id.steerFF);
     driveEncoder = driveSpark.getEncoder();
     steeringEncoder = new ResolverPWM(id.resolverChannel, id.angleOffset);
     // steeringEncoder =
@@ -61,6 +74,9 @@ public ModuleIOReal(ModuleInfo id) {
 
     turnPositionQueue = SparkOdometryThread.getInstance().registerSignal(steerSpark,
         () -> steeringEncoder.getDegrees() % 360);
+
+    logPath = "Subsystems/Module/" + id.id.toString();
+    maxSteerRateRadiansPerSecond = 10 / steerFF.getKv();
   }
 
   @Override
@@ -105,14 +121,16 @@ public void updateInputs(ModuleIOInputs inputs) {
     driveVelocityQueue.clear();
 
     // inputs.rawEncoderValue = steeringEncoder.getRawValue();
+
+    lastSteerSetpointCounter = Math.max(0, lastSteerSetpointCounter - 1);
   }
 
   @Override
   public void setDriveVelocity(double velocity, ModuleIOInputs inputs) {
     double pidSpeed = driveFF.calculate(velocity);
     pidSpeed += drivePID.calculate(inputs.driveVelocityMetersPerSecond, velocity);
     setDriveVoltage(pidSpeed);
-    velocitySetpoint = velocity;
+    driveVelocitySetpoint = velocity;
   }
 
   @Override
@@ -124,7 +142,39 @@ public void setDriveVoltage(double voltage) {
   public void setSteerPosition(Rotation2d angle, ModuleIOInputs inputs) {
     Rotation2d delta = angle.minus(Rotation2d.fromDegrees(inputs.steerAngleDegrees));
     double sin = Math.sin(delta.getRadians());
-    setSteerVoltage(steerPID.calculate(sin, 0) * 6);
+    double pidVoltage = steerPID.calculate(sin, 0) * 6;
+
+    Logger.recordOutput(logPath + "/errorRadians",
+        (Math.abs(inputs.steerAngleDegrees - angle.getDegrees()) % 180) * Math.PI / 180);
+
+    // calculate setpoint velocity using last input
+    double setpointDerivative;
+    if (!isLastSteerSetpointValid() || !useSteerVelocitySetpoint)
+      setpointDerivative = 0; // do not set velocity if estimate is stale
+    else { // if last setpoint is fresh (last robot loop), approximate derivative of
+        // setpoint
+      setpointDerivative = angle.minus(lastSteerSetpoint).getRadians() / 0.02;
+    } // clamp velocity setpoint
+    Logger.recordOutput(logPath + "/steerSetpointDerivative", setpointDerivative, Units.RadiansPerSecond);
+
+    double angularVelocityRadPerSec;
+    // force velocity setpoint to zero if setpoint discontinuous
+    if (MathUtil.isNear(0, setpointDerivative, DriveConstants.steerSetpointContinuityDeltaRadPerSec))
+      angularVelocityRadPerSec = MathUtil.clamp(setpointDerivative, -maxSteerRateRadiansPerSecond,
+          maxSteerRateRadiansPerSecond);
+    else
+      angularVelocityRadPerSec = 0;
+    angularVelocityRadPerSec = Math.abs(angularVelocityRadPerSec) * Math.max(0, Math.signum(pidVoltage));
+
+    steerVelocitySetpoint = angularVelocityRadPerSec;
+    // update last setpoint
+    updateLastSteerSetpoint(angle);
+
+    // compute output voltage
+    double ffVoltage = steerFF.calculate(angularVelocityRadPerSec);
+    Logger.recordOutput(logPath + "/steerPIDVoltage", pidVoltage);
+    Logger.recordOutput(logPath + "/steerFFVoltage", ffVoltage);
+    setSteerVoltage(ffVoltage + pidVoltage);
   }
 
   @Override
@@ -133,7 +183,21 @@ public void setSteerVoltage(double voltage) {
   }
 
   @Override
-  public double velocitySetpoint() {
-    return velocitySetpoint;
+  public double driveVelocitySetpoint() {
+    return driveVelocitySetpoint;
+  }
+
+  @Override
+  public double steerVelocitySetpoint() {
+    return steerVelocitySetpoint;
+  }
+
+  private void updateLastSteerSetpoint(Rotation2d lastSteerSetpoint) {
+    this.lastSteerSetpoint = lastSteerSetpoint;
+    lastSteerSetpointCounter = 2;
+  }
+
+  private boolean isLastSteerSetpointValid() {
+    return lastSteerSetpointCounter > 0;
   }
 }