From 3e0f4d3256c3f1ece04cdbe37b5ef4d056ee7b8b Mon Sep 17 00:00:00 2001 From: Ethan Torres Date: Wed, 11 Feb 2026 18:17:46 -0500 Subject: [PATCH] Added some fuel position tracking and fixed tunerconstants and motor ID --- src/main/java/frc/robot/Constants.java | 95 ++- .../java/frc/robot/commands/MoveToFuel.java | 2 +- .../frc/robot/generated/TunerConstants.java | 544 +++++++++--------- .../java/frc/robot/subsystems/Swerve.java | 226 ++++++-- 4 files changed, 513 insertions(+), 354 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index d4f00a9..8a07237 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -164,10 +164,10 @@ public static class SwerveConstants { } public static class IntakeConstants { - public static final int armMainID = 15; - public static final int armFollowerID = 16; - public static final int intakeID = 17; - public static final int armEncoderID = 18; + public static final int armMainID = 14; + public static final int armFollowerID = 15; + public static final int intakeID = 16; + public static final int armEncoderID = 17; public static final double armGearRatio = 3.0; @@ -232,40 +232,81 @@ public static class VisionConstants { public static final Transform3d arducamLeftTransform = new Transform3d( - Units.inchesToMeters(-10.02), - Units.inchesToMeters(10.02), - Units.inchesToMeters(5), + Units.inchesToMeters(0.036), + Units.inchesToMeters(12.89), + Units.inchesToMeters(11.8), new Rotation3d( - 0, Units.degreesToRadians(-25), Units.degreesToRadians(180 - 45))); // Pitch: 65 + 0, Units.degreesToRadians(-25), Units.degreesToRadians(90))); // Pitch: 65 public static final String arducamRightName = "Arducam_Right"; public static final Transform3d arducamRightTransform = new Transform3d( - Units.inchesToMeters(-10.02), - Units.inchesToMeters(-10.02), - Units.inchesToMeters(5), + Units.inchesToMeters(2.8), + Units.inchesToMeters(-12.8), + Units.inchesToMeters(9.4), new Rotation3d( - 0, Units.degreesToRadians(-25), Units.degreesToRadians(180 + 45))); // Pitch: 65 + 0, Units.degreesToRadians(-25), Units.degreesToRadians(-90))); // Pitch: 65 - public static final String arducamFrontName = "Arducam_Front"; + public static final String arducamBackLeftName = "Arducam_BackLeft"; - public static final Transform3d arducamFrontTransform = + public static final Transform3d arducamBackLeftTransform = new Transform3d( - Units.inchesToMeters(0), - Units.inchesToMeters(-1), - Units.inchesToMeters(10.07), - new Rotation3d(0, Units.degreesToRadians(-15), Units.degreesToRadians(0))); // Pitch: 65 + Units.inchesToMeters(-9.6), + Units.inchesToMeters(10.6), + Units.inchesToMeters(8.319), + new Rotation3d( + 0, Units.degreesToRadians(-25), Units.degreesToRadians(180 - 45))); // Pitch: 65 + + public static final String arducamBackRightName = "Arducam_BackRight"; + + public static final Transform3d arducamBackRightTransform = + new Transform3d( + Units.inchesToMeters(-12.147), + Units.inchesToMeters(-12.505), + Units.inchesToMeters(15), + new Rotation3d( + 0, Units.degreesToRadians(-25), Units.degreesToRadians(180 + 45))); // Pitch: 65 + + // public static final String arducamFrontName = "Arducam_Front"; + + // public static final Transform3d arducamFrontTransform = + // new Transform3d( + // Units.inchesToMeters(0), + // Units.inchesToMeters(-1), + // Units.inchesToMeters(10.07), + // new Rotation3d(0, Units.degreesToRadians(-15), Units.degreesToRadians(0))); // Pitch: + // 65 public static final String arducamFuelName = "Arducam_Fuel"; // TODO: Update this transform public static final Transform3d arducamFuelTransform = new Transform3d( + Units.inchesToMeters(24.468), Units.inchesToMeters(0), - Units.inchesToMeters(0), - Units.inchesToMeters(0), - new Rotation3d(0, Units.degreesToRadians(0), Units.degreesToRadians(0))); + Units.inchesToMeters(10.591), + new Rotation3d(0, Units.degreesToRadians(-13), Units.degreesToRadians(0))); + + public static InterpolatingDoubleTreeMap fuelAreaToDistanceMap = + new InterpolatingDoubleTreeMap(); + + static { + fuelAreaToDistanceMap.put(80640.0, 15.0); // PIXEL area - INCHES horizontal distnace + fuelAreaToDistanceMap.put(56882.0, 18.0); + fuelAreaToDistanceMap.put(43264.0, 20.0); + fuelAreaToDistanceMap.put(33488.0, 23.0); + fuelAreaToDistanceMap.put(26080.0, 26.0); + fuelAreaToDistanceMap.put(21025.0, 29.0); + fuelAreaToDistanceMap.put(14884.0, 33.0); + fuelAreaToDistanceMap.put(13110.0, 37.0); + fuelAreaToDistanceMap.put(10816.0, 40.0); + fuelAreaToDistanceMap.put(9312.0, 43.0); + fuelAreaToDistanceMap.put(8190.0, 46.0); + fuelAreaToDistanceMap.put(7225.0, 49.0); + fuelAreaToDistanceMap.put(6400.0, 52.0); + fuelAreaToDistanceMap.put(5550.0, 56.0); + } } public static class FieldConstants { @@ -458,9 +499,9 @@ public static class TurretConstants { public static final Transform2d robotToTurretTransform = new Transform2d(TurretConstants.robotToTurret.toTranslation2d(), Rotation2d.kZero); - public static final int turretMotorID = 19; - public static final int encoderAID = 20; - public static final int encoderBID = 21; + public static final int turretMotorID = 18; + public static final int encoderAID = 19; + public static final int encoderBID = 20; } public static class AutoConstants { @@ -475,7 +516,7 @@ public static class AutoConstants { } public static class HoodConstants { - public static final int hoodMotorID = 22; + public static final int hoodMotorID = 21; public static final Angle minAngle = Degrees.of(21.448); public static final Angle maxAngle = Degrees.of(59.231); @@ -529,8 +570,8 @@ public static class HoodConstants { } public static class SpindexerConstants { - public static final int SpindexerMotorID = 23; - public static final int SpindexerLaserID = 24; + public static final int SpindexerMotorID = 22; + public static final int SpindexerLaserID = 23; public static final double SpindexerMotorSpeed = 0.5; public static final double SpindexerDistance = 100; } diff --git a/src/main/java/frc/robot/commands/MoveToFuel.java b/src/main/java/frc/robot/commands/MoveToFuel.java index 516b3a5..0accb84 100644 --- a/src/main/java/frc/robot/commands/MoveToFuel.java +++ b/src/main/java/frc/robot/commands/MoveToFuel.java @@ -40,7 +40,7 @@ public MoveToFuel(Swerve swerve) { } private void updateDesiredRotation() { - desiredRotation = swerve.desiredFuelRotation; + // desiredRotation = swerve.desiredFuelRotation; } // Called when the command is initially scheduled. diff --git a/src/main/java/frc/robot/generated/TunerConstants.java b/src/main/java/frc/robot/generated/TunerConstants.java index 2841368..432da98 100644 --- a/src/main/java/frc/robot/generated/TunerConstants.java +++ b/src/main/java/frc/robot/generated/TunerConstants.java @@ -8,307 +8,279 @@ import com.ctre.phoenix6.signals.*; import com.ctre.phoenix6.swerve.*; import com.ctre.phoenix6.swerve.SwerveModuleConstants.*; + import edu.wpi.first.math.Matrix; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; import edu.wpi.first.units.measure.*; -import frc.robot.Constants.SwerveConstants; + import frc.robot.subsystems.Swerve; // Generated by the 2026 Tuner X Swerve Project Generator // https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/tuner-swerve/index.html public class TunerConstants { - // Both sets of gains need to be tuned to your individual robot. - - // The steer motor uses any SwerveModule.SteerRequestType control request with the - // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput - private static final Slot0Configs steerGains = - new Slot0Configs() - .withKP(SwerveConstants.steerKP) - .withKI(SwerveConstants.steerKI) - .withKD(SwerveConstants.steerKD) - .withKS(SwerveConstants.steerKS) - .withKV(SwerveConstants.steerKV) - .withKA(SwerveConstants.steerKA) - .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); - // When using closed-loop control, the drive motor uses the control - // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput - private static final Slot0Configs driveGains = - new Slot0Configs().withKP(0.1).withKI(0).withKD(0).withKS(0).withKV(0.124); - - // The closed-loop output type to use for the steer motors; - // This affects the PID/FF gains for the steer motors - private static final ClosedLoopOutputType kSteerClosedLoopOutput = ClosedLoopOutputType.Voltage; - // The closed-loop output type to use for the drive motors; - // This affects the PID/FF gains for the drive motors - private static final ClosedLoopOutputType kDriveClosedLoopOutput = ClosedLoopOutputType.Voltage; - - // The type of motor used for the drive motor - private static final DriveMotorArrangement kDriveMotorType = - DriveMotorArrangement.TalonFX_Integrated; - // The type of motor used for the drive motor - private static final SteerMotorArrangement kSteerMotorType = - SteerMotorArrangement.TalonFX_Integrated; - - // The remote sensor feedback type to use for the steer motors; - // When not Pro-licensed, Fused*/Sync* automatically fall back to Remote* - private static final SteerFeedbackType kSteerFeedbackType = SteerFeedbackType.FusedCANcoder; - - // The stator current at which the wheels start to slip; - // This needs to be tuned to your individual robot - private static final Current kSlipCurrent = Amps.of(120); - - // Initial configs for the drive and steer motors and the azimuth encoder; these cannot be null. - // Some configs will be overwritten; check the `with*InitialConfigs()` API documentation. - private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration(); - private static final TalonFXConfiguration steerInitialConfigs = - new TalonFXConfiguration() - .withCurrentLimits( - new CurrentLimitsConfigs() - // Swerve azimuth does not require much torque output, so we can set a relatively - // low - // stator current limit to help avoid brownouts without impacting performance. - .withStatorCurrentLimit(Amps.of(60)) - .withStatorCurrentLimitEnable(true)); - private static final CANcoderConfiguration encoderInitialConfigs = new CANcoderConfiguration(); - // Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs - private static final Pigeon2Configuration pigeonConfigs = null; - - // CAN bus that the devices are located on; - // All swerve devices must share the same CAN bus - public static final CANBus kCANBus = new CANBus("canivore", "./logs/example.hoot"); - - // Theoretical free speed (m/s) at 12 V applied output; - // This needs to be tuned to your individual robot - public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(4.54); - - // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns; - // This may need to be tuned to your individual robot - private static final double kCoupleRatio = 3.8181818181818183; - - private static final double kDriveGearRatio = 7.363636363636365; - private static final double kSteerGearRatio = 15.42857142857143; - private static final Distance kWheelRadius = Inches.of(2.167); - - private static final boolean kInvertLeftSide = false; - private static final boolean kInvertRightSide = true; - - private static final int kPigeonId = 1; - - // These are only used for simulation - private static final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(0.01); - private static final MomentOfInertia kDriveInertia = KilogramSquareMeters.of(0.01); - // Simulated voltage necessary to overcome friction - private static final Voltage kSteerFrictionVoltage = Volts.of(0.2); - private static final Voltage kDriveFrictionVoltage = Volts.of(0.2); - - public static final SwerveDrivetrainConstants DrivetrainConstants = - new SwerveDrivetrainConstants() - .withCANBusName(kCANBus.getName()) - .withPigeon2Id(kPigeonId) - .withPigeon2Configs(pigeonConfigs); - - private static final SwerveModuleConstantsFactory< - TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> - ConstantCreator = - new SwerveModuleConstantsFactory< - TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration>() - .withDriveMotorGearRatio(kDriveGearRatio) - .withSteerMotorGearRatio(kSteerGearRatio) - .withCouplingGearRatio(kCoupleRatio) - .withWheelRadius(kWheelRadius) - .withSteerMotorGains(steerGains) - .withDriveMotorGains(driveGains) - .withSteerMotorClosedLoopOutput(kSteerClosedLoopOutput) - .withDriveMotorClosedLoopOutput(kDriveClosedLoopOutput) - .withSlipCurrent(kSlipCurrent) - .withSpeedAt12Volts(kSpeedAt12Volts) - .withDriveMotorType(kDriveMotorType) - .withSteerMotorType(kSteerMotorType) - .withFeedbackSource(kSteerFeedbackType) - .withDriveMotorInitialConfigs(driveInitialConfigs) - .withSteerMotorInitialConfigs(steerInitialConfigs) - .withEncoderInitialConfigs(encoderInitialConfigs) - .withSteerInertia(kSteerInertia) - .withDriveInertia(kDriveInertia) - .withSteerFrictionVoltage(kSteerFrictionVoltage) - .withDriveFrictionVoltage(kDriveFrictionVoltage); - - // Front Left - private static final int kFrontLeftDriveMotorId = 3; - private static final int kFrontLeftSteerMotorId = 2; - private static final int kFrontLeftEncoderId = 1; - private static final Angle kFrontLeftEncoderOffset = Rotations.of(0.15234375); - private static final boolean kFrontLeftSteerMotorInverted = true; - private static final boolean kFrontLeftEncoderInverted = false; - - private static final Distance kFrontLeftXPos = Inches.of(10); - private static final Distance kFrontLeftYPos = Inches.of(10); - - // Front Right - private static final int kFrontRightDriveMotorId = 1; - private static final int kFrontRightSteerMotorId = 0; - private static final int kFrontRightEncoderId = 0; - private static final Angle kFrontRightEncoderOffset = Rotations.of(-0.4873046875); - private static final boolean kFrontRightSteerMotorInverted = true; - private static final boolean kFrontRightEncoderInverted = false; - - private static final Distance kFrontRightXPos = Inches.of(10); - private static final Distance kFrontRightYPos = Inches.of(-10); - - // Back Left - private static final int kBackLeftDriveMotorId = 7; - private static final int kBackLeftSteerMotorId = 6; - private static final int kBackLeftEncoderId = 3; - private static final Angle kBackLeftEncoderOffset = Rotations.of(-0.219482421875); - private static final boolean kBackLeftSteerMotorInverted = true; - private static final boolean kBackLeftEncoderInverted = false; - - private static final Distance kBackLeftXPos = Inches.of(-10); - private static final Distance kBackLeftYPos = Inches.of(10); - - // Back Right - private static final int kBackRightDriveMotorId = 5; - private static final int kBackRightSteerMotorId = 4; - private static final int kBackRightEncoderId = 2; - private static final Angle kBackRightEncoderOffset = Rotations.of(0.17236328125); - private static final boolean kBackRightSteerMotorInverted = true; - private static final boolean kBackRightEncoderInverted = false; - - private static final Distance kBackRightXPos = Inches.of(-10); - private static final Distance kBackRightYPos = Inches.of(-10); - - public static final SwerveModuleConstants< - TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> - FrontLeft = - ConstantCreator.createModuleConstants( - kFrontLeftSteerMotorId, - kFrontLeftDriveMotorId, - kFrontLeftEncoderId, - kFrontLeftEncoderOffset, - kFrontLeftXPos, - kFrontLeftYPos, - kInvertLeftSide, - kFrontLeftSteerMotorInverted, - kFrontLeftEncoderInverted); - public static final SwerveModuleConstants< - TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> - FrontRight = - ConstantCreator.createModuleConstants( - kFrontRightSteerMotorId, - kFrontRightDriveMotorId, - kFrontRightEncoderId, - kFrontRightEncoderOffset, - kFrontRightXPos, - kFrontRightYPos, - kInvertRightSide, - kFrontRightSteerMotorInverted, - kFrontRightEncoderInverted); - public static final SwerveModuleConstants< - TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> - BackLeft = - ConstantCreator.createModuleConstants( - kBackLeftSteerMotorId, - kBackLeftDriveMotorId, - kBackLeftEncoderId, - kBackLeftEncoderOffset, - kBackLeftXPos, - kBackLeftYPos, - kInvertLeftSide, - kBackLeftSteerMotorInverted, - kBackLeftEncoderInverted); - public static final SwerveModuleConstants< - TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> - BackRight = - ConstantCreator.createModuleConstants( - kBackRightSteerMotorId, - kBackRightDriveMotorId, - kBackRightEncoderId, - kBackRightEncoderOffset, - kBackRightXPos, - kBackRightYPos, - kInvertRightSide, - kBackRightSteerMotorInverted, - kBackRightEncoderInverted); - - /** - * Creates a CommandSwerveDrivetrain instance. This should only be called once in your robot - * program,. - */ - public static Swerve createDrivetrain() { - return new Swerve(DrivetrainConstants, FrontLeft, FrontRight, BackLeft, BackRight); - } - - /** Swerve Drive class utilizing CTR Electronics' Phoenix 6 API with the selected device types. */ - public static class TunerSwerveDrivetrain extends SwerveDrivetrain { - /** - * Constructs a CTRE SwerveDrivetrain using the specified constants. - * - *

This constructs the underlying hardware devices, so users should not construct the devices - * themselves. If they need the devices, they can access them through getters in the classes. - * - * @param drivetrainConstants Drivetrain-wide constants for the swerve drive - * @param modules Constants for each specific module - */ - public TunerSwerveDrivetrain( - SwerveDrivetrainConstants drivetrainConstants, SwerveModuleConstants... modules) { - super(TalonFX::new, TalonFX::new, CANcoder::new, drivetrainConstants, modules); - } + // Both sets of gains need to be tuned to your individual robot. + + // The steer motor uses any SwerveModule.SteerRequestType control request with the + // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput + private static final Slot0Configs steerGains = new Slot0Configs() + .withKP(100).withKI(0).withKD(0.5) + .withKS(0.1).withKV(1.16).withKA(0) + .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); + // When using closed-loop control, the drive motor uses the control + // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput + private static final Slot0Configs driveGains = new Slot0Configs() + .withKP(0.1).withKI(0).withKD(0) + .withKS(0).withKV(0.124); + + // The closed-loop output type to use for the steer motors; + // This affects the PID/FF gains for the steer motors + private static final ClosedLoopOutputType kSteerClosedLoopOutput = ClosedLoopOutputType.Voltage; + // The closed-loop output type to use for the drive motors; + // This affects the PID/FF gains for the drive motors + private static final ClosedLoopOutputType kDriveClosedLoopOutput = ClosedLoopOutputType.Voltage; + + // The type of motor used for the drive motor + private static final DriveMotorArrangement kDriveMotorType = DriveMotorArrangement.TalonFX_Integrated; + // The type of motor used for the drive motor + private static final SteerMotorArrangement kSteerMotorType = SteerMotorArrangement.TalonFX_Integrated; + + // The remote sensor feedback type to use for the steer motors; + // When not Pro-licensed, Fused*/Sync* automatically fall back to Remote* + private static final SteerFeedbackType kSteerFeedbackType = SteerFeedbackType.FusedCANcoder; + + // The stator current at which the wheels start to slip; + // This needs to be tuned to your individual robot + private static final Current kSlipCurrent = Amps.of(120); + + // Initial configs for the drive and steer motors and the azimuth encoder; these cannot be null. + // Some configs will be overwritten; check the `with*InitialConfigs()` API documentation. + private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration(); + private static final TalonFXConfiguration steerInitialConfigs = new TalonFXConfiguration() + .withCurrentLimits( + new CurrentLimitsConfigs() + // Swerve azimuth does not require much torque output, so we can set a relatively low + // stator current limit to help avoid brownouts without impacting performance. + .withStatorCurrentLimit(Amps.of(60)) + .withStatorCurrentLimitEnable(true) + ); + private static final CANcoderConfiguration encoderInitialConfigs = new CANcoderConfiguration(); + // Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs + private static final Pigeon2Configuration pigeonConfigs = null; + + // CAN bus that the devices are located on; + // All swerve devices must share the same CAN bus + public static final CANBus kCANBus = new CANBus("Default Name", "./logs/example.hoot"); + + // Theoretical free speed (m/s) at 12 V applied output; + // This needs to be tuned to your individual robot + public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(4.76); + + // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns; + // This may need to be tuned to your individual robot + private static final double kCoupleRatio = 5.4; + + private static final double kDriveGearRatio = 6.48; + private static final double kSteerGearRatio = 12.1; + private static final Distance kWheelRadius = Inches.of(2); + + private static final boolean kInvertLeftSide = false; + private static final boolean kInvertRightSide = true; + + private static final int kPigeonId = 13; + + // These are only used for simulation + private static final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(0.01); + private static final MomentOfInertia kDriveInertia = KilogramSquareMeters.of(0.01); + // Simulated voltage necessary to overcome friction + private static final Voltage kSteerFrictionVoltage = Volts.of(0.2); + private static final Voltage kDriveFrictionVoltage = Volts.of(0.2); + + public static final SwerveDrivetrainConstants DrivetrainConstants = new SwerveDrivetrainConstants() + .withCANBusName(kCANBus.getName()) + .withPigeon2Id(kPigeonId) + .withPigeon2Configs(pigeonConfigs); + + private static final SwerveModuleConstantsFactory ConstantCreator = + new SwerveModuleConstantsFactory() + .withDriveMotorGearRatio(kDriveGearRatio) + .withSteerMotorGearRatio(kSteerGearRatio) + .withCouplingGearRatio(kCoupleRatio) + .withWheelRadius(kWheelRadius) + .withSteerMotorGains(steerGains) + .withDriveMotorGains(driveGains) + .withSteerMotorClosedLoopOutput(kSteerClosedLoopOutput) + .withDriveMotorClosedLoopOutput(kDriveClosedLoopOutput) + .withSlipCurrent(kSlipCurrent) + .withSpeedAt12Volts(kSpeedAt12Volts) + .withDriveMotorType(kDriveMotorType) + .withSteerMotorType(kSteerMotorType) + .withFeedbackSource(kSteerFeedbackType) + .withDriveMotorInitialConfigs(driveInitialConfigs) + .withSteerMotorInitialConfigs(steerInitialConfigs) + .withEncoderInitialConfigs(encoderInitialConfigs) + .withSteerInertia(kSteerInertia) + .withDriveInertia(kDriveInertia) + .withSteerFrictionVoltage(kSteerFrictionVoltage) + .withDriveFrictionVoltage(kDriveFrictionVoltage); + + + // Front Left + private static final int kFrontLeftDriveMotorId = 1; + private static final int kFrontLeftSteerMotorId = 2; + private static final int kFrontLeftEncoderId = 3; + private static final Angle kFrontLeftEncoderOffset = Rotations.of(-0.174072265625); + private static final boolean kFrontLeftSteerMotorInverted = true; + private static final boolean kFrontLeftEncoderInverted = false; + + private static final Distance kFrontLeftXPos = Inches.of(11); + private static final Distance kFrontLeftYPos = Inches.of(11); + + // Front Right + private static final int kFrontRightDriveMotorId = 4; + private static final int kFrontRightSteerMotorId = 5; + private static final int kFrontRightEncoderId = 6; + private static final Angle kFrontRightEncoderOffset = Rotations.of(0.45556640625); + private static final boolean kFrontRightSteerMotorInverted = true; + private static final boolean kFrontRightEncoderInverted = false; + + private static final Distance kFrontRightXPos = Inches.of(11); + private static final Distance kFrontRightYPos = Inches.of(-11); + + // Back Left + private static final int kBackLeftDriveMotorId = 7; + private static final int kBackLeftSteerMotorId = 8; + private static final int kBackLeftEncoderId = 9; + private static final Angle kBackLeftEncoderOffset = Rotations.of(-0.3955078125); + private static final boolean kBackLeftSteerMotorInverted = false; + private static final boolean kBackLeftEncoderInverted = false; + + private static final Distance kBackLeftXPos = Inches.of(-11); + private static final Distance kBackLeftYPos = Inches.of(11); + + // Back Right + private static final int kBackRightDriveMotorId = 10; + private static final int kBackRightSteerMotorId = 11; + private static final int kBackRightEncoderId = 12; + private static final Angle kBackRightEncoderOffset = Rotations.of(0.436767578125); + private static final boolean kBackRightSteerMotorInverted = true; + private static final boolean kBackRightEncoderInverted = false; + + private static final Distance kBackRightXPos = Inches.of(-11); + private static final Distance kBackRightYPos = Inches.of(-11); + + + public static final SwerveModuleConstants FrontLeft = + ConstantCreator.createModuleConstants( + kFrontLeftSteerMotorId, kFrontLeftDriveMotorId, kFrontLeftEncoderId, kFrontLeftEncoderOffset, + kFrontLeftXPos, kFrontLeftYPos, kInvertLeftSide, kFrontLeftSteerMotorInverted, kFrontLeftEncoderInverted + ); + public static final SwerveModuleConstants FrontRight = + ConstantCreator.createModuleConstants( + kFrontRightSteerMotorId, kFrontRightDriveMotorId, kFrontRightEncoderId, kFrontRightEncoderOffset, + kFrontRightXPos, kFrontRightYPos, kInvertRightSide, kFrontRightSteerMotorInverted, kFrontRightEncoderInverted + ); + public static final SwerveModuleConstants BackLeft = + ConstantCreator.createModuleConstants( + kBackLeftSteerMotorId, kBackLeftDriveMotorId, kBackLeftEncoderId, kBackLeftEncoderOffset, + kBackLeftXPos, kBackLeftYPos, kInvertLeftSide, kBackLeftSteerMotorInverted, kBackLeftEncoderInverted + ); + public static final SwerveModuleConstants BackRight = + ConstantCreator.createModuleConstants( + kBackRightSteerMotorId, kBackRightDriveMotorId, kBackRightEncoderId, kBackRightEncoderOffset, + kBackRightXPos, kBackRightYPos, kInvertRightSide, kBackRightSteerMotorInverted, kBackRightEncoderInverted + ); /** - * Constructs a CTRE SwerveDrivetrain using the specified constants. - * - *

This constructs the underlying hardware devices, so users should not construct the devices - * themselves. If they need the devices, they can access them through getters in the classes. - * - * @param drivetrainConstants Drivetrain-wide constants for the swerve drive - * @param odometryUpdateFrequency The frequency to run the odometry loop. If unspecified or set - * to 0 Hz, this is 250 Hz on CAN FD, and 100 Hz on CAN 2.0. - * @param modules Constants for each specific module + * Creates a CommandSwerveDrivetrain instance. + * This should only be called once in your robot program,. */ - public TunerSwerveDrivetrain( - SwerveDrivetrainConstants drivetrainConstants, - double odometryUpdateFrequency, - SwerveModuleConstants... modules) { - super( - TalonFX::new, - TalonFX::new, - CANcoder::new, - drivetrainConstants, - odometryUpdateFrequency, - modules); + public static Swerve createDrivetrain() { + return new Swerve( + DrivetrainConstants, FrontLeft, FrontRight, BackLeft, BackRight + ); } + /** - * Constructs a CTRE SwerveDrivetrain using the specified constants. - * - *

This constructs the underlying hardware devices, so users should not construct the devices - * themselves. If they need the devices, they can access them through getters in the classes. - * - * @param drivetrainConstants Drivetrain-wide constants for the swerve drive - * @param odometryUpdateFrequency The frequency to run the odometry loop. If unspecified or set - * to 0 Hz, this is 250 Hz on CAN FD, and 100 Hz on CAN 2.0. - * @param odometryStandardDeviation The standard deviation for odometry calculation in the form - * [x, y, theta]ᵀ, with units in meters and radians - * @param visionStandardDeviation The standard deviation for vision calculation in the form [x, - * y, theta]ᵀ, with units in meters and radians - * @param modules Constants for each specific module + * Swerve Drive class utilizing CTR Electronics' Phoenix 6 API with the selected device types. */ - public TunerSwerveDrivetrain( - SwerveDrivetrainConstants drivetrainConstants, - double odometryUpdateFrequency, - Matrix odometryStandardDeviation, - Matrix visionStandardDeviation, - SwerveModuleConstants... modules) { - super( - TalonFX::new, - TalonFX::new, - CANcoder::new, - drivetrainConstants, - odometryUpdateFrequency, - odometryStandardDeviation, - visionStandardDeviation, - modules); + public static class TunerSwerveDrivetrain extends SwerveDrivetrain { + /** + * Constructs a CTRE SwerveDrivetrain using the specified constants. + *

+ * This constructs the underlying hardware devices, so users should not construct + * the devices themselves. If they need the devices, they can access them through + * getters in the classes. + * + * @param drivetrainConstants Drivetrain-wide constants for the swerve drive + * @param modules Constants for each specific module + */ + public TunerSwerveDrivetrain( + SwerveDrivetrainConstants drivetrainConstants, + SwerveModuleConstants... modules + ) { + super( + TalonFX::new, TalonFX::new, CANcoder::new, + drivetrainConstants, modules + ); + } + + /** + * Constructs a CTRE SwerveDrivetrain using the specified constants. + *

+ * This constructs the underlying hardware devices, so users should not construct + * the devices themselves. If they need the devices, they can access them through + * getters in the classes. + * + * @param drivetrainConstants Drivetrain-wide constants for the swerve drive + * @param odometryUpdateFrequency The frequency to run the odometry loop. If + * unspecified or set to 0 Hz, this is 250 Hz on + * CAN FD, and 100 Hz on CAN 2.0. + * @param modules Constants for each specific module + */ + public TunerSwerveDrivetrain( + SwerveDrivetrainConstants drivetrainConstants, + double odometryUpdateFrequency, + SwerveModuleConstants... modules + ) { + super( + TalonFX::new, TalonFX::new, CANcoder::new, + drivetrainConstants, odometryUpdateFrequency, modules + ); + } + + /** + * Constructs a CTRE SwerveDrivetrain using the specified constants. + *

+ * This constructs the underlying hardware devices, so users should not construct + * the devices themselves. If they need the devices, they can access them through + * getters in the classes. + * + * @param drivetrainConstants Drivetrain-wide constants for the swerve drive + * @param odometryUpdateFrequency The frequency to run the odometry loop. If + * unspecified or set to 0 Hz, this is 250 Hz on + * CAN FD, and 100 Hz on CAN 2.0. + * @param odometryStandardDeviation The standard deviation for odometry calculation + * in the form [x, y, theta]áµ€, with units in meters + * and radians + * @param visionStandardDeviation The standard deviation for vision calculation + * in the form [x, y, theta]áµ€, with units in meters + * and radians + * @param modules Constants for each specific module + */ + public TunerSwerveDrivetrain( + SwerveDrivetrainConstants drivetrainConstants, + double odometryUpdateFrequency, + Matrix odometryStandardDeviation, + Matrix visionStandardDeviation, + SwerveModuleConstants... modules + ) { + super( + TalonFX::new, TalonFX::new, CANcoder::new, + drivetrainConstants, odometryUpdateFrequency, + odometryStandardDeviation, visionStandardDeviation, modules + ); + } } - } } diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index 561de77..0defa97 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -24,9 +24,11 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.numbers.N1; @@ -41,6 +43,7 @@ import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.Field2d; +import edu.wpi.first.wpilibj.smartdashboard.FieldObject2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; @@ -59,6 +62,7 @@ import java.util.Collection; import java.util.Collections; import java.util.Comparator; +import java.util.HashMap; import java.util.List; import java.util.Map; import java.util.Optional; @@ -73,6 +77,7 @@ import org.photonvision.simulation.SimCameraProperties; import org.photonvision.targeting.PhotonPipelineResult; import org.photonvision.targeting.PhotonTrackedTarget; +import org.photonvision.targeting.TargetCorner; /** * Class that extends the Phoenix 6 SwerveDrivetrain class and implements Subsystem so it can easily @@ -103,20 +108,26 @@ public int compareTo(PoseEstimate other) { private final SwerveRequest.ApplyRobotSpeeds pathApplyRobotSpeeds = new SwerveRequest.ApplyRobotSpeeds().withDriveRequestType(DriveRequestType.Velocity); + public static record TrackedFuel(Pose2d pose, double lastSeen) {} + + private HashMap trackedFuels = new HashMap<>(); + + private int nextFuelID = 0; + private Field2d field = new Field2d(); + private FieldObject2d gamePieces = field.getObject("Fuel_GamePieces"); + private SwerveRequest.FieldCentric fieldOriented = new SwerveRequest.FieldCentric() .withForwardPerspective(ForwardPerspectiveValue.OperatorPerspective) .withSteerRequestType(SteerRequestType.Position); - private double targetFuelYaw = 0; - public Rotation2d desiredFuelRotation = Rotation2d.k180deg; - private PhotonCamera arducamLeft = new PhotonCamera(VisionConstants.arducamLeftName); - private PhotonCamera arducamFront = new PhotonCamera(VisionConstants.arducamFrontName); - private PhotonCamera arducamFuel = new PhotonCamera(VisionConstants.arducamFuelName); private PhotonCamera arducamRight = new PhotonCamera(VisionConstants.arducamRightName); + private PhotonCamera arducamBackLeft = new PhotonCamera(VisionConstants.arducamBackLeftName); + private PhotonCamera arducamBackRight = new PhotonCamera(VisionConstants.arducamBackRightName); + private PhotonCamera arducamFuel = new PhotonCamera(VisionConstants.arducamFuelName); private PhotonPoseEstimator leftPoseEstimator = new PhotonPoseEstimator(FieldConstants.aprilTagLayout, VisionConstants.arducamLeftTransform); @@ -124,12 +135,21 @@ public int compareTo(PoseEstimate other) { private PhotonPoseEstimator rightPoseEstimator = new PhotonPoseEstimator(FieldConstants.aprilTagLayout, VisionConstants.arducamRightTransform); - private PhotonPoseEstimator frontPoseEstimator = - new PhotonPoseEstimator(FieldConstants.aprilTagLayout, VisionConstants.arducamFrontTransform); + private PhotonPoseEstimator backLeftPoseEstimator = + new PhotonPoseEstimator(FieldConstants.aprilTagLayout, VisionConstants.arducamLeftTransform); + + private PhotonPoseEstimator backRightPoseEstimator = + new PhotonPoseEstimator(FieldConstants.aprilTagLayout, VisionConstants.arducamRightTransform); + + // private PhotonPoseEstimator frontPoseEstimator = + // new PhotonPoseEstimator(FieldConstants.aprilTagLayout, + // VisionConstants.arducamFrontTransform); private List latestArducamLeftResult; private List latestArducamRightResult; - private List latestArducamFrontResult; + private List latestArducamBackLeftResult; + private List latestArducamBackRightResult; + // private List latestArducamFrontResult; private List latestArducamFuelResult; private Optional> arducamLeftMatrix = Optional.empty(); @@ -138,8 +158,14 @@ public int compareTo(PoseEstimate other) { private Optional> arducamRightMatrix = Optional.empty(); private Optional> arducamRightDistCoeffs = Optional.empty(); - private Optional> arducamFrontMatrix = Optional.empty(); - private Optional> arducamFrontDistCoeffs = Optional.empty(); + private Optional> arducamBackLeftMatrix = Optional.empty(); + private Optional> arducamBackLeftDistCoeffs = Optional.empty(); + + private Optional> arducamBackRightMatrix = Optional.empty(); + private Optional> arducamBackRightDistCoeffs = Optional.empty(); + + // private Optional> arducamFrontMatrix = Optional.empty(); + // private Optional> arducamFrontDistCoeffs = Optional.empty(); private Optional> arducamFuelMatrix = Optional.empty(); private Optional> arducamFuelDistCoeffs = Optional.empty(); @@ -152,7 +178,9 @@ public int compareTo(PoseEstimate other) { private PhotonCameraSim arducamLeftSim; private PhotonCameraSim arducamRightSim; - private PhotonCameraSim arducamFrontSim; + private PhotonCameraSim arducamBackLeftSim; + private PhotonCameraSim arducamBackRightSim; + // private PhotonCameraSim arducamFrontSim; private PhotonCameraSim arducamFuelSim; @Logged(name = "Detected Targets") @@ -402,9 +430,9 @@ public Command pathFindThroughTrench() { } public boolean tooCloseToHub() { - Translation2d turretPose = - stateCache.Pose + stateCache + .Pose .getTranslation() .plus( TurretConstants.robotToTurretTransform @@ -548,14 +576,18 @@ public void periodic() { latestArducamLeftResult = arducamLeft.getAllUnreadResults(); latestArducamRightResult = arducamRight.getAllUnreadResults(); - latestArducamFrontResult = arducamFront.getAllUnreadResults(); + latestArducamBackLeftResult = arducamBackLeft.getAllUnreadResults(); + latestArducamBackRightResult = arducamBackRight.getAllUnreadResults(); + // latestArducamFrontResult = arducamFront.getAllUnreadResults(); latestArducamFuelResult = arducamFuel.getAllUnreadResults(); double stateTimestamp = Utils.currentTimeToFPGATime(stateCache.Timestamp); leftPoseEstimator.addHeadingData(stateTimestamp, stateCache.Pose.getRotation()); rightPoseEstimator.addHeadingData(stateTimestamp, stateCache.Pose.getRotation()); - frontPoseEstimator.addHeadingData(stateTimestamp, stateCache.Pose.getRotation()); + backLeftPoseEstimator.addHeadingData(stateTimestamp, stateCache.Pose.getRotation()); + backRightPoseEstimator.addHeadingData(stateTimestamp, stateCache.Pose.getRotation()); + // frontPoseEstimator.addHeadingData(stateTimestamp, stateCache.Pose.getRotation()); if (arducamLeftMatrix.isEmpty()) { arducamLeftMatrix = arducamLeft.getCameraMatrix(); @@ -571,13 +603,27 @@ public void periodic() { arducamRightDistCoeffs = arducamRight.getDistCoeffs(); } - if (arducamFrontMatrix.isEmpty()) { - arducamFrontMatrix = arducamFront.getCameraMatrix(); + if (arducamBackLeftMatrix.isEmpty()) { + arducamBackLeftMatrix = arducamBackLeft.getCameraMatrix(); + } + if (arducamBackLeftDistCoeffs.isEmpty()) { + arducamBackLeftDistCoeffs = arducamBackLeft.getDistCoeffs(); + } + + if (arducamBackRightMatrix.isEmpty()) { + arducamBackRightMatrix = arducamBackRight.getCameraMatrix(); } - if (arducamFrontDistCoeffs.isEmpty()) { - arducamFrontDistCoeffs = arducamFront.getDistCoeffs(); + if (arducamBackRightDistCoeffs.isEmpty()) { + arducamBackRightDistCoeffs = arducamBackRight.getDistCoeffs(); } + // if (arducamFrontMatrix.isEmpty()) { + // arducamFrontMatrix = arducamFront.getCameraMatrix(); + // } + // if (arducamFrontDistCoeffs.isEmpty()) { + // arducamFrontDistCoeffs = arducamFront.getDistCoeffs(); + // } + if (arducamFuelMatrix.isEmpty()) { arducamFuelMatrix = arducamFuel.getCameraMatrix(); } @@ -585,7 +631,6 @@ public void periodic() { arducamFuelDistCoeffs = arducamFuel.getDistCoeffs(); } updateVisionPoseEstimates(); - updateFuelRotation(); field.setRobotPose(stateCache.Pose); @@ -788,15 +833,35 @@ private void updateVisionPoseEstimates() { 1); updateVisionPoses( - latestArducamFrontResult, - frontPoseEstimator, - arducamFrontMatrix, - arducamFrontDistCoeffs, - VisionConstants.arducamFrontTransform, + latestArducamBackLeftResult, + backLeftPoseEstimator, + arducamBackLeftMatrix, + arducamBackLeftDistCoeffs, + VisionConstants.arducamBackLeftTransform, Units.inchesToMeters(3.0), Units.inchesToMeters(2.5), 1); + updateVisionPoses( + latestArducamBackRightResult, + backRightPoseEstimator, + arducamBackRightMatrix, + arducamBackRightDistCoeffs, + VisionConstants.arducamBackRightTransform, + Units.inchesToMeters(3.0), + Units.inchesToMeters(2.5), + 1); + + // updateVisionPoses( + // latestArducamFrontResult, + // frontPoseEstimator, + // arducamFrontMatrix, + // arducamFrontDistCoeffs, + // VisionConstants.arducamFrontTransform, + // Units.inchesToMeters(3.0), + // Units.inchesToMeters(2.5), + // 1); + Collections.sort(poseEstimates); for (PoseEstimate poseEstimate : poseEstimates) { @@ -872,23 +937,84 @@ private boolean isValidMultitagPose( return true; } - public void updateFuelRotation() { - double currentRotation = stateCache.Pose.getRotation().getDegrees(); - if (latestArducamFuelResult == null || latestArducamFuelResult.isEmpty()) { - return; - } + // @Logged(name = "Object Detection Positions") + public Pose2d[] updateFuelPositions() { + double now = Timer.getFPGATimestamp(); + + if (latestArducamFuelResult == null || latestArducamFuelResult.isEmpty()) return new Pose2d[1]; PhotonPipelineResult newestResult = latestArducamFuelResult.get(latestArducamFuelResult.size() - 1); + if (!newestResult.hasTargets()) return new Pose2d[1]; - if (!newestResult.hasTargets()) { - return; + for (PhotonTrackedTarget target : newestResult.getTargets()) { + List targetCorners = target.getMinAreaRectCorners(); + + double dX = Math.abs(targetCorners.get(1).x - targetCorners.get(0).x); + double dY = Math.abs(targetCorners.get(3).y - targetCorners.get(0).y); + + double boundingBoxArea = dX * dY; + + double fuelXDistance = VisionConstants.fuelAreaToDistanceMap.get(boundingBoxArea); + + double targetYawRad = Units.degreesToRadians(target.getYaw()); + + Translation3d camToTargetTranslation = + new Translation3d(fuelXDistance, fuelXDistance * Math.tan(targetYawRad), 0.0); + + Transform3d camToTarget = new Transform3d(camToTargetTranslation, new Rotation3d()); + + Transform3d robotToTarget = VisionConstants.arducamFuelTransform.plus(camToTarget); + + Pose3d fuelFieldPose3d = new Pose3d(stateCache.Pose).transformBy(robotToTarget); + + Pose2d fuelFieldPose = fuelFieldPose3d.toPose2d(); + + int key = nextFuelID++; + trackedFuels.put(key, new TrackedFuel(fuelFieldPose, now)); } - PhotonTrackedTarget target = newestResult.getBestTarget(); + Pose2d[] posesArray = trackedFuels.values().stream().map(tf -> tf.pose).toArray(Pose2d[]::new); - targetFuelYaw = target.getYaw(); + return posesArray; - desiredFuelRotation = Rotation2d.fromDegrees(currentRotation - targetFuelYaw); + // List fuelPosesArray = new ArrayList<>(); + // for (TrackedFuel fuel : trackedFuels.values()) { + // fuelPosesArray.add(fuel.pose); + // } + // gamePieces.setPoses(fuelPosesArray); + } + + @Logged(name = "Object Detection Positions") + public Pose2d[] updateFuelPositionsTemp() { + double[][] temporary = { + {20, 0}, + {25, 15}, + {30, -15} + }; + + List poseList = new ArrayList<>(); + + for (double[] sample : temporary) { + double fuelXDistance = Units.inchesToMeters(sample[0]); + double targetYawRad = Units.degreesToRadians(sample[1]); + + Translation3d camToTargetTranslation = + new Translation3d(fuelXDistance, fuelXDistance * Math.tan(targetYawRad), 0.0); + + Transform3d camToTarget = new Transform3d(camToTargetTranslation, new Rotation3d()); + + Transform3d robotToTarget = VisionConstants.arducamFuelTransform.plus(camToTarget); + + Pose3d fuelFieldPose3d = new Pose3d(stateCache.Pose).transformBy(robotToTarget); + + poseList.add(fuelFieldPose3d.toPose2d()); + } + + // Pose3d temp = new Pose3d(stateCache.Pose).transformBy(VisionConstants.arducamFuelTransform); + + // return new Pose2d[] {temp.toPose2d()}; + + return poseList.toArray(Pose2d[]::new); } private void initVisionSim() { @@ -935,12 +1061,23 @@ private void initVisionSim() { new PhotonCameraSim(arducamLeft, arducamProperties, FieldConstants.aprilTagLayout); arducamRightSim = new PhotonCameraSim(arducamRight, arducamProperties, FieldConstants.aprilTagLayout); - arducamFrontSim = - new PhotonCameraSim(arducamFront, arducamProperties, FieldConstants.aprilTagLayout); + arducamBackLeftSim = + new PhotonCameraSim(arducamBackLeft, arducamProperties, FieldConstants.aprilTagLayout); + arducamBackRightSim = + new PhotonCameraSim(arducamBackRight, arducamProperties, FieldConstants.aprilTagLayout); + + arducamFuelSim = + new PhotonCameraSim(arducamFuel, arducamProperties, FieldConstants.aprilTagLayout); + // arducamFrontSim = + // new PhotonCameraSim(arducamFront, arducamProperties, FieldConstants.aprilTagLayout); visionSim.addCamera(arducamLeftSim, VisionConstants.arducamLeftTransform); visionSim.addCamera(arducamRightSim, VisionConstants.arducamRightTransform); - visionSim.addCamera(arducamFrontSim, VisionConstants.arducamFrontTransform); + visionSim.addCamera(arducamBackLeftSim, VisionConstants.arducamBackLeftTransform); + visionSim.addCamera(arducamBackRightSim, VisionConstants.arducamBackRightTransform); + // visionSim.addCamera(arducamFuelSim, VisionConstants.arducamFuelTransform); + + // visionSim.addCamera(arducamFrontSim, VisionConstants.arducamFrontTransform); arducamLeftSim.enableRawStream(true); arducamLeftSim.enableProcessedStream(true); @@ -948,8 +1085,17 @@ private void initVisionSim() { arducamRightSim.enableRawStream(true); arducamRightSim.enableProcessedStream(true); - arducamFrontSim.enableRawStream(true); - arducamFrontSim.enableProcessedStream(true); + arducamBackLeftSim.enableRawStream(true); + arducamBackLeftSim.enableProcessedStream(true); + + arducamBackRightSim.enableRawStream(true); + arducamBackRightSim.enableProcessedStream(true); + + arducamFuelSim.enableRawStream(true); + arducamFuelSim.enableProcessedStream(true); + + // arducamFrontSim.enableRawStream(true); + // arducamFrontSim.enableProcessedStream(true); } public void configureAutoBuilder() {