From 2bcc96bfbc72ab889083b8eac27c8957ad4f8363 Mon Sep 17 00:00:00 2001 From: joelzoto <118407587+joelzoto@users.noreply.github.com> Date: Tue, 4 Mar 2025 20:55:05 -0500 Subject: [PATCH] attempt at some VIsion changes --- .../drivetrain/CommandSwerveDrivetrain.java | 3 + .../frc/robot/drivetrain/TunerConstants.java | 9 +- .../robot/vision/SwerveVisionEstimator.java | 121 +++++++++--------- 3 files changed, 75 insertions(+), 58 deletions(-) diff --git a/src/main/java/frc/robot/drivetrain/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/drivetrain/CommandSwerveDrivetrain.java index 8384bdc..454cbeb 100644 --- a/src/main/java/frc/robot/drivetrain/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/drivetrain/CommandSwerveDrivetrain.java @@ -6,6 +6,7 @@ import com.ctre.phoenix6.SignalLogger; import com.ctre.phoenix6.Utils; +import com.ctre.phoenix6.hardware.Pigeon2; import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants; import com.ctre.phoenix6.swerve.SwerveModuleConstants; import com.ctre.phoenix6.swerve.SwerveRequest; @@ -36,6 +37,8 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su public static CommandSwerveDrivetrain mInstance; + public static Pigeon2 mGyro = new Pigeon2(TunerConstants.kPigeonId, "Canivore"); + /* Blue alliance sees forward as 0 degrees (toward red alliance wall) */ private static final Rotation2d kBlueAlliancePerspectiveRotation = Rotation2d.kZero; /* Red alliance sees forward as 180 degrees (toward blue alliance wall) */ diff --git a/src/main/java/frc/robot/drivetrain/TunerConstants.java b/src/main/java/frc/robot/drivetrain/TunerConstants.java index 308c0a2..655c4af 100644 --- a/src/main/java/frc/robot/drivetrain/TunerConstants.java +++ b/src/main/java/frc/robot/drivetrain/TunerConstants.java @@ -2,6 +2,8 @@ import static edu.wpi.first.units.Units.*; +import java.util.function.Supplier; + import com.ctre.phoenix6.CANBus; import com.ctre.phoenix6.configs.*; import com.ctre.phoenix6.hardware.*; @@ -10,11 +12,15 @@ import com.ctre.phoenix6.swerve.SwerveModuleConstants.*; import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; import edu.wpi.first.units.measure.*; import edu.wpi.first.wpilibj.motorcontrol.Talon; import frc.robot.drivetrain.CommandSwerveDrivetrain; +import frc.robot.vision.Vision; +import frc.robot.vision.VisionConfig; + import com.ctre.phoenix6.swerve.SwerveDrivetrain.DeviceConstructor.*; // Generated by the Tuner X Swerve Project Generator @@ -99,7 +105,8 @@ public class TunerConstants { private static final boolean kInvertLeftSide = false; private static final boolean kInvertRightSide = true; - private static final int kPigeonId = 18; + public static final int kPigeonId = 18; + // These are only used for simulation private static final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(0.01); diff --git a/src/main/java/frc/robot/vision/SwerveVisionEstimator.java b/src/main/java/frc/robot/vision/SwerveVisionEstimator.java index e0d08e9..03b3bd2 100644 --- a/src/main/java/frc/robot/vision/SwerveVisionEstimator.java +++ b/src/main/java/frc/robot/vision/SwerveVisionEstimator.java @@ -6,6 +6,8 @@ import java.util.Arrays; import java.util.function.Supplier; +import com.ctre.phoenix6.swerve.SwerveRequest; + import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -13,77 +15,82 @@ import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.drivetrain.CommandSwerveDrivetrain; public class SwerveVisionEstimator extends SubsystemBase{ - // private final Vision[] visions; - // private static SwerveVisionEstimator mInstance; - // private final Supplier modSupplier; - // private final Supplier rotationSupplier; - // private final SwerveDrivePoseEstimator poseEstimator; - // private final Field2d field2d = new Field2d(); + private final Vision[] visions; + private static SwerveVisionEstimator mInstance; + private final Supplier modSupplier; + private final Supplier rotationSupplier; + private final SwerveDrivePoseEstimator poseEstimator; + private final Field2d field2d = new Field2d(); + - // public SwerveVisionEstimator( - // Supplier rotSup, Supplier modSup){ - // this.modSupplier = modSup; - // this.rotationSupplier = rotSup; + public SwerveVisionEstimator( + Supplier rotSup, Supplier modSup){ + this.modSupplier = modSup; + this.rotationSupplier = rotSup; - // poseEstimator = new SwerveDrivePoseEstimator( - // DriveConstants.swerveKinematics, - // rotationSupplier.get(), - // modSupplier.get(), - // new Pose2d()); + poseEstimator = new SwerveDrivePoseEstimator( + //DriveConstants.swerveKinematics + null, + rotationSupplier.get(), + modSupplier.get(), + new Pose2d()); - // visions = new Vision[camNames.length]; - // for (int i = 0; i < camNames.length; i++){ - // visions[i] = new Vision(camNames[i], robotToCamTransforms[i]); - // } - // } + visions = new Vision[camNames.length]; + for (int i = 0; i < camNames.length; i++){ + visions[i] = new Vision(camNames[i], robotToCamTransforms[i]); + } + } - // public static SwerveVisionEstimator getInstance(){ - // if (mInstance == null){ - // final var drivetrain = Drivetrain.getInstance(); - // mInstance = new SwerveVisionEstimator(drivetrain::getGyroYaw, drivetrain::getModulePositions); - // } - // return mInstance; - // } + public static SwerveVisionEstimator getInstance(){ + if (mInstance == null){ + final var drivetrain = CommandSwerveDrivetrain.getInstance(); + //mInstance = new SwerveVisionEstimator(drivetrain::getGyroYaw, drivetrain::getModulePositions); + mInstance = new SwerveVisionEstimator(() -> Rotation2d.fromDegrees(CommandSwerveDrivetrain.mGyro.getYaw().getValueAsDouble()), null); + } + return mInstance; + } - // public Pose2d getCurrrentPose(){ - // return poseEstimator.getEstimatedPosition(); - // } + public Pose2d getCurrrentPose(){ + return poseEstimator.getEstimatedPosition(); + } - // public void estimatorUpdate(){ - // Arrays.stream(visions).forEach(vision -> { - // var visionEst = vision.getEstimatedGlobalPose(); + public void estimatorUpdate(){ + Arrays.stream(visions).forEach(vision -> { + var visionEst = vision.getEstimatedGlobalPose(); - // visionEst.ifPresent(est -> { - // var estStdDevs = vision.getEstimationStdDevs(); + visionEst.ifPresent(est -> { + var estStdDevs = vision.getEstimationStdDevs(); - // poseEstimator.addVisionMeasurement(est.estimatedPose.toPose2d(), est.timestampSeconds, estStdDevs); - // }); - // }); - // } + poseEstimator.addVisionMeasurement(est.estimatedPose.toPose2d(), est.timestampSeconds, estStdDevs); + }); + }); + } - // private String getFormattedPose(){ - // var pose = getCurrrentPose(); - // return String.format("(%.3f, %.3f) %.2f degrees", - // pose.getX(), - // pose.getY(), - // pose.getRotation().getDegrees()); - // } + private String getFormattedPose(){ + var pose = getCurrrentPose(); + return String.format("(%.3f, %.3f) %.2f degrees", + pose.getX(), + pose.getY(), + pose.getRotation().getDegrees()); + } - // @Override - // public void periodic(){ - // // //Update swerve pose estimator using drivetrain - // // poseEstimator.update(rotationSupplier.get(), modSupplier.get()); - // // //Add vision measurements to swerve pose estimator - // // estimatorUpdate(); + @Override + public void periodic(){ + // //Update swerve pose estimator using drivetrain + poseEstimator.update(rotationSupplier.get(), modSupplier.get()); + //Add vision measurements to swerve pose estimator + estimatorUpdate(); - // // //log to dashboard - // // var dashboardPose = poseEstimator.getEstimatedPosition(); - // // SmartDashboard.putString("Estimated Pose", getFormattedPose()); - // // SmartDashboard.putData(field2d); - // // field2d.setRobotPose(getCurrrentPose()); + //log to dashboard + var dashboardPose = poseEstimator.getEstimatedPosition(); + SmartDashboard.putString("Estimated Pose", getFormattedPose()); + SmartDashboard.putData(field2d); + field2d.setRobotPose(getCurrrentPose()); // } + } } \ No newline at end of file