From 47606ade0f0f9941916b169379d1a13f8667e4c0 Mon Sep 17 00:00:00 2001 From: Tylr-J42 Date: Sat, 14 Mar 2026 15:56:08 -0400 Subject: [PATCH] pose localization trouble --- src/main/java/frc/robot/RobotContainer.java | 8 ++++---- .../constants/IntakeRollerConstants.java | 6 +++--- .../frc/robot/constants/PhotonConstants.java | 19 ++++++++++++++----- .../robot/constants/SpindexerConstants.java | 2 +- .../java/frc/robot/subsystems/Drivetrain.java | 7 ++++++- .../frc/robot/subsystems/IntakeRoller.java | 3 +++ 6 files changed, 31 insertions(+), 14 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index b7232f8..d2f3bfe 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -43,7 +43,7 @@ import frc.robot.utilities.Elastic; import frc.robot.utilities.Utilities; public class RobotContainer { - // private PhotonVision vision; + private PhotonVision vision; private Drivetrain drivetrain; private Hood hood; private Shooter shooter; @@ -60,7 +60,7 @@ public class RobotContainer { private Timer shiftTimer; public RobotContainer() { - //vision = new PhotonVision(); + vision = new PhotonVision(); drivetrain = new Drivetrain(null); hood = new Hood(); shooter = new Shooter(); @@ -70,7 +70,7 @@ public class RobotContainer { //climber = new Climber(); configureNamedCommands(); - /* + vision.addPoseEstimateConsumer(drivetrain::consumeVisualPose); vision.addPoseEstimateConsumer((vp) -> { Logger.recordOutput( @@ -78,7 +78,7 @@ public class RobotContainer { vp.visualPose() ); }); - */ + driver = new CommandXboxController(OIConstants.kDriverControllerPort); secondary = new CommandXboxController(OIConstants.kOperatorControllerPort); diff --git a/src/main/java/frc/robot/constants/IntakeRollerConstants.java b/src/main/java/frc/robot/constants/IntakeRollerConstants.java index 55c275a..8f5014c 100644 --- a/src/main/java/frc/robot/constants/IntakeRollerConstants.java +++ b/src/main/java/frc/robot/constants/IntakeRollerConstants.java @@ -10,8 +10,8 @@ public class IntakeRollerConstants { public static final int kCurrentLimit = 65; - public static final boolean kInvertLeftMotor = true; - public static final boolean kInvertRightMotor = false; + public static final boolean kInvertLeftMotor = false; + public static final boolean kInvertRightMotor = true; public static final double kSpeed = 1; @@ -31,6 +31,6 @@ public class IntakeRollerConstants { .idleMode(kIdleMode) .smartCurrentLimit(kCurrentLimit) .inverted(kInvertRightMotor) - .follow(kLeftMotorCANID); + ; } } diff --git a/src/main/java/frc/robot/constants/PhotonConstants.java b/src/main/java/frc/robot/constants/PhotonConstants.java index eaf3cc4..21897c0 100644 --- a/src/main/java/frc/robot/constants/PhotonConstants.java +++ b/src/main/java/frc/robot/constants/PhotonConstants.java @@ -12,15 +12,24 @@ public class PhotonConstants { public static final String kCamera2Name = "CameraPV2"; // TODO Need actual values for all of this - public static final Transform3d kCamera1RobotToCam = new Transform3d(); + public static final Transform3d kCamera1RobotToCam = new Transform3d( + Units.inchesToMeters(1.5), + Units.inchesToMeters(-8.5), + Units.inchesToMeters(28.5), + new Rotation3d( + Units.degreesToRadians(0), + Units.degreesToRadians(24.0), + Units.degreesToRadians(30.0) + ) + ); public static final Transform3d kCamera2RobotToCam = new Transform3d( Units.inchesToMeters(1.5), Units.inchesToMeters(-10.5), Units.inchesToMeters(28.5), new Rotation3d( - Units.degreesToRadians(0), - Units.degreesToRadians(24), - Units.degreesToRadians(-10) + Units.degreesToRadians(0.0), + Units.degreesToRadians(24.0), + Units.degreesToRadians(-10.0) ) ); @@ -32,7 +41,7 @@ public class PhotonConstants { // YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A CONFIGURATION ITEM public static final List configs = List.of( - //new PhotonVisionConfig(kCamera1Name, kCamera1RobotToCam, kCamera1HeightMeters, kCamera1PitchRadians), + new PhotonVisionConfig(kCamera1Name, kCamera1RobotToCam, kCamera1HeightMeters, kCamera1PitchRadians), new PhotonVisionConfig(kCamera2Name, kCamera2RobotToCam, kCamera2HeightMeters, kCamera2PitchRadians) ); } \ No newline at end of file diff --git a/src/main/java/frc/robot/constants/SpindexerConstants.java b/src/main/java/frc/robot/constants/SpindexerConstants.java index 86c5041..8036523 100644 --- a/src/main/java/frc/robot/constants/SpindexerConstants.java +++ b/src/main/java/frc/robot/constants/SpindexerConstants.java @@ -19,7 +19,7 @@ public class SpindexerConstants { public static final double kSpindexerSpeed = 1; public static final double kFeederSpeed = 1; - public static final boolean kFeederMotorInverted = true; + public static final boolean kFeederMotorInverted = false; public static final InvertedValue kSpindexerInversionState = InvertedValue.Clockwise_Positive; public static final NeutralModeValue kSpindexerIdleMode = NeutralModeValue.Coast; diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java index d8ee4ce..c2b2642 100644 --- a/src/main/java/frc/robot/subsystems/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/Drivetrain.java @@ -332,6 +332,10 @@ public class Drivetrain extends SubsystemBase { } public void consumeVisualPose(VisualPose pose) { + if(Math.abs(pose.visualPose().minus(getPose()).getTranslation().getNorm()) > 1) { + return; + } + estimator.addVisionMeasurement( pose.visualPose(), pose.timestamp() @@ -376,7 +380,8 @@ public class Drivetrain extends SubsystemBase { SwerveModuleState[] swerveModuleStates = DrivetrainConstants.kDriveKinematics.toSwerveModuleStates( fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedDelivered, ySpeedDelivered, rotationDelivered, - estimator.getEstimatedPosition().getRotation()) : + //estimator.getEstimatedPosition().getRotation()) : + Rotation2d.fromDegrees(getGyroValue())) : new ChassisSpeeds(xSpeedDelivered, ySpeedDelivered, rotationDelivered) ); diff --git a/src/main/java/frc/robot/subsystems/IntakeRoller.java b/src/main/java/frc/robot/subsystems/IntakeRoller.java index 2f75eab..e2a4d7d 100644 --- a/src/main/java/frc/robot/subsystems/IntakeRoller.java +++ b/src/main/java/frc/robot/subsystems/IntakeRoller.java @@ -33,18 +33,21 @@ public class IntakeRoller extends SubsystemBase { public Command runIn() { return run(() -> { leftMotor.set(IntakeRollerConstants.kSpeed*0.8); + rightMotor.set(IntakeRollerConstants.kSpeed*0.8); }); } public Command runOut() { return run(() -> { leftMotor.set(-IntakeRollerConstants.kSpeed); + rightMotor.set(-IntakeRollerConstants.kSpeed); }); } public Command stop() { return run(() -> { leftMotor.set(0); + rightMotor.set(0); }); }