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/Configs.java b/src/main/java/frc/robot/Configs.java index e01c759..7b1cec0 100644 --- a/src/main/java/frc/robot/Configs.java +++ b/src/main/java/frc/robot/Configs.java @@ -1,13 +1,17 @@ package frc.robot; +import java.util.HashMap; +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 +151,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 = new HashMap<>(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, new Translation2d(0, 0.1)), + Map.entry(ReefScoringLocation.K, new Translation2d(-0.1, 0)), + 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 d381744..8d2107b 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,14 +97,13 @@ 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; + 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; @@ -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,9 @@ 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 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 +459,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 +605,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 +638,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 +686,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 +758,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 +787,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 +813,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/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 2eb9bc5..ec99a0f 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("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); SmartDashboard.putNumber("Kraken Ki", DriveConstants.KrakenX60Driving.KI); @@ -184,11 +184,16 @@ 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()); + for (var offset : Configs.Offsets.CORAL_SCORE_POSITION_OFFSETS.entrySet()) { + var key = offset.getKey(); + var value = offset.getValue(); + value = new Translation2d( + 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); + } - NarwhalConstants.SCORING_RELATIVE_TRANSFORM = new Transform2d(new Translation2d(autoScoreOffsetX, autoScoreOffsetY), Rotation2d.fromDegrees(autoScoreOffsetRot)); // --- Section: Get routine/alliance selections AutonomousRoutine autonomousRoutine = autonomousSelector.getSelected(); 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/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;}) ); - } /** 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.