From 40f507632c924355ec43601cec80306f9aade786 Mon Sep 17 00:00:00 2001 From: caTrixLiu Date: Wed, 4 Dec 2024 18:49:22 +0800 Subject: [PATCH] some tunings during test --- src/main/java/frc/robot/RobotContainer.java | 5 ++++- src/main/java/frc/robot/constants/TunerConstants.java | 4 ++-- src/main/java/frc/robot/constants/VisionConstants.java | 4 ++-- 3 files changed, 8 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 70f5690..b47803d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -37,6 +37,7 @@ import org.ironmaple.simulation.drivesims.SwerveDriveSimulation; import org.ironmaple.simulation.drivesims.SwerveModuleSimulation; import org.ironmaple.simulation.drivesims.configs.DriveTrainSimulationConfig; +import org.ironmaple.utils.FieldMirroringUtils; import org.littletonrobotics.junction.Logger; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; @@ -296,7 +297,9 @@ public void configureButtonBindings() { driverXBox .start() .onTrue(Commands.runOnce( - () -> drive.setPose(new Pose2d(drive.getPose().getTranslation(), new Rotation2d())), + () -> drive.setPose(new Pose2d( + drive.getPose().getTranslation(), + FieldMirroringUtils.getCurrentAllianceDriverStationFacing())), drive) .ignoringDisable(true)); diff --git a/src/main/java/frc/robot/constants/TunerConstants.java b/src/main/java/frc/robot/constants/TunerConstants.java index 28b62fc..93b3660 100644 --- a/src/main/java/frc/robot/constants/TunerConstants.java +++ b/src/main/java/frc/robot/constants/TunerConstants.java @@ -147,7 +147,7 @@ public class TunerConstants { public static final int kFrontRightDriveMotorId = 3; public static final int kFrontRightSteerMotorId = 4; public static final int kFrontRightEncoderId = 2; - public static final Angle kFrontRightEncoderOffset = Rotations.of(-0.230712890625); + public static final Angle kFrontRightEncoderOffset = Rotations.of(-0.730712890625); public static final boolean kFrontRightSteerMotorInverted = true; public static final boolean kFrontRightCANcoderInverted = false; @@ -169,7 +169,7 @@ public class TunerConstants { public static final int kBackRightDriveMotorId = 7; public static final int kBackRightSteerMotorId = 8; public static final int kBackRightEncoderId = 4; - public static final Angle kBackRightEncoderOffset = Rotations.of(-0.05419921875); + public static final Angle kBackRightEncoderOffset = Rotations.of(-0.55419921875); public static final boolean kBackRightSteerMotorInverted = true; public static final boolean kBackRightCANcoderInverted = false; diff --git a/src/main/java/frc/robot/constants/VisionConstants.java b/src/main/java/frc/robot/constants/VisionConstants.java index 5ef1749..b3a958d 100644 --- a/src/main/java/frc/robot/constants/VisionConstants.java +++ b/src/main/java/frc/robot/constants/VisionConstants.java @@ -11,8 +11,8 @@ public class VisionConstants { public static final AprilTagFieldLayout fieldLayout = AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField); public static final double /* default standard error for vision observation, if only one apriltag observed */ - TRANSLATIONAL_STANDARD_ERROR_METERS_FOR_SINGLE_OBSERVATION = 2, - ROTATIONAL_STANDARD_ERROR_RADIANS_FOR_SINGLE_OBSERVATION = Math.toRadians(30), + TRANSLATIONAL_STANDARD_ERROR_METERS_FOR_SINGLE_OBSERVATION = 4, + ROTATIONAL_STANDARD_ERROR_RADIANS_FOR_SINGLE_OBSERVATION = Math.toRadians(60), TRANSLATIONAL_STANDARD_ERROR_METERS_FOR_MULTITAG = 0.5, ROTATIONAL_STANDARD_ERROR_RADIANS_FOR_MULTITAG = Math.toRadians(8),