Swerve SysID

src/main/java/frc/lib/math/SeparatingAxis.java

@@ -93,6 +93,9 @@ public static boolean solve(ConvexShape shape1, ConvexShape shape2, Penetration
         Translation2d c1 = shape1.getCenter();
         Translation2d c2 = shape2.getCenter();
         Translation2d cToc = c1.minus(c2);
+        if (n == null) {
+            return false;
+        }
         if (n.dot(cToc) < 0) {
             n = n.unaryMinus();
         }

src/main/java/frc/lib/util/swerve/SwerveModule.java

@@ -1,6 +1,8 @@
 package frc.lib.util.swerve;
 
+import static edu.wpi.first.units.Units.Radians;
 import static edu.wpi.first.units.Units.Rotations;
+import static edu.wpi.first.units.Units.RotationsPerSecond;
 import org.littletonrobotics.junction.Logger;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
@@ -109,4 +111,20 @@ public SwerveModulePosition getPosition() {
                 Constants.Swerve.wheelCircumference),
             Rotation2d.fromRotations(inputs.angleMotorSelectedPosition.in(Rotations)));
     }
+
+    /** Run motor at given voltage with angle motor pointing forward */
+    public void runCharacterization(double output) {
+        io.setDriveMotorPower(output);
+        io.setAngleMotor(0.0);
+    }
+
+    /** Returns the module position in radians. */
+    public double getWheelRadiusCharacterizationPosition() {
+        return inputs.driveMotorSelectedPosition.in(Radians);
+    }
+
+    /** Returns the module velocity in rotations/sec (Phoenix native units). */
+    public double getFFCharacterizationVelocity() {
+        return inputs.driveMotorSelectedSensorVelocity.in(RotationsPerSecond);
+    }
 }

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

@@ -152,6 +152,16 @@ public static final class Climb {
      * Swerve Constants
      */
     public static final class Swerve {
+
+        /** Constants for sysid routines */
+        public static final class Characterization {
+            public static final double ffStartDelay = 2.0; // Secs
+            public static final double ffRampRate = 0.1; // Volts/Sec
+            public static final double wheelRadiusMaxVelocity = 0.25; // Rad/Sec
+            public static final double wheelRadiusRampRate = 0.05; // Rad/Sec^2
+        }
+
+
         public static final double AUTO_ROTATION_KP = 5.0;
         public static final double AUTO_ROTATION_KI = 0.0;
         public static final double AUTO_ROTATION_KD = 0.0;
@@ -267,7 +277,7 @@ public static class ModuleConstants {
             public static final double wheelCoeffFriction = 1.2;
             public static final MomentOfInertia angleMomentOfInertia =
                 KilogramSquareMeters.of(0.02);
-            public static final Distance wheelRadius = Inches.of(3.87 / 2);
+            public static final Distance wheelRadius = Inches.of(2.764);
             public static final Current slipCurrent = Amps.of(120.0);
 
             public static final Current driveCurrentLimit = Amps.of(35.0);
@@ -284,8 +294,8 @@ public static class ModuleConstants {
             public static final LinearAcceleration maxDriveRate = MetersPerSecondPerSecond.of(50.0);
 
 
-            public static final double ffkS = 1.0;
-            public static final double ffkV = 1.51;
+            public static final double ffkS = .194;
+            public static final double ffkV = 1.001;
             public static final double ffkT = 1.0 / driveMotor.KtNMPerAmp;
             public static final double ffkA = 0.27;
             public static final double drivekP = 0.12;
@@ -434,7 +444,7 @@ public static final class CoralScoringConstants {
      * MoveToPos constants.
      */
     public static class SwerveTransformPID {
-        public static final double PID_XKP = 1.0;
+        public static final double PID_XKP = 4.2;
         public static final double PID_XKI = 0.0;
         public static final double PID_XKD = 0.0;
         public static final double PID_TKP = 2.5;

src/main/java/frc/robot/RobotContainer.java

@@ -1,6 +1,8 @@
 package frc.robot;
 
 import static edu.wpi.first.units.Units.Degrees;
+import static edu.wpi.first.units.Units.Inches;
+import static edu.wpi.first.units.Units.Meters;
 import static edu.wpi.first.units.Units.Radians;
 import java.util.ArrayList;
 import java.util.HashSet;
@@ -13,6 +15,7 @@
 import choreo.auto.AutoFactory;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Transform2d;
 import edu.wpi.first.wpilibj.AddressableLED;
 import edu.wpi.first.wpilibj.AddressableLEDBuffer;
 import edu.wpi.first.wpilibj.DriverStation;
@@ -32,6 +35,7 @@
 import frc.lib.util.viz.FieldViz;
 import frc.lib.util.viz.Viz2025;
 import frc.robot.Robot.RobotRunType;
+import frc.robot.commands.MoveToPose;
 import frc.robot.subsystems.LEDs;
 import frc.robot.subsystems.algaewrist.AlgaeWrist;
 import frc.robot.subsystems.algaewrist.AlgaeWristIO;
@@ -332,6 +336,15 @@ private void setupPitController() {
             .onFalse(wrist.runVolts(() -> 0.0));
         // driver.a().whileTrue(
         // elevator.moveTo(() -> Inches.of(SmartDashboard.getNumber("elevatorTargetHeight", 20))));
+
+        pitController.povDown().whileTrue(swerve.feedforwardCharacterization())
+            .onFalse(swerve.stop());
+        pitController.povUp().whileTrue(swerve.wheelRadiusCharacterization())
+            .onFalse(swerve.stop());
+        pitController.povLeft().whileTrue(new MoveToPose(swerve, () -> {
+            return swerve.getPose()
+                .plus(new Transform2d(Inches.of(24), Meters.of(0), Rotation2d.kZero));
+        }, () -> 200.0, false, 0, 0)).onFalse(swerve.stop());
     }
 
     private void configureTriggerBindings() {

src/main/java/frc/robot/RobotState.java

@@ -96,8 +96,9 @@ public Pose2d getGlobalPoseEstimate() {
 
     private final Circle stdDevGlobalCircle =
         new Circle("State/GlobalEstimateStdDev", new Translation2d(), 0);
-    private final Circle stdDevLocalCircle =
-        new Circle("State/LocalEstimateStdDev", new Translation2d(), 0);
+    private final Circle[] stdDevLocalCircle =
+        new Circle[] {new Circle("State/LocalEstimateStdDev0", new Translation2d(), 0),
+            new Circle("State/LocalEstimateStdDev1", new Translation2d(), 0)};
 
     private void addVisionObservation(Pose3d cameraPose, Pose3d robotPose, double timestamp,
         Vector<N3> baseUncertainty, List<PhotonTrackedTarget> targets, String prefix,
@@ -157,48 +158,59 @@ public void addVisionObservation(PhotonPipelineResult result, Transform3d robotT
                     Constants.StateEstimator.globalVisionTrust.getAsDouble(),
                     Constants.StateEstimator.globalVisionTrustRotation.getAsDouble()),
                 result.getTargets(), "Global", true, stdDevGlobalCircle);
-        }
-        if (whichCamera == 1) {
+        } else {
             for (var target : result.targets) {
                 double dist =
                     target.getBestCameraToTarget().getTranslation().toTranslation2d().getNorm();
-                if (dist > Units.inchesToMeters(36)) {
+                if (dist > Units.feetToMeters(6)) {
                     continue;
                 }
-                localCircle.setRadius(dist - Constants.Vision.cameras[whichCamera].offset());
-                localCircle.setCenter(Constants.Vision.fieldLayout.getTagPose(target.fiducialId)
-                    .get().getTranslation().toTranslation2d());
-                localCircle.draw();
+                localCircle[whichCamera]
+                    .setRadius(dist - Constants.Vision.cameras[whichCamera].offset());
+                localCircle[whichCamera].setCenter(Constants.Vision.fieldLayout
+                    .getTagPose(target.fiducialId).get().getTranslation().toTranslation2d());
+                localCircle[whichCamera].draw();
                 Optional<Rotation2d> maybeRobotYaw = sampleRotationAt(result.getTimestampSeconds());
                 Rotation2d robotYaw;
                 if (maybeRobotYaw.isPresent()) {
                     robotYaw = maybeRobotYaw.get();
                 } else {
                     continue;
                 }
-                Rotation2d yaw = Rotation2d.fromDegrees(robotYaw.getDegrees() - target.getYaw()
-                    + 180 + Units.radiansToDegrees(robotToCamera.getRotation().getZ()));
-                xCircle.setCenter(localCircle.getVertex(yaw));
-                xCircle.draw();
+                Rotation2d yaw;
+                if (whichCamera == 0) {
+                    yaw = Rotation2d.fromDegrees(robotYaw.getDegrees() + target.getYaw()
+                        + Units.radiansToDegrees(robotToCamera.getRotation().getZ()) + 180);
+                } else {
+                    yaw = Rotation2d.fromDegrees(robotYaw.getDegrees() - target.getYaw()
+                        + Units.radiansToDegrees(robotToCamera.getRotation().getZ()) + 180);
+                }
+                xCircle[whichCamera].setCenter(localCircle[whichCamera].getVertex(yaw));
+                xCircle[whichCamera].draw();
                 Pose2d robotPose2d = new Pose2d(
-                    xCircle.getCenter()
+                    xCircle[whichCamera].getCenter()
                         .minus(robotToCamera.getTranslation().toTranslation2d().rotateBy(robotYaw)),
                     robotYaw);
                 Pose3d robotPose = new Pose3d(robotPose2d);
                 Pose3d cameraPose = robotPose.plus(robotToCamera);
                 addVisionObservation(cameraPose, robotPose, result.getTimestampSeconds(),
-                    VecBuilder.fill(Constants.StateEstimator.localVisionTrust.getAsDouble(),
-                        Constants.StateEstimator.localVisionTrust.getAsDouble(),
+                    VecBuilder.fill(
+                        Constants.StateEstimator.localVisionTrust.getAsDouble()
+                            * Math.max(dist, 0.5),
+                        Constants.StateEstimator.localVisionTrust.getAsDouble()
+                            * Math.max(dist, 0.5),
                         Double.POSITIVE_INFINITY),
-                    result.getTargets(), "Local", false, stdDevLocalCircle);
+                    result.getTargets(), "Local", false, stdDevLocalCircle[whichCamera]);
             }
         }
     }
 
-    private final Circle localCircle =
-        new Circle("State/LocalEstimationDistance", new Translation2d(), 0);
-    private final Circle xCircle =
-        new Circle("State/LocalEstimationPose", new Translation2d(), Units.inchesToMeters(2));
+    private final Circle[] localCircle =
+        new Circle[] {new Circle("State/LocalEstimationDistance0", new Translation2d(), 0),
+            new Circle("State/LocalEstimationDistance1", new Translation2d(), 0)};
+    private final Circle[] xCircle = new Circle[] {
+        new Circle("State/LocalEstimationPose0", new Translation2d(), Units.inchesToMeters(2)),
+        new Circle("State/LocalEstimationPose1", new Translation2d(), Units.inchesToMeters(2))};
 
     /**
      * Add information from swerve drive.
@@ -212,8 +224,10 @@ public void addSwerveObservation(SwerveModulePosition[] positions, Rotation2d gy
             Stream.of(positions).map(x -> x.angle).toArray(this::swerveRotationsArray));
         stdDevGlobalCircle.setCenter(getGlobalPoseEstimate().getTranslation());
         stdDevGlobalCircle.setRadius(stdDevGlobalCircle.getRadius() + 0.01);
-        stdDevLocalCircle.setCenter(new Translation2d());
-        stdDevLocalCircle.setRadius(0.0);
+        stdDevLocalCircle[0].setCenter(new Translation2d());
+        stdDevLocalCircle[0].setRadius(0.0);
+        stdDevLocalCircle[1].setCenter(new Translation2d());
+        stdDevLocalCircle[1].setRadius(0.0);
     }
 
     private Rotation2d[] swerveRotations = new Rotation2d[4];

src/main/java/frc/robot/commands/MoveAndAvoidReef.java

@@ -10,6 +10,7 @@
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Transform2d;
 import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.event.EventLoop;
 import edu.wpi.first.wpilibj2.command.Command;
@@ -124,13 +125,32 @@ public void initialize() {
         if (flipForRed) {
             pose2d = AllianceFlipUtil.apply(pose2d);
         }
+        prev = pose2d;
     }
 
+    private final TrapezoidProfile profile = new TrapezoidProfile(new TrapezoidProfile.Constraints(
+        Constants.SwerveTransformPID.MAX_VELOCITY, Constants.SwerveTransformPID.MAX_ACCELERATION));
+
+    private Pose2d prev;
+
     @Override
     public void execute() {
-        // draw();
+        double speed = pose2d.minus(prev).getTranslation().getNorm();
+        double dist = swerve.state.getGlobalPoseEstimate().getTranslation()
+            .minus(pose2d.getTranslation()).getNorm();
+        double maxSpeed = Math.abs(profile.calculate(0.02, new TrapezoidProfile.State(dist, speed),
+            new TrapezoidProfile.State(0, 0)).velocity);
+        double nominalSpeed = maxSpeedSupplier.getAsDouble();
+        if (maxSpeed > speed) {
+            maxSpeed = nominalSpeed;
+        }
+        Logger.recordOutput("MoveAndAvoidReef/speed", speed);
+        Logger.recordOutput("MoveAndAvoidReef/dist", dist);
+        Logger.recordOutput("MoveAndAvoidReef/maxSpeed", maxSpeed);
+        Logger.recordOutput("MoveAndAvoidReef/nominalSpeed", nominalSpeed);
+
         swerve.moveToPose(getNextIntermediateTarget(swerve.state.getGlobalPoseEstimate(), pose2d),
-            maxSpeedSupplier.getAsDouble());
+            Math.min(nominalSpeed, maxSpeed));
     }
 
     @Override