From 1f683d0ab4f54a1dee7b79427fb92d7e8fa1ec4b Mon Sep 17 00:00:00 2001 From: wilsonwatson Date: Mon, 1 Sep 2025 09:49:46 -0500 Subject: [PATCH 1/6] characterization procedures --- .../frc/lib/util/swerve/SwerveModule.java | 17 ++ src/main/java/frc/robot/Constants.java | 9 + .../frc/robot/subsystems/swerve/Swerve.java | 162 +++++++++++++++++- 3 files changed, 186 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/lib/util/swerve/SwerveModule.java b/src/main/java/frc/lib/util/swerve/SwerveModule.java index e5ca3b02..9dde760f 100644 --- a/src/main/java/frc/lib/util/swerve/SwerveModule.java +++ b/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,19 @@ public SwerveModulePosition getPosition() { Constants.Swerve.wheelCircumference), Rotation2d.fromRotations(inputs.angleMotorSelectedPosition.in(Rotations))); } + + public void runCharacterization(double output) { + io.setDriveMotor(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); + } } diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 3f608a8d..6a0fc87d 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -152,6 +152,15 @@ public static final class Climb { * Swerve Constants */ public static final class Swerve { + + 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; diff --git a/src/main/java/frc/robot/subsystems/swerve/Swerve.java b/src/main/java/frc/robot/subsystems/swerve/Swerve.java index 2487f81d..4c82375a 100644 --- a/src/main/java/frc/robot/subsystems/swerve/Swerve.java +++ b/src/main/java/frc/robot/subsystems/swerve/Swerve.java @@ -1,7 +1,12 @@ package frc.robot.subsystems.swerve; +import static edu.wpi.first.units.Units.Meters; import static edu.wpi.first.units.Units.Rotation; +import java.text.DecimalFormat; +import java.text.NumberFormat; +import java.util.LinkedList; +import java.util.List; import java.util.Optional; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; @@ -10,6 +15,7 @@ import edu.wpi.first.math.controller.HolonomicDriveController; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.math.filter.SlewRateLimiter; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; @@ -18,11 +24,14 @@ import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; @@ -325,8 +334,6 @@ public Command teleOpDrive(CommandXboxController controller, boolean fieldRelati }); } - - public Command stop() { return this.runOnce(this::setMotorsZero); } @@ -391,4 +398,155 @@ public void moveToPose(Pose2d pose) { Constants.SwerveTransformPID.MAX_ACCELERATION); } + private void runCharacterization(double output) { + for (int i = 0; i < 4; i++) { + swerveMods[i].runCharacterization(output); + } + } + + private double getFFCharacterizationVelocity() { + double output = 0.0; + for (int i = 0; i < 4; i++) { + output += swerveMods[i].getFFCharacterizationVelocity() / 4.0; + } + return output; + } + + private double[] getWheelRadiusCharacterizationPositions() { + double[] values = new double[4]; + for (int i = 0; i < 4; i++) { + values[i] = swerveMods[i].getWheelRadiusCharacterizationPosition(); + } + return values; + } + + public Command feedforwardCharacterization() { + List velocitySamples = new LinkedList<>(); + List voltageSamples = new LinkedList<>(); + Timer timer = new Timer(); + + return Commands.sequence( + // Reset data + Commands.runOnce(() -> { + velocitySamples.clear(); + voltageSamples.clear(); + }), + + // Allow modules to orient + Commands.run(() -> this.runCharacterization(0.0), this) + .withTimeout(Constants.Swerve.Characterization.ffStartDelay), + + // Start timer + Commands.runOnce(timer::restart), + + // Accelerate and gather data + Commands.run(() -> { + double voltage = timer.get() * Constants.Swerve.Characterization.ffRampRate; + this.runCharacterization(voltage); + velocitySamples.add(this.getFFCharacterizationVelocity()); + voltageSamples.add(voltage); + }, this) + + // When cancelled, calculate and print results + .finallyDo(() -> { + int n = velocitySamples.size(); + double sumX = 0.0; + double sumY = 0.0; + double sumXY = 0.0; + double sumX2 = 0.0; + for (int i = 0; i < n; i++) { + sumX += velocitySamples.get(i); + sumY += voltageSamples.get(i); + sumXY += velocitySamples.get(i) * voltageSamples.get(i); + sumX2 += velocitySamples.get(i) * velocitySamples.get(i); + } + double kS = (sumY * sumX2 - sumX * sumXY) / (n * sumX2 - sumX * sumX); + double kV = (n * sumXY - sumX * sumY) / (n * sumX2 - sumX * sumX); + + NumberFormat formatter = new DecimalFormat("#0.00000"); + System.out.println("********** Drive FF Characterization Results **********"); + System.out.println("\tkS: " + formatter.format(kS)); + System.out.println("\tkV: " + formatter.format(kV)); + })); + } + + public Command wheelRadiusCharacterization() { + SlewRateLimiter limiter = + new SlewRateLimiter(Constants.Swerve.Characterization.wheelRadiusRampRate); + WheelRadiusCharacterizationState state = new WheelRadiusCharacterizationState(); + + double driveBaseRadius = Math.hypot(Constants.Swerve.bumperFront.in(Meters), + Constants.Swerve.bumperRight.in(Meters)); + + return Commands.parallel( + // Drive control sequence + Commands.sequence( + // Reset acceleration limiter + Commands.runOnce(() -> limiter.reset(0.0)), + + // Turn in place, accelerating up to full speed + Commands.run(() -> { + double speed = + limiter.calculate(Constants.Swerve.Characterization.wheelRadiusMaxVelocity); + this.setModuleStates(new ChassisSpeeds(0.0, 0.0, speed)); + }, this)), + + // Measurement sequence + Commands.sequence( + // Wait for modules to fully orient before starting measurement + Commands.waitSeconds(1.0), + + // Record starting measurement + Commands.runOnce(() -> { + state.positions = this.getWheelRadiusCharacterizationPositions(); + state.lastAngle = this.getGyroYaw(); + state.gyroDelta = 0.0; + }), + + // Update gyro delta + Commands.run(() -> { + var rotation = this.getGyroYaw(); + state.gyroDelta += Math.abs(rotation.minus(state.lastAngle).getRadians()); + state.lastAngle = rotation; + + double[] positions = this.getWheelRadiusCharacterizationPositions(); + double wheelDelta = 0.0; + for (int i = 0; i < 4; i++) { + wheelDelta += Math.abs(positions[i] - state.positions[i]) / 4.0; + } + double wheelRadius = (state.gyroDelta * driveBaseRadius) / wheelDelta; + + Logger.recordOutput("Drive/WheelDelta", wheelDelta); + Logger.recordOutput("Drive/WheelRadius", wheelRadius); + }) + + // When cancelled, calculate and print results + .finallyDo(() -> { + double[] positions = this.getWheelRadiusCharacterizationPositions(); + double wheelDelta = 0.0; + for (int i = 0; i < 4; i++) { + wheelDelta += Math.abs(positions[i] - state.positions[i]) / 4.0; + } + double wheelRadius = (state.gyroDelta * driveBaseRadius) / wheelDelta; + + NumberFormat formatter = + new DecimalFormat("#0.000000000000000000000000000"); + System.out + .println("********** Wheel Radius Characterization Results **********"); + System.out + .println("\tWheel Delta: " + formatter.format(wheelDelta) + " radians"); + System.out.println( + "\tGyro Delta: " + formatter.format(state.gyroDelta) + " radians"); + System.out.println( + "\tWheel Radius: " + formatter.format(wheelRadius) + " meters, " + + formatter.format(Units.metersToInches(wheelRadius)) + " inches"); + }))); + } + + private static class WheelRadiusCharacterizationState { + double[] positions = new double[4]; + Rotation2d lastAngle = Rotation2d.kZero; + double gyroDelta = 0.0; + } + } From d512e0be55d124e38199b86464944ee347278bf2 Mon Sep 17 00:00:00 2001 From: wilsonwatson Date: Mon, 1 Sep 2025 09:51:24 -0500 Subject: [PATCH 2/6] pit controller for sysid procedures --- src/main/java/frc/robot/RobotContainer.java | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 114bfb11..c7d11b44 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -332,6 +332,9 @@ private void setupPitController() { .onFalse(wrist.runVolts(() -> 0.0)); // driver.a().whileTrue( // elevator.moveTo(() -> Inches.of(SmartDashboard.getNumber("elevatorTargetHeight", 20)))); + + pitController.povDown().whileTrue(swerve.feedforwardCharacterization()); + pitController.povUp().whileTrue(swerve.wheelRadiusCharacterization()); } private void configureTriggerBindings() { From f9ca1ac8782c1660c5a67e03da746901ef0bd169 Mon Sep 17 00:00:00 2001 From: Wilson Watson Date: Tue, 2 Sep 2025 10:05:31 -0500 Subject: [PATCH 3/6] safety --- src/main/java/frc/robot/RobotContainer.java | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c7d11b44..2c766a93 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -333,8 +333,10 @@ private void setupPitController() { // driver.a().whileTrue( // elevator.moveTo(() -> Inches.of(SmartDashboard.getNumber("elevatorTargetHeight", 20)))); - pitController.povDown().whileTrue(swerve.feedforwardCharacterization()); - pitController.povUp().whileTrue(swerve.wheelRadiusCharacterization()); + pitController.povDown().whileTrue(swerve.feedforwardCharacterization()) + .onFalse(swerve.stop()); + pitController.povUp().whileTrue(swerve.wheelRadiusCharacterization()) + .onFalse(swerve.stop()); } private void configureTriggerBindings() { From b334ee0bf48b42ba6c8b0741c0fb24c517fffb45 Mon Sep 17 00:00:00 2001 From: Wilson Watson Date: Tue, 2 Sep 2025 10:11:14 -0500 Subject: [PATCH 4/6] fixes and set up for testing --- src/main/java/frc/lib/util/swerve/SwerveModule.java | 3 ++- src/main/java/frc/robot/Constants.java | 1 + src/main/java/frc/robot/subsystems/swerve/Swerve.java | 5 +++++ 3 files changed, 8 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/lib/util/swerve/SwerveModule.java b/src/main/java/frc/lib/util/swerve/SwerveModule.java index 9dde760f..ac7db468 100644 --- a/src/main/java/frc/lib/util/swerve/SwerveModule.java +++ b/src/main/java/frc/lib/util/swerve/SwerveModule.java @@ -112,8 +112,9 @@ public SwerveModulePosition getPosition() { Rotation2d.fromRotations(inputs.angleMotorSelectedPosition.in(Rotations))); } + /** Run motor at given voltage with angle motor pointing forward */ public void runCharacterization(double output) { - io.setDriveMotor(output); + io.setDriveMotorPower(output); io.setAngleMotor(0.0); } diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 6a0fc87d..708353c4 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -153,6 +153,7 @@ public static final class Climb { */ 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 diff --git a/src/main/java/frc/robot/subsystems/swerve/Swerve.java b/src/main/java/frc/robot/subsystems/swerve/Swerve.java index 4c82375a..e19ee800 100644 --- a/src/main/java/frc/robot/subsystems/swerve/Swerve.java +++ b/src/main/java/frc/robot/subsystems/swerve/Swerve.java @@ -420,6 +420,7 @@ private double[] getWheelRadiusCharacterizationPositions() { return values; } + /** Sysid routine to determine kS and kV */ public Command feedforwardCharacterization() { List velocitySamples = new LinkedList<>(); List voltageSamples = new LinkedList<>(); @@ -467,9 +468,12 @@ public Command feedforwardCharacterization() { System.out.println("********** Drive FF Characterization Results **********"); System.out.println("\tkS: " + formatter.format(kS)); System.out.println("\tkV: " + formatter.format(kV)); + Logger.recordOutput("Sysid/FF/kS", kS); + Logger.recordOutput("Sysid/FF/kV", kV); })); } + /** Sysid routine to determine wheel radius */ public Command wheelRadiusCharacterization() { SlewRateLimiter limiter = new SlewRateLimiter(Constants.Swerve.Characterization.wheelRadiusRampRate); @@ -540,6 +544,7 @@ public Command wheelRadiusCharacterization() { System.out.println( "\tWheel Radius: " + formatter.format(wheelRadius) + " meters, " + formatter.format(Units.metersToInches(wheelRadius)) + " inches"); + Logger.recordOutput("Sysid/FF/radius", wheelRadius); }))); } From 6a1e10ef66e3608bf8444d6d5a8a09c5d6379d35 Mon Sep 17 00:00:00 2001 From: Reed Date: Tue, 2 Sep 2025 18:29:15 -0500 Subject: [PATCH 5/6] camera changes and sysid --- .../java/frc/lib/math/SeparatingAxis.java | 3 ++ src/main/java/frc/robot/Constants.java | 8 +-- src/main/java/frc/robot/RobotContainer.java | 8 +++ src/main/java/frc/robot/RobotState.java | 49 +++++++++++-------- 4 files changed, 44 insertions(+), 24 deletions(-) diff --git a/src/main/java/frc/lib/math/SeparatingAxis.java b/src/main/java/frc/lib/math/SeparatingAxis.java index 235cce2d..f037c272 100644 --- a/src/main/java/frc/lib/math/SeparatingAxis.java +++ b/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(); } diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 708353c4..eb05ab67 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -277,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); @@ -294,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; @@ -444,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; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 2c766a93..cb51f2dc 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/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; @@ -337,6 +341,10 @@ private void setupPitController() { .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() { diff --git a/src/main/java/frc/robot/RobotState.java b/src/main/java/frc/robot/RobotState.java index 3e992b3d..26ad6cbc 100755 --- a/src/main/java/frc/robot/RobotState.java +++ b/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 baseUncertainty, List targets, String prefix, @@ -157,18 +158,18 @@ 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.inchesToMeters(48)) { 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 maybeRobotYaw = sampleRotationAt(result.getTimestampSeconds()); Rotation2d robotYaw; if (maybeRobotYaw.isPresent()) { @@ -177,11 +178,15 @@ public void addVisionObservation(PhotonPipelineResult result, Transform3d robotT continue; } Rotation2d yaw = Rotation2d.fromDegrees(robotYaw.getDegrees() - target.getYaw() - + 180 + Units.radiansToDegrees(robotToCamera.getRotation().getZ())); - xCircle.setCenter(localCircle.getVertex(yaw)); - xCircle.draw(); + // * Math.cos(Constants.Vision.cameras[whichCamera].robotToCamera().getX()) + + 180 + // - Constants.Vision.cameras[whichCamera].robotToCamera().getX() * 180.0 / + // Math.PI + + Units.radiansToDegrees(robotToCamera.getRotation().getZ())); + 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); @@ -190,15 +195,17 @@ public void addVisionObservation(PhotonPipelineResult result, Transform3d robotT VecBuilder.fill(Constants.StateEstimator.localVisionTrust.getAsDouble(), Constants.StateEstimator.localVisionTrust.getAsDouble(), 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 +219,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]; From 6c01d7c9d2994aabbb64edf20d44fa2235af6f26 Mon Sep 17 00:00:00 2001 From: Wilson Watson Date: Thu, 4 Sep 2025 09:57:06 -0500 Subject: [PATCH 6/6] some more attempts at better vision --- src/main/java/frc/robot/RobotState.java | 23 +++++++++++------- .../frc/robot/commands/MoveAndAvoidReef.java | 24 +++++++++++++++++-- 2 files changed, 36 insertions(+), 11 deletions(-) diff --git a/src/main/java/frc/robot/RobotState.java b/src/main/java/frc/robot/RobotState.java index 26ad6cbc..76564ded 100755 --- a/src/main/java/frc/robot/RobotState.java +++ b/src/main/java/frc/robot/RobotState.java @@ -162,7 +162,7 @@ public void addVisionObservation(PhotonPipelineResult result, Transform3d robotT for (var target : result.targets) { double dist = target.getBestCameraToTarget().getTranslation().toTranslation2d().getNorm(); - if (dist > Units.inchesToMeters(48)) { + if (dist > Units.feetToMeters(6)) { continue; } localCircle[whichCamera] @@ -177,12 +177,14 @@ public void addVisionObservation(PhotonPipelineResult result, Transform3d robotT } else { continue; } - Rotation2d yaw = Rotation2d.fromDegrees(robotYaw.getDegrees() - target.getYaw() - // * Math.cos(Constants.Vision.cameras[whichCamera].robotToCamera().getX()) - + 180 - // - Constants.Vision.cameras[whichCamera].robotToCamera().getX() * 180.0 / - // Math.PI - + Units.radiansToDegrees(robotToCamera.getRotation().getZ())); + 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( @@ -192,8 +194,11 @@ public void addVisionObservation(PhotonPipelineResult result, Transform3d robotT 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[whichCamera]); } diff --git a/src/main/java/frc/robot/commands/MoveAndAvoidReef.java b/src/main/java/frc/robot/commands/MoveAndAvoidReef.java index 8510a92d..a8145f68 100755 --- a/src/main/java/frc/robot/commands/MoveAndAvoidReef.java +++ b/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