From 72499af1345f71ed5a8240538124a4b064129455 Mon Sep 17 00:00:00 2001 From: joelzoto <118407587+joelzoto@users.noreply.github.com> Date: Wed, 5 Mar 2025 19:06:33 -0500 Subject: [PATCH] Swerve Vision Estimator Integration Attempt --- .../drivetrain/CommandSwerveDrivetrain.java | 8 ++++++++ .../frc/robot/drivetrain/TunerConstants.java | 18 +++++++++++++----- .../robot/vision/SwerveVisionEstimator.java | 5 ++++- 3 files changed, 25 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/drivetrain/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/drivetrain/CommandSwerveDrivetrain.java index 61aa27d..6ce7d29 100644 --- a/src/main/java/frc/robot/drivetrain/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/drivetrain/CommandSwerveDrivetrain.java @@ -7,6 +7,7 @@ import com.ctre.phoenix6.SignalLogger; import com.ctre.phoenix6.Utils; import com.ctre.phoenix6.hardware.Pigeon2; +import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants; import com.ctre.phoenix6.swerve.SwerveModuleConstants; import com.ctre.phoenix6.swerve.SwerveRequest; @@ -18,6 +19,7 @@ import edu.wpi.first.math.numbers.N3; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj.motorcontrol.Talon; import edu.wpi.first.wpilibj.Notifier; import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj2.command.Command; @@ -39,6 +41,12 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su public static Pigeon2 mGyro = new Pigeon2(TunerConstants.kPigeonId, "Canivore"); + public static TalonFX FrontLeftSteerMotor = new TalonFX(TunerConstants.kFrontLeftSteerMotorId, "Canbus"); + public static TalonFX FrontRightSteerMotor = new TalonFX(TunerConstants.kFrontRightSteerMotorId, "Canbus"); + public static TalonFX BackLeftSteerMotor = new TalonFX(TunerConstants.kBackLeftSteerMotorId, "Canbus"); + public static TalonFX BackRightSteerMotor = new TalonFX(TunerConstants.kBackRightSteerMotorId, "Canbus"); + + /* 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 655c4af..50652c7 100644 --- a/src/main/java/frc/robot/drivetrain/TunerConstants.java +++ b/src/main/java/frc/robot/drivetrain/TunerConstants.java @@ -12,11 +12,13 @@ import com.ctre.phoenix6.swerve.SwerveModuleConstants.*; import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.geometry.Rotation2d; 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.crevolib.math.Conversions; import frc.robot.drivetrain.CommandSwerveDrivetrain; import frc.robot.vision.Vision; import frc.robot.vision.VisionConfig; @@ -107,7 +109,6 @@ public class TunerConstants { public static final int kPigeonId = 18; - // These are only used for simulation private static final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(0.01); private static final MomentOfInertia kDriveInertia = KilogramSquareMeters.of(0.01); @@ -146,7 +147,7 @@ public class TunerConstants { // Front Left private static final int kFrontLeftDriveMotorId = 1; - private static final int kFrontLeftSteerMotorId = 5; + public static final int kFrontLeftSteerMotorId = 5; private static final int kFrontLeftEncoderId = 19; private static final Angle kFrontLeftEncoderOffset = Rotations.of(0.384765625); private static final boolean kFrontLeftSteerMotorInverted = false; @@ -157,7 +158,7 @@ public class TunerConstants { // Front Right private static final int kFrontRightDriveMotorId = 2; - private static final int kFrontRightSteerMotorId = 6; + public static final int kFrontRightSteerMotorId = 6; private static final int kFrontRightEncoderId = 20; private static final Angle kFrontRightEncoderOffset = Rotations.of(0.110107421875); private static final boolean kFrontRightSteerMotorInverted = false; @@ -168,7 +169,7 @@ public class TunerConstants { // Back Left private static final int kBackLeftDriveMotorId = 3; - private static final int kBackLeftSteerMotorId = 7; + public static final int kBackLeftSteerMotorId = 7; private static final int kBackLeftEncoderId = 21; private static final Angle kBackLeftEncoderOffset = Rotations.of(0.086181640625); private static final boolean kBackLeftSteerMotorInverted = false; @@ -179,7 +180,7 @@ public class TunerConstants { // Back Right private static final int kBackRightDriveMotorId = 4; - private static final int kBackRightSteerMotorId = 8; + public static final int kBackRightSteerMotorId = 8; private static final int kBackRightEncoderId = 22; private static final Angle kBackRightEncoderOffset = Rotations.of(0.274658203125); private static final boolean kBackRightSteerMotorInverted = false; @@ -210,6 +211,7 @@ public class TunerConstants { kBackRightXPos, kBackRightYPos, kInvertRightSide, kBackRightSteerMotorInverted, kBackRightEncoderInverted ); + /** * Creates a CommandSwerveDrivetrain instance. * This should only be called once in your robot program,. @@ -220,6 +222,12 @@ public static CommandSwerveDrivetrain createDrivetrain() { ); } + public static final SwerveModulePosition[] K_SWERVE_MODULE_POSITIONS = new SwerveModulePosition[]{ + new SwerveModulePosition(kFrontLeftXPos, Rotation2d.fromDegrees(CommandSwerveDrivetrain.FrontLeftSteerMotor.getPosition().getValueAsDouble())), + new SwerveModulePosition(kFrontRightXPos, Rotation2d.fromDegrees(CommandSwerveDrivetrain.FrontRightSteerMotor.getPosition().getValueAsDouble())), + new SwerveModulePosition(kBackLeftXPos, Rotation2d.fromDegrees(CommandSwerveDrivetrain.BackLeftSteerMotor.getPosition().getValueAsDouble())), + new SwerveModulePosition(kBackRightXPos, Rotation2d.fromDegrees(CommandSwerveDrivetrain.BackRightSteerMotor.getPosition().getValueAsDouble())) + }; /** * Swerve Drive class utilizing CTR Electronics' Phoenix 6 API with the selected device types. diff --git a/src/main/java/frc/robot/vision/SwerveVisionEstimator.java b/src/main/java/frc/robot/vision/SwerveVisionEstimator.java index 03b3bd2..51cad53 100644 --- a/src/main/java/frc/robot/vision/SwerveVisionEstimator.java +++ b/src/main/java/frc/robot/vision/SwerveVisionEstimator.java @@ -16,6 +16,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.drivetrain.CommandSwerveDrivetrain; +import frc.robot.drivetrain.TunerConstants; public class SwerveVisionEstimator extends SubsystemBase{ @@ -50,7 +51,9 @@ 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); + mInstance = new SwerveVisionEstimator(() -> + Rotation2d.fromDegrees(drivetrain.mGyro.getYaw().getValueAsDouble()), + ()-> TunerConstants.K_SWERVE_MODULE_POSITIONS); } return mInstance; }