From 72f18bdd45db1748156d00bd0fa1126840a6d98c Mon Sep 17 00:00:00 2001 From: Graham Mueller Date: Thu, 22 May 2025 09:37:13 -0500 Subject: [PATCH 1/4] use rotation2d constants where we can --- src/main/java/frc/robot/Constants.java | 53 +++++++++---------- .../robot/commands/ManualDriveCommand.java | 6 +-- .../subsystems/narwhal/NarwhalClimber.java | 2 +- 3 files changed, 30 insertions(+), 31 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 7ec11ad..08a5f6b 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -62,7 +62,7 @@ public static class OperatorConstants { // Position offsets, used to determine the orientation of the driver public static final Rotation2d BLUE_ALLIANCE_OFFSET = new Rotation2d(-0.5 * Math.PI); public static final Rotation2d RED_ALLIANCE_OFFSET = new Rotation2d(0.5 * Math.PI); - public static final Rotation2d DEMO_ALLIANCE_OFFSET = new Rotation2d(0.0); + public static final Rotation2d DEMO_ALLIANCE_OFFSET = Rotation2d.kZero; // Controller Constants public static final double LINEAR_INPUT_CURVE_POWER = 2.5; @@ -97,11 +97,10 @@ public final class DriveConstants { public static final double ROTATION_THRESHOLD = 0.157; // Radians public static final double NARWHAL_CAN_RAISE_LIFT_DISTANCE = 1.0; // Meters - public static final Transform2d NARWHAL_RAKE_ALAGE_TRANSFORM = new Transform2d(0.5,0.0,new Rotation2d()); + public static final Transform2d NARWHAL_RAKE_ALAGE_TRANSFORM = new Transform2d(0.5, 0.0, Rotation2d.kZero); public static final double[] DRIVE_STANDARD_DEVIATION_COEFFICIENTS = { 0.006611986432, 0.3500199104, 0 - }; public static final double AUTO_DRIVING_TRANSLATIONAL_SPEED_SAFETY_FACTOR = 0.15; @@ -130,8 +129,8 @@ public final class DriveConstants { // Angular Offsets for Swerve Modules public static final Rotation2d FRONT_LEFT_MODULE_ANGULAR_OFFSET = new Rotation2d(-0.5 * Math.PI); - public static final Rotation2d REAR_LEFT_MODULE_ANGULAR_OFFSET = new Rotation2d(Math.PI); - public static final Rotation2d FRONT_RIGHT_MODULE_ANGULAR_OFFSET = new Rotation2d(0); + public static final Rotation2d REAR_LEFT_MODULE_ANGULAR_OFFSET = Rotation2d.kPi; + public static final Rotation2d FRONT_RIGHT_MODULE_ANGULAR_OFFSET = Rotation2d.kZero; public static final Rotation2d REAR_RIGHT_MODULE_ANGULAR_OFFSET = new Rotation2d(Math.PI / 2); // SPARK MAX CAN IDs for Driving Motors @@ -317,10 +316,10 @@ public static class NarwhalConstants { public static final double UPPER_ASSEMBLY_MASS = 17.2; //Kg public static final double UPPER_ASSEMBLY_MOI = 0.995; //Kg m^2 - public static final Transform2d INTAKING_RELATIVE_TRANSFORM = new Transform2d(new Translation2d(0.0, -0.019),new Rotation2d(Math.PI)); - public static Transform2d SCORING_RELATIVE_TRANSFORM = new Transform2d(new Translation2d(0.4, 0.019),new Rotation2d(0)); - public static final Transform2d ALGAE_REMOVAL_RELATIVE_TRANSFORM = new Transform2d(new Translation2d(0.0, -0.019),new Rotation2d(0.0)); - public static final Transform2d CLIMB_RELATIVE_TRANSFORM = new Transform2d(new Translation2d(0.0,0.0),new Rotation2d(Math.PI)); + public static final Transform2d INTAKING_RELATIVE_TRANSFORM = new Transform2d(new Translation2d(0.0, -0.019), Rotation2d.kPi); + public static Transform2d SCORING_RELATIVE_TRANSFORM = new Transform2d(new Translation2d(0.4, 0.019), Rotation2d.kZero); + public static final Transform2d ALGAE_REMOVAL_RELATIVE_TRANSFORM = new Transform2d(new Translation2d(0.0, -0.019), Rotation2d.kZero); + public static final Transform2d CLIMB_RELATIVE_TRANSFORM = new Transform2d(new Translation2d(0.0,0.0), Rotation2d.kPi); public static class NarwhalIntakeOuttakeConstants { @@ -461,9 +460,9 @@ public static class SquidConstants { public static final double UPPER_ASSEMBLY_MASS = 0.0; public static final double UPPER_ASSEMBLY_MOI = 0.0; //Kg m^2 - public static final Transform2d INTAKING_RELATIVE_TRANSFORM = new Transform2d(new Translation2d(0.5,0.0),new Rotation2d(Math.PI)); - public static final Transform2d SCORING_RELATIVE_TRANSFORM = new Transform2d(new Translation2d(0.5,0.0),new Rotation2d(0)); - public static final Transform2d CLIMB_RELATIVE_TRANSFORM = new Transform2d(new Translation2d(0.5,0.0),new Rotation2d(Math.PI)); + public static final Transform2d INTAKING_RELATIVE_TRANSFORM = new Transform2d(new Translation2d(0.5,0.0),Rotation2d.kPi); + public static final Transform2d SCORING_RELATIVE_TRANSFORM = new Transform2d(new Translation2d(0.5,0.0),Rotation2d.kZero); + public static final Transform2d CLIMB_RELATIVE_TRANSFORM = new Transform2d(new Translation2d(0.5,0.0),Rotation2d.kPi); public static class SquidManipulatorConstants { @@ -607,7 +606,7 @@ private static Pose2d rotate180(Pose2d bluePose) { double redX = 2 * FIELD_ORIGIN.getX() - blueTranslation.getX(); double redY = 2 * FIELD_ORIGIN.getY() - blueTranslation.getY(); // Orientation: add π and normalize (Rotation2d takes care of that if needed) - Rotation2d redRotation = bluePose.getRotation().plus(new Rotation2d(Math.PI)); + Rotation2d redRotation = bluePose.getRotation().plus(Rotation2d.kPi); return new Pose2d(new Translation2d(redX, redY), redRotation); } @@ -640,13 +639,13 @@ public static class ReefConstants { REEF_CENTER.getY() + APOTHEM * Math.sin(Math.PI)); public static final Pose2d BLUE_CORAL_SCORE_POSITION_A = new Pose2d( new Translation2d(BLUE_LEFT_BASE.getX(), BLUE_LEFT_BASE.getY() + TANGENT_OFFSET), - new Rotation2d(Math.PI)); + Rotation2d.kPi); public static final Pose2d BLUE_CORAL_SCORE_POSITION_B = new Pose2d( new Translation2d(BLUE_LEFT_BASE.getX(), BLUE_LEFT_BASE.getY() - TANGENT_OFFSET), - new Rotation2d(Math.PI)); + Rotation2d.kPi); // ALGAE REMOVAL position (midpoint on left side) public static final Pose2d BLUE_ALGAE_REMOVAL_POSITION_AB = new Pose2d( - BLUE_LEFT_BASE, new Rotation2d(Math.PI)); + BLUE_LEFT_BASE, Rotation2d.kPi); // BOTTOM-LEFT SIDE (normal = 4PI/3) private static final double NORMAL_BL = 4 * Math.PI / 3; @@ -688,13 +687,13 @@ public static class ReefConstants { REEF_CENTER.getY() + APOTHEM * Math.sin(0)); public static final Pose2d BLUE_CORAL_SCORE_POSITION_G = new Pose2d( new Translation2d(BLUE_RIGHT_BASE.getX(), BLUE_RIGHT_BASE.getY() - TANGENT_OFFSET), - new Rotation2d(0)); + Rotation2d.kZero); public static final Pose2d BLUE_CORAL_SCORE_POSITION_H = new Pose2d( new Translation2d(BLUE_RIGHT_BASE.getX(), BLUE_RIGHT_BASE.getY() + TANGENT_OFFSET), - new Rotation2d(0)); + Rotation2d.kZero); // ALGAE REMOVAL position (midpoint on bottom-left side) public static final Pose2d BLUE_ALGAE_REMOVAL_POSITION_GH = new Pose2d( - BLUE_RIGHT_BASE, new Rotation2d(0)); + BLUE_RIGHT_BASE, Rotation2d.kZero); // TOP-RIGHT SIDE (normal = PI/3) private static final double NORMAL_TR = Math.PI / 3; @@ -760,12 +759,12 @@ public static class CoralStationConstants { private static final double CORAL_INTAKE_SPACING = 0.2032; // Convert to a field–relative angle: add π/2 since the wall is vertical. - private static final Rotation2d FIELD_INTAKE_ANGLE = new Rotation2d(3.76971235639); + private static final Rotation2d PLAYER_STATION_ANGLE = Rotation2d.fromDegrees(216); // Base pose for BLUE LEFT intake positions. // Index 0 is given as (1.70244, 7.57545) with the computed intake angle. private static final Pose2d BLUE_INTAKE_LEFT_BASE = new Pose2d( - new Translation2d(1.70298, 7.57847), FIELD_INTAKE_ANGLE); + new Translation2d(1.70298, 7.57847), PLAYER_STATION_ANGLE); // Lists for the coral station intake poses. // The human player’s list is from indices 0 to 8 (left-to-right from the driver perspective). @@ -789,7 +788,7 @@ public static class CoralStationConstants { for (int i = 0; i < 9; i++) { Pose2d pose = BLUE_INTAKE_LEFT_BASE.transformBy( new Transform2d(new Translation2d(i * CORAL_INTAKE_SPACING, 0), new Rotation2d(0.5 * Math.PI))); - blueCoralIntakeLeft.add(pose); + blueCoralIntakeLeft.add(pose); } // Generate BLUE RIGHT intake positions by mirroring across the horizontal line at y = FIELD_ORIGIN.getY() // Reverse the order so that the human player's indices run left-to-right. @@ -815,19 +814,19 @@ public static class CoralStationConstants { public static class StartingPoseConstants { - public static final Pose2d BLUE_LEFT_STARTING_POSE = new Pose2d(new Translation2d(7.247,6.16),new Rotation2d()); - public static final Pose2d BLUE_CENTER_STARTING_POSE = new Pose2d(new Translation2d(7.247,4.209),new Rotation2d()); - public static final Pose2d BLUE_RIGHT_STARTING_POSE = new Pose2d(new Translation2d(7.247,1.88),new Rotation2d()); + public static final Pose2d BLUE_LEFT_STARTING_POSE = new Pose2d(new Translation2d(7.247, 6.16), Rotation2d.kZero); + public static final Pose2d BLUE_CENTER_STARTING_POSE = new Pose2d(new Translation2d(7.247, 4.209), Rotation2d.kZero); + public static final Pose2d BLUE_RIGHT_STARTING_POSE = new Pose2d(new Translation2d(7.247, 1.88), Rotation2d.kZero); public static final Pose2d RED_LEFT_STARTING_POSE = rotate180(BLUE_LEFT_STARTING_POSE); public static final Pose2d RED_CENTER_STARTING_POSE = rotate180(BLUE_CENTER_STARTING_POSE); public static final Pose2d RED_RIGHT_STARTING_POSE = rotate180(BLUE_RIGHT_STARTING_POSE); // The offset if starting with a tush push (middle of the tape, rather than edge) - public static final Transform2d TUSH_PUSH_STARTING_TRANSFORM = new Transform2d(-0.0254,0.0,new Rotation2d()); + public static final Transform2d TUSH_PUSH_STARTING_TRANSFORM = new Transform2d(-0.0254, 0.0, Rotation2d.kZero); // The offset tush push will move the robot. This is relative to the transformed starting pose. - public static final Transform2d TUSH_PUSH_TRANSFORM = new Transform2d(0.10,0.0,new Rotation2d()); + public static final Transform2d TUSH_PUSH_TRANSFORM = new Transform2d(0.10, 0.0, Rotation2d.kZero); } diff --git a/src/main/java/frc/robot/commands/ManualDriveCommand.java b/src/main/java/frc/robot/commands/ManualDriveCommand.java index dc01740..5233709 100644 --- a/src/main/java/frc/robot/commands/ManualDriveCommand.java +++ b/src/main/java/frc/robot/commands/ManualDriveCommand.java @@ -20,7 +20,7 @@ public abstract class ManualDriveCommand extends Command { /** * The rotational offset of the driver */ - private Rotation2d driverRotationalOffset = new Rotation2d(0.0); + private Rotation2d driverRotationalOffset = Rotation2d.kZero; /** * Indicates whether target translation is active. @@ -55,7 +55,7 @@ public abstract class ManualDriveCommand extends Command { /** * The rotation (angle) where the robot is locked. */ - // private Rotation2d thetaLockRotation = new Rotation2d(0.0); + // private Rotation2d thetaLockRotation = Rotation2d.kZero; /** * Tracks whether rotational velocity has been reset. @@ -70,7 +70,7 @@ public abstract class ManualDriveCommand extends Command { /** * The origin angle for target rotation calculations. */ - private Rotation2d targetRotationOrigin = new Rotation2d(0.0); + private Rotation2d targetRotationOrigin = Rotation2d.kZero; /** * Creates a new manual drive command to allow for the robot to be controlled manually during teleop. diff --git a/src/main/java/frc/robot/subsystems/narwhal/NarwhalClimber.java b/src/main/java/frc/robot/subsystems/narwhal/NarwhalClimber.java index 8371620..3ecbd46 100644 --- a/src/main/java/frc/robot/subsystems/narwhal/NarwhalClimber.java +++ b/src/main/java/frc/robot/subsystems/narwhal/NarwhalClimber.java @@ -34,7 +34,7 @@ public class NarwhalClimber extends SubsystemBase { private final SparkMax climber; private final SparkMaxConfig climberConfig; private final SparkClosedLoopController climberPID; - private Rotation2d currentTarget = new Rotation2d(0); + private Rotation2d currentTarget = Rotation2d.kZero; /** * Creates a new instance of the NarwhalClimber class, setting up all necessary hardware in the process. From 6fb2a2420b2774c5a9af4e4fd93d78ee1702fa47 Mon Sep 17 00:00:00 2001 From: Graham Mueller Date: Thu, 22 May 2025 09:55:34 -0500 Subject: [PATCH 2/4] part one of adding configurable offsets for all the reef positions. --- src/main/java/frc/robot/Configs.java | 31 ++++++++++++------- src/main/java/frc/robot/Constants.java | 1 - src/main/java/frc/robot/RobotContainer.java | 21 +++++++------ .../frc/robot/subsystems/DriveSubsystem.java | 6 ++-- 4 files changed, 35 insertions(+), 24 deletions(-) diff --git a/src/main/java/frc/robot/Configs.java b/src/main/java/frc/robot/Configs.java index e01c759..ae9bb5c 100644 --- a/src/main/java/frc/robot/Configs.java +++ b/src/main/java/frc/robot/Configs.java @@ -1,13 +1,16 @@ package frc.robot; +import java.util.Map; + import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor; import com.revrobotics.spark.config.SparkMaxConfig; +import edu.wpi.first.math.geometry.Translation2d; import frc.robot.Constants.DriveConstants; +import frc.robot.util.field.ReefScoringLocation; public final class Configs { - public static final class Swerve { public static final class Driving { @@ -147,20 +150,24 @@ public static final class Turning { NEO_550_TURNING_CONFIG .idleMode(DriveConstants.Neo550Turning.IDLE_MODE) .smartCurrentLimit(DriveConstants.Neo550Turning.CURRENT_LIMIT); - } - } - - } - public static final class Squid { - + public static final class Offsets { + public static Map CORAL_SCORE_POSITION_OFFSETS = Map.ofEntries( + Map.entry(ReefScoringLocation.A, Translation2d.kZero), + Map.entry(ReefScoringLocation.B, Translation2d.kZero), + Map.entry(ReefScoringLocation.C, Translation2d.kZero), + Map.entry(ReefScoringLocation.D, Translation2d.kZero), + Map.entry(ReefScoringLocation.E, Translation2d.kZero), + Map.entry(ReefScoringLocation.F, Translation2d.kZero), + Map.entry(ReefScoringLocation.G, Translation2d.kZero), + Map.entry(ReefScoringLocation.H, new Translation2d(0.4, 0.019)), // H is our Center Score + Map.entry(ReefScoringLocation.I, Translation2d.kZero), + Map.entry(ReefScoringLocation.J, Translation2d.kZero), + Map.entry(ReefScoringLocation.K, Translation2d.kZero), + Map.entry(ReefScoringLocation.L, Translation2d.kZero) + ); } - - public static final class Narwhal { - - } - } diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 08a5f6b..da85800 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -317,7 +317,6 @@ public static class NarwhalConstants { public static final double UPPER_ASSEMBLY_MOI = 0.995; //Kg m^2 public static final Transform2d INTAKING_RELATIVE_TRANSFORM = new Transform2d(new Translation2d(0.0, -0.019), Rotation2d.kPi); - public static Transform2d SCORING_RELATIVE_TRANSFORM = new Transform2d(new Translation2d(0.4, 0.019), Rotation2d.kZero); public static final Transform2d ALGAE_REMOVAL_RELATIVE_TRANSFORM = new Transform2d(new Translation2d(0.0, -0.019), Rotation2d.kZero); public static final Transform2d CLIMB_RELATIVE_TRANSFORM = new Transform2d(new Translation2d(0.0,0.0), Rotation2d.kPi); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 2eb9bc5..0fdc931 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -27,7 +27,6 @@ import frc.robot.util.upper_assembly.UpperAssemblyType; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; @@ -148,9 +147,10 @@ private void setupSmartDashboard() { SmartDashboard.putNumber("Deploy Rotations (degrees)", NarwhalConstants.NarwhalClimberConstants.DEPLOYED_WINCH_ROTATIONS.getDegrees()); // Add autonomous configuration options. - SmartDashboard.putNumber("Auto Score Offset X", NarwhalConstants.SCORING_RELATIVE_TRANSFORM.getX()); - SmartDashboard.putNumber("Auto Score Offset Y", NarwhalConstants.SCORING_RELATIVE_TRANSFORM.getY()); - SmartDashboard.putNumber("Auto Score Offset Rot", NarwhalConstants.SCORING_RELATIVE_TRANSFORM.getRotation().getDegrees()); + for (var offset : Configs.Offsets.CORAL_SCORE_POSITION_OFFSETS.entrySet()) { + SmartDashboard.putNumber("Auto Score " + offset.getKey().toString() + " Offset X", offset.getValue().getX()); + SmartDashboard.putNumber("Auto Score " + offset.getKey().toString() + " Offset Y", offset.getValue().getY()); + } SmartDashboard.putNumber("Kraken Kp", DriveConstants.KrakenX60Driving.KP); SmartDashboard.putNumber("Kraken Ki", DriveConstants.KrakenX60Driving.KI); @@ -184,11 +184,14 @@ public void scheduleAutonomous() { this.delayTimeSeconds = SmartDashboard.getNumber("Auto Delay", this.delayTimeSeconds); this.startWithTushPush = SmartDashboard.getBoolean("Tush Push Mode", this.startWithTushPush); - double autoScoreOffsetX = SmartDashboard.getNumber("Auto Score Offset X", NarwhalConstants.SCORING_RELATIVE_TRANSFORM.getX()); - double autoScoreOffsetY = SmartDashboard.getNumber("Auto Score Offset Y", NarwhalConstants.SCORING_RELATIVE_TRANSFORM.getY()); - double autoScoreOffsetRot = SmartDashboard.getNumber("Auto Score Offset Rot", NarwhalConstants.SCORING_RELATIVE_TRANSFORM.getRotation().getDegrees()); - - NarwhalConstants.SCORING_RELATIVE_TRANSFORM = new Transform2d(new Translation2d(autoScoreOffsetX, autoScoreOffsetY), Rotation2d.fromDegrees(autoScoreOffsetRot)); + for (var offset : Configs.Offsets.CORAL_SCORE_POSITION_OFFSETS.entrySet()) { + var key = offset.getKey(); + var value = offset.getValue(); + value = new Translation2d( + SmartDashboard.getNumber("Auto Score " + key.toString() + " Offset X", value.getX()), + SmartDashboard.getNumber("Auto Score " + key.toString() + " Offset Y", value.getY()) + ); + } // --- Section: Get routine/alliance selections AutonomousRoutine autonomousRoutine = autonomousSelector.getSelected(); diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java index 7bafa77..c51638f 100644 --- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java @@ -20,6 +20,7 @@ import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveModulePosition; @@ -34,6 +35,7 @@ import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.WaitUntilCommand; +import frc.robot.Configs; import frc.robot.Constants.Defaults; import frc.robot.Constants.DriveConstants; import frc.robot.Constants.FieldConstants.CoralStationConstants; @@ -490,7 +492,8 @@ public Command getScoringCommand(Alliance alliance, ReefScoringLocation location } if (upperAssemblyType == UpperAssemblyType.NARWHAL || upperAssemblyType == UpperAssemblyType.NONE) { - targetPose = targetPose.transformBy(NarwhalConstants.SCORING_RELATIVE_TRANSFORM); + Translation2d positionOffset = Configs.Offsets.CORAL_SCORE_POSITION_OFFSETS.get(location); + targetPose = targetPose.transformBy(new Transform2d(positionOffset, Rotation2d.kZero)); } System.out.println("Going to drive to position ("+ targetPose.getX() + ", " + targetPose.getY() + ')'); @@ -502,7 +505,6 @@ public Command getScoringCommand(Alliance alliance, ReefScoringLocation location ).andThen( new InstantCommand(() -> {inScorePose = true;}) ); - } /** From de3bdac256160d994c90687535ad6cd8e97be0a0 Mon Sep 17 00:00:00 2001 From: Graham Mueller Date: Thu, 22 May 2025 10:16:33 -0500 Subject: [PATCH 3/4] add to dashboard. shorten key to make it fit --- elastic-layout.json | 350 +++++++++++++++++++- src/main/java/frc/robot/RobotContainer.java | 8 +- 2 files changed, 351 insertions(+), 7 deletions(-) diff --git a/elastic-layout.json b/elastic-layout.json index d3b513d..11db7f9 100644 --- a/elastic-layout.json +++ b/elastic-layout.json @@ -140,7 +140,7 @@ { "title": "Upper Assembly", "x": 0.0, - "y": 256.0, + "y": 128.0, "width": 256.0, "height": 128.0, "type": "ComboBox Chooser", @@ -153,7 +153,7 @@ { "title": "Drive Motor", "x": 256.0, - "y": 256.0, + "y": 128.0, "width": 256.0, "height": 128.0, "type": "ComboBox Chooser", @@ -166,7 +166,7 @@ { "title": "Auto Delay", "x": 512.0, - "y": 256.0, + "y": 128.0, "width": 256.0, "height": 128.0, "type": "Number Slider", @@ -195,6 +195,350 @@ } ] } + }, + { + "name": "Scoring Offsets", + "grid_layout": { + "layouts": [], + "containers": [ + { + "title": "Reef B Offset Y", + "x": 384.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/Reef B Offset Y", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Reef A Offset X", + "x": 512.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/Reef A Offset X", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Reef A Offset Y", + "x": 640.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/Reef A Offset Y", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Reef B Offset X", + "x": 256.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/Reef B Offset X", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Reef C Offset X", + "x": 128.0, + "y": 128.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/Reef C Offset X", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Reef C Offset Y", + "x": 256.0, + "y": 128.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/Reef C Offset Y", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Reef D Offset X", + "x": 0.0, + "y": 256.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/Reef D Offset X", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Reef D Offset Y", + "x": 128.0, + "y": 256.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/Reef D Offset Y", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Reef E Offset X", + "x": 0.0, + "y": 384.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/Reef E Offset X", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Reef E Offset Y", + "x": 128.0, + "y": 384.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/Reef E Offset Y", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Reef F Offset X", + "x": 128.0, + "y": 512.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/Reef F Offset X", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Reef F Offset Y", + "x": 256.0, + "y": 512.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/Reef F Offset Y", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Reef G Offset X", + "x": 256.0, + "y": 640.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/Reef G Offset X", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Reef G Offset Y", + "x": 384.0, + "y": 640.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/Reef G Offset Y", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Reef H Offset X", + "x": 512.0, + "y": 640.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/Reef H Offset X", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Reef H Offset Y", + "x": 640.0, + "y": 640.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/Reef H Offset Y", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Reef I Offset X", + "x": 640.0, + "y": 512.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/Reef I Offset X", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Reef I Offset Y", + "x": 768.0, + "y": 512.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/Reef I Offset Y", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Reef J Offset Y", + "x": 896.0, + "y": 384.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/Reef J Offset Y", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Reef J Offset X", + "x": 768.0, + "y": 384.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/Reef J Offset X", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Reef K Offset X", + "x": 768.0, + "y": 256.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/Reef K Offset X", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Reef K Offset Y", + "x": 896.0, + "y": 256.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/Reef K Offset Y", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Reef L Offset X", + "x": 640.0, + "y": 128.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/Reef L Offset X", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Reef L Offset Y", + "x": 768.0, + "y": 128.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/Reef L Offset Y", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + } + ] + } } ] } \ No newline at end of file diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 0fdc931..8710eb5 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -148,8 +148,8 @@ private void setupSmartDashboard() { // Add autonomous configuration options. for (var offset : Configs.Offsets.CORAL_SCORE_POSITION_OFFSETS.entrySet()) { - SmartDashboard.putNumber("Auto Score " + offset.getKey().toString() + " Offset X", offset.getValue().getX()); - SmartDashboard.putNumber("Auto Score " + offset.getKey().toString() + " Offset Y", offset.getValue().getY()); + SmartDashboard.putNumber("Reef " + offset.getKey().toString() + " Offset X", offset.getValue().getX()); + SmartDashboard.putNumber("Reef " + offset.getKey().toString() + " Offset Y", offset.getValue().getY()); } SmartDashboard.putNumber("Kraken Kp", DriveConstants.KrakenX60Driving.KP); @@ -188,8 +188,8 @@ public void scheduleAutonomous() { var key = offset.getKey(); var value = offset.getValue(); value = new Translation2d( - SmartDashboard.getNumber("Auto Score " + key.toString() + " Offset X", value.getX()), - SmartDashboard.getNumber("Auto Score " + key.toString() + " Offset Y", value.getY()) + SmartDashboard.getNumber("Reef " + key.toString() + " Offset X", value.getX()), + SmartDashboard.getNumber("Reef " + key.toString() + " Offset Y", value.getY()) ); } From ade81896b2fbac31b6b04603b5e03df2c85a6dc3 Mon Sep 17 00:00:00 2001 From: N/A Date: Thu, 5 Jun 2025 18:38:20 -0500 Subject: [PATCH 4/4] feat: update odometry offsets --- src/main/java/frc/robot/Configs.java | 9 +++++---- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/Robot.java | 3 +++ src/main/java/frc/robot/RobotContainer.java | 2 ++ 4 files changed, 11 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/Configs.java b/src/main/java/frc/robot/Configs.java index ae9bb5c..7b1cec0 100644 --- a/src/main/java/frc/robot/Configs.java +++ b/src/main/java/frc/robot/Configs.java @@ -1,5 +1,6 @@ package frc.robot; +import java.util.HashMap; import java.util.Map; import com.ctre.phoenix6.configs.TalonFXConfiguration; @@ -155,7 +156,7 @@ public static final class Turning { } public static final class Offsets { - public static Map CORAL_SCORE_POSITION_OFFSETS = Map.ofEntries( + public static Map CORAL_SCORE_POSITION_OFFSETS = new HashMap<>(Map.ofEntries( Map.entry(ReefScoringLocation.A, Translation2d.kZero), Map.entry(ReefScoringLocation.B, Translation2d.kZero), Map.entry(ReefScoringLocation.C, Translation2d.kZero), @@ -165,9 +166,9 @@ public static final class Offsets { Map.entry(ReefScoringLocation.G, Translation2d.kZero), Map.entry(ReefScoringLocation.H, new Translation2d(0.4, 0.019)), // H is our Center Score Map.entry(ReefScoringLocation.I, Translation2d.kZero), - Map.entry(ReefScoringLocation.J, Translation2d.kZero), - Map.entry(ReefScoringLocation.K, Translation2d.kZero), + Map.entry(ReefScoringLocation.J, new Translation2d(0, 0.1)), + Map.entry(ReefScoringLocation.K, new Translation2d(-0.1, 0)), Map.entry(ReefScoringLocation.L, Translation2d.kZero) - ); + )); } } diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 64b5b47..8d2107b 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -103,7 +103,7 @@ public final class DriveConstants { 0.006611986432, 0.3500199104, 0 }; - public static final double AUTO_DRIVING_TRANSLATIONAL_SPEED_SAFETY_FACTOR = 0.15; + public static final double AUTO_DRIVING_TRANSLATIONAL_SPEED_SAFETY_FACTOR = 0.1; public static final double AUTO_DRIVING_TRANSLATIONAL_ACCELERATION_SAFETY_FACTOR = 0.4; public static final double AUTO_DRIVING_ROTATIONAL_SPEED_SAFETY_FACTOR = 0.25; public static final double AUTO_DRIVING_ROTATIONAL_ACCELERATION_FACTOR = 0.5; diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index b54742e..720105b 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -80,6 +80,8 @@ public void robotPeriodic() { @Override public void autonomousInit() { CommandScheduler.getInstance().cancelAll(); + CommandScheduler.getInstance().clearComposedCommands(); + robotContainer.scheduleAutonomous(); frc537StringLog.append("Autonomous Init\n"); @@ -106,6 +108,7 @@ public void teleopInit() { // continue until interrupted by another command, remove // this line or comment it out. CommandScheduler.getInstance().cancelAll(); + CommandScheduler.getInstance().clearComposedCommands(); // Schedule Teleop Commands robotContainer.scheduleTeleOp(); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 8710eb5..ec99a0f 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -191,8 +191,10 @@ public void scheduleAutonomous() { SmartDashboard.getNumber("Reef " + key.toString() + " Offset X", value.getX()), SmartDashboard.getNumber("Reef " + key.toString() + " Offset Y", value.getY()) ); + Configs.Offsets.CORAL_SCORE_POSITION_OFFSETS.put(key, value); } + // --- Section: Get routine/alliance selections AutonomousRoutine autonomousRoutine = autonomousSelector.getSelected(); Alliance alliance = allianceSelector.getSelected();