From 479b08ad764521036e405de81d21c352497d0bfc Mon Sep 17 00:00:00 2001 From: ethanli1337 Date: Wed, 26 Mar 2025 15:51:01 -0500 Subject: [PATCH 01/12] Initial Commit --- src/main/java/frc/robot/pilot/Pilot.java | 1 + src/main/java/frc/robot/swerve/SwerveStates.java | 5 +++++ 2 files changed, 6 insertions(+) diff --git a/src/main/java/frc/robot/pilot/Pilot.java b/src/main/java/frc/robot/pilot/Pilot.java index e122fc24..927e265c 100644 --- a/src/main/java/frc/robot/pilot/Pilot.java +++ b/src/main/java/frc/robot/pilot/Pilot.java @@ -36,6 +36,7 @@ public class Pilot extends Gamepad { // vision Drive public final Trigger reefAim_A = A.and(teleop); public final Trigger cageAim_B = B.and(teleop); + public final Trigger netAim_X = X.and(teleop); // Drive Triggers public final Trigger upReorient = upDpad.and(fn, teleop); diff --git a/src/main/java/frc/robot/swerve/SwerveStates.java b/src/main/java/frc/robot/swerve/SwerveStates.java index 1c574cf6..a29253b6 100644 --- a/src/main/java/frc/robot/swerve/SwerveStates.java +++ b/src/main/java/frc/robot/swerve/SwerveStates.java @@ -63,6 +63,7 @@ protected static void setStates() { // backReefOffset::getY, // () -> Math.toRadians(180))); // alignToYDrive(() -> Field.fieldWidth / // 2)); + pilot.netAim_X.whileTrue(log) } /** Pilot Commands ************************************************************************ */ @@ -99,6 +100,10 @@ public static Command reefAimDrive() { .withName("Swerve.reefAimDrive"); } + public static Command netAimDrive() { + return alignToXDrive(); + } + public static Command alignToXDrive(DoubleSupplier xGoalMeters) { return resetXController() .andThen( From be81f2f713657f289fbc9dec6b5038139cbf6deb Mon Sep 17 00:00:00 2001 From: ethanli1337 Date: Wed, 26 Mar 2025 16:11:33 -0500 Subject: [PATCH 02/12] net aim command --- src/main/java/frc/robot/swerve/SwerveStates.java | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/swerve/SwerveStates.java b/src/main/java/frc/robot/swerve/SwerveStates.java index a29253b6..b17c9260 100644 --- a/src/main/java/frc/robot/swerve/SwerveStates.java +++ b/src/main/java/frc/robot/swerve/SwerveStates.java @@ -18,6 +18,7 @@ import frc.spectrumLib.SpectrumState; import frc.spectrumLib.Telemetry; import java.util.function.DoubleSupplier; +import frc.reefscape.Field; public class SwerveStates { static Swerve swerve = Robot.getSwerve(); @@ -63,7 +64,7 @@ protected static void setStates() { // backReefOffset::getY, // () -> Math.toRadians(180))); // alignToYDrive(() -> Field.fieldWidth / // 2)); - pilot.netAim_X.whileTrue(log) + pilot.netAim_X.whileTrue(log()) } /** Pilot Commands ************************************************************************ */ @@ -101,7 +102,9 @@ public static Command reefAimDrive() { } public static Command netAimDrive() { - return alignToXDrive(); + // return aimDrive(SwerveStates::getTagDistanceVelocity, + // pilot::getDriveLeftPositive, + // ); } public static Command alignToXDrive(DoubleSupplier xGoalMeters) { From 22d71dfbaf78a4d956fceb541a8c6810022cf362 Mon Sep 17 00:00:00 2001 From: ethanli1337 Date: Wed, 26 Mar 2025 16:18:06 -0500 Subject: [PATCH 03/12] Progress --- src/main/java/frc/robot/swerve/SwerveStates.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/swerve/SwerveStates.java b/src/main/java/frc/robot/swerve/SwerveStates.java index b17c9260..ab8f726f 100644 --- a/src/main/java/frc/robot/swerve/SwerveStates.java +++ b/src/main/java/frc/robot/swerve/SwerveStates.java @@ -102,9 +102,9 @@ public static Command reefAimDrive() { } public static Command netAimDrive() { - // return aimDrive(SwerveStates::getTagDistanceVelocity, - // pilot::getDriveLeftPositive, - // ); + return aimDrive(SwerveStates::getTagDistanceVelocity, + pilot::getDriveLeftPositive, + () -> 0.0); } public static Command alignToXDrive(DoubleSupplier xGoalMeters) { From d4ecbe31129c7331edd9adb3cc21e1d50388eecc Mon Sep 17 00:00:00 2001 From: ethanli1337 Date: Thu, 27 Mar 2025 15:51:33 -0500 Subject: [PATCH 04/12] binds for net aim --- src/main/java/frc/robot/pilot/Pilot.java | 1 - src/main/java/frc/robot/swerve/SwerveStates.java | 6 ++++-- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/pilot/Pilot.java b/src/main/java/frc/robot/pilot/Pilot.java index 927e265c..e122fc24 100644 --- a/src/main/java/frc/robot/pilot/Pilot.java +++ b/src/main/java/frc/robot/pilot/Pilot.java @@ -36,7 +36,6 @@ public class Pilot extends Gamepad { // vision Drive public final Trigger reefAim_A = A.and(teleop); public final Trigger cageAim_B = B.and(teleop); - public final Trigger netAim_X = X.and(teleop); // Drive Triggers public final Trigger upReorient = upDpad.and(fn, teleop); diff --git a/src/main/java/frc/robot/swerve/SwerveStates.java b/src/main/java/frc/robot/swerve/SwerveStates.java index ab8f726f..479d8b2c 100644 --- a/src/main/java/frc/robot/swerve/SwerveStates.java +++ b/src/main/java/frc/robot/swerve/SwerveStates.java @@ -14,6 +14,7 @@ import edu.wpi.first.wpilibj2.command.WaitCommand; import frc.reefscape.Field; import frc.robot.Robot; +import frc.robot.RobotStates; import frc.robot.pilot.Pilot; import frc.spectrumLib.SpectrumState; import frc.spectrumLib.Telemetry; @@ -56,6 +57,7 @@ protected static void setStates() { // // vision aim pilot.reefAim_A.whileTrue(log(reefAimDrive())); + pilot.reefAim_A.and(RobotStates.netAlgae).whileTrue(log(netAimDrive())); Pose2d backReefOffset = Field.Reef.getOffsetPosition(21, Units.inchesToMeters(24)); // pilot.cageAim_B.whileTrue( @@ -64,7 +66,7 @@ protected static void setStates() { // backReefOffset::getY, // () -> Math.toRadians(180))); // alignToYDrive(() -> Field.fieldWidth / // 2)); - pilot.netAim_X.whileTrue(log()) + } /** Pilot Commands ************************************************************************ */ @@ -102,7 +104,7 @@ public static Command reefAimDrive() { } public static Command netAimDrive() { - return aimDrive(SwerveStates::getTagDistanceVelocity, + return aimDrive(getAlignToX(() -> Field.Barge.farCage.getX()), pilot::getDriveLeftPositive, () -> 0.0); } From 7bc151084a4971afddf09420ae87513cff5f18d7 Mon Sep 17 00:00:00 2001 From: ethanli1337 Date: Thu, 27 Mar 2025 16:29:55 -0500 Subject: [PATCH 05/12] drive to net command --- src/main/java/frc/reefscape/Field.java | 2 ++ src/main/java/frc/robot/swerve/SwerveStates.java | 14 +++++++++++--- 2 files changed, 13 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/reefscape/Field.java b/src/main/java/frc/reefscape/Field.java index 316550f0..331c0c16 100644 --- a/src/main/java/frc/reefscape/Field.java +++ b/src/main/java/frc/reefscape/Field.java @@ -46,6 +46,8 @@ public static class Barge { public static final Translation2d closeCage = new Translation2d(Units.inchesToMeters(345.428), Units.inchesToMeters(199.947)); + public static final Translation2d bargeXBlue = new Translation2d(Units.inchesToMeters(325.68), Units.inchesToMeters(241.64)); + public static final Translation2d bargeXRed = new Translation2d(Field.flipXifRed(Units.inchesToMeters(325.68)), Field.flipYifRed(Units.inchesToMeters(241.64))); // Measured from floor to bottom of cage public static final double deepHeight = Units.inchesToMeters(3.125); public static final double shallowHeight = Units.inchesToMeters(30.125); diff --git a/src/main/java/frc/robot/swerve/SwerveStates.java b/src/main/java/frc/robot/swerve/SwerveStates.java index 479d8b2c..41900907 100644 --- a/src/main/java/frc/robot/swerve/SwerveStates.java +++ b/src/main/java/frc/robot/swerve/SwerveStates.java @@ -104,9 +104,17 @@ public static Command reefAimDrive() { } public static Command netAimDrive() { - return aimDrive(getAlignToX(() -> Field.Barge.farCage.getX()), - pilot::getDriveLeftPositive, - () -> 0.0); + if (Field.isBlue()) { + return aimDrive(getAlignToX(() -> Field.Barge.bargeXBlue.getX()), + pilot::getDriveLeftPositive, + pilot::getDriveCCWPositive); + } + else { + return aimDrive(getAlignToX(() -> Field.Barge.bargeXRed.getX()), + pilot::getDriveLeftPositive, + pilot::getDriveCCWPositive); + } + } public static Command alignToXDrive(DoubleSupplier xGoalMeters) { From 19f8c2281607afbb65bb1fd3dda22fecd10b67f7 Mon Sep 17 00:00:00 2001 From: ethanli1337 Date: Fri, 11 Apr 2025 16:05:18 -0500 Subject: [PATCH 06/12] net aim command finished --- simgui-window.json | 18 ++++----- .../java/frc/robot/swerve/SwerveStates.java | 38 ++++++++++--------- 2 files changed, 29 insertions(+), 27 deletions(-) diff --git a/simgui-window.json b/simgui-window.json index f32bbc28..e9c6c947 100644 --- a/simgui-window.json +++ b/simgui-window.json @@ -8,11 +8,11 @@ "GLOBAL": { "font": "Proggy Dotted", "fps": "120", - "height": "1312", + "height": "1170", "maximized": "0", "style": "0", "userScale": "2", - "width": "2253", + "width": "1924", "xpos": "0", "ypos": "38" } @@ -34,7 +34,7 @@ "###/SmartDashboard/Auto Chooser": { "Collapsed": "0", "Pos": "339,253", - "Size": "307,60" + "Size": "361,66" }, "###/SmartDashboard/Field2d": { "Collapsed": "0", @@ -74,17 +74,17 @@ "###Addressable LEDs": { "Collapsed": "0", "Pos": "585,29", - "Size": "416,220" + "Size": "491,246" }, "###FMS": { "Collapsed": "0", "Pos": "368,30", - "Size": "336,158" + "Size": "235,244" }, "###Joysticks": { "Collapsed": "0", "Pos": "6,384", - "Size": "976,252" + "Size": "1156,287" }, "###Keyboard 0 Settings": { "Collapsed": "0", @@ -127,12 +127,12 @@ "###System Joysticks": { "Collapsed": "0", "Pos": "684,642", - "Size": "232,254" + "Size": "273,290" }, "###Timing": { "Collapsed": "0", "Pos": "5,187", - "Size": "162,194" + "Size": "188,215" }, "Debug##Default": { "Collapsed": "0", @@ -142,7 +142,7 @@ "Robot State": { "Collapsed": "0", "Pos": "193,186", - "Size": "118,134" + "Size": "137,152" } } } diff --git a/src/main/java/frc/robot/swerve/SwerveStates.java b/src/main/java/frc/robot/swerve/SwerveStates.java index 11cf4b7e..1f9aefb2 100644 --- a/src/main/java/frc/robot/swerve/SwerveStates.java +++ b/src/main/java/frc/robot/swerve/SwerveStates.java @@ -17,7 +17,6 @@ import frc.spectrumLib.SpectrumState; import frc.spectrumLib.Telemetry; import java.util.function.DoubleSupplier; -import frc.reefscape.Field; public class SwerveStates { static Swerve swerve = Robot.getSwerve(); @@ -85,7 +84,7 @@ protected static void setStates() { // backReefOffset::getY, // () -> Math.toRadians(180))); // alignToYDrive(() -> Field.fieldWidth / // 2)); - + } /** Pilot Commands ************************************************************************ */ @@ -112,26 +111,29 @@ public static Command reefAimDrive() { } public static Command netAimDrive() { - if (Field.isBlue()) { - return aimDrive(getAlignToX(() -> Field.Barge.bargeXBlue.getX()), - pilot::getDriveLeftPositive, - pilot::getDriveCCWPositive); - } - else { - return aimDrive(getAlignToX(() -> Field.Barge.bargeXRed.getX()), - pilot::getDriveLeftPositive, - pilot::getDriveCCWPositive); + if (Field.isRed()) { + return alignToXDrive(() -> Field.Barge.bargeXRed.getX()); + } else { + return alignToXDrive(() -> Field.Barge.bargeXBlue.getX()); } - } public static Command alignToXDrive(DoubleSupplier xGoalMeters) { - return resetXController() - .andThen( - drive( - getAlignToX(xGoalMeters), - pilot::getDriveLeftPositive, - pilot::getDriveCCWPositive)); + if (Field.isRed()) { + return resetXController() + .andThen( + drive( + () -> -getAlignToX(xGoalMeters).getAsDouble(), + pilot::getDriveLeftPositive, + pilot::getDriveCCWPositive)); + } else { + return resetXController() + .andThen( + drive( + getAlignToX(xGoalMeters), + pilot::getDriveLeftPositive, + pilot::getDriveCCWPositive)); + } } public static Command alignToYDrive(DoubleSupplier yGoalMeters) { From e7e425769ce52a7effc77aa3339ee015105156c7 Mon Sep 17 00:00:00 2001 From: Spectrum3847 Date: Mon, 14 Apr 2025 16:32:59 -0500 Subject: [PATCH 07/12] barge align progress --- simgui-window.json | 20 +++++++++---------- src/main/java/frc/reefscape/Field.java | 8 ++++++-- .../java/frc/robot/swerve/SwerveStates.java | 17 ++++++++-------- 3 files changed, 25 insertions(+), 20 deletions(-) diff --git a/simgui-window.json b/simgui-window.json index e9c6c947..20e6c988 100644 --- a/simgui-window.json +++ b/simgui-window.json @@ -8,13 +8,13 @@ "GLOBAL": { "font": "Proggy Dotted", "fps": "120", - "height": "1170", + "height": "1415", "maximized": "0", "style": "0", "userScale": "2", - "width": "1924", + "width": "2256", "xpos": "0", - "ypos": "38" + "ypos": "29" } }, "Table": { @@ -34,7 +34,7 @@ "###/SmartDashboard/Auto Chooser": { "Collapsed": "0", "Pos": "339,253", - "Size": "361,66" + "Size": "307,60" }, "###/SmartDashboard/Field2d": { "Collapsed": "0", @@ -74,17 +74,17 @@ "###Addressable LEDs": { "Collapsed": "0", "Pos": "585,29", - "Size": "491,246" + "Size": "416,220" }, "###FMS": { "Collapsed": "0", "Pos": "368,30", - "Size": "235,244" + "Size": "210,214" }, "###Joysticks": { "Collapsed": "0", "Pos": "6,384", - "Size": "1156,287" + "Size": "976,252" }, "###Keyboard 0 Settings": { "Collapsed": "0", @@ -127,12 +127,12 @@ "###System Joysticks": { "Collapsed": "0", "Pos": "684,642", - "Size": "273,290" + "Size": "232,254" }, "###Timing": { "Collapsed": "0", "Pos": "5,187", - "Size": "188,215" + "Size": "162,194" }, "Debug##Default": { "Collapsed": "0", @@ -142,7 +142,7 @@ "Robot State": { "Collapsed": "0", "Pos": "193,186", - "Size": "137,152" + "Size": "118,134" } } } diff --git a/src/main/java/frc/reefscape/Field.java b/src/main/java/frc/reefscape/Field.java index a03d7e31..3cd146d9 100644 --- a/src/main/java/frc/reefscape/Field.java +++ b/src/main/java/frc/reefscape/Field.java @@ -49,8 +49,12 @@ public static class Barge { public static final Translation2d closeCage = new Translation2d(Units.inchesToMeters(345.428), Units.inchesToMeters(199.947)); - public static final Translation2d bargeXBlue = new Translation2d(Units.inchesToMeters(325.68), Units.inchesToMeters(241.64)); - public static final Translation2d bargeXRed = new Translation2d(Field.flipXifRed(Units.inchesToMeters(325.68)), Field.flipYifRed(Units.inchesToMeters(241.64))); + public static final Translation2d bargeXBlue = + new Translation2d(Units.inchesToMeters(325.68), Units.inchesToMeters(241.64)); + public static final Translation2d bargeXRed = + new Translation2d( + Field.flipXifRed(Units.inchesToMeters(325.68)), + Field.flipYifRed(Units.inchesToMeters(241.64))); // Measured from floor to bottom of cage public static final double deepHeight = Units.inchesToMeters(3.125); public static final double shallowHeight = Units.inchesToMeters(30.125); diff --git a/src/main/java/frc/robot/swerve/SwerveStates.java b/src/main/java/frc/robot/swerve/SwerveStates.java index 1f9aefb2..51092a0f 100644 --- a/src/main/java/frc/robot/swerve/SwerveStates.java +++ b/src/main/java/frc/robot/swerve/SwerveStates.java @@ -12,7 +12,6 @@ import frc.reefscape.HomeOffsets; import frc.reefscape.Zones; import frc.robot.Robot; -import frc.robot.RobotStates; import frc.robot.pilot.Pilot; import frc.spectrumLib.SpectrumState; import frc.spectrumLib.Telemetry; @@ -73,10 +72,10 @@ protected static void setStates() { pilot.rightReorient.onTrue(log(reorientRight())); // // vision aim - pilot.reefAim_A.whileTrue(log(reefAimDrive())); + // pilot.reefAim_A.whileTrue(log(reefAimDrive())); pilot.reefVision_A.whileTrue(log(reefAimDriveVision())); - pilot.reefAim_A.and(RobotStates.netAlgae).whileTrue(log(netAimDrive())); - + // pilot.reefAim_A.and(RobotStates.netAlgae).whileTrue(log(netAimDrive())); + pilot.reefAim_A.whileTrue(log(netAimDrive())); // Pose2d backReefOffset = Field.Reef.getOffsetPosition(21, Units.inchesToMeters(24)); // pilot.cageAim_B.whileTrue( // alignDrive( @@ -111,15 +110,17 @@ public static Command reefAimDrive() { } public static Command netAimDrive() { - if (Field.isRed()) { - return alignToXDrive(() -> Field.Barge.bargeXRed.getX()); + double poseX = Robot.getSwerve().getRobotPose().getX(); + if (poseX > Field.fieldWidth / 2) { + return alignToXDrive(() -> Field.getFieldLength() - Field.Barge.bargeXBlue.getX()); } else { return alignToXDrive(() -> Field.Barge.bargeXBlue.getX()); } } public static Command alignToXDrive(DoubleSupplier xGoalMeters) { - if (Field.isRed()) { + double poseX = Robot.getSwerve().getRobotPose().getX(); + if (poseX > Field.fieldWidth / 2) { return resetXController() .andThen( drive( @@ -130,7 +131,7 @@ public static Command alignToXDrive(DoubleSupplier xGoalMeters) { return resetXController() .andThen( drive( - getAlignToX(xGoalMeters), + () -> getAlignToX(xGoalMeters).getAsDouble(), pilot::getDriveLeftPositive, pilot::getDriveCCWPositive)); } From c114802b8a235989c2334cb11127747613fc39c7 Mon Sep 17 00:00:00 2001 From: Spectrum3847 Date: Mon, 14 Apr 2025 17:16:10 -0500 Subject: [PATCH 08/12] BargeAlign works in sim --- src/main/java/frc/reefscape/Field.java | 13 +++++++++++++ src/main/java/frc/reefscape/Zones.java | 11 +++++++++++ src/main/java/frc/robot/swerve/SwerveStates.java | 10 ++-------- 3 files changed, 26 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/reefscape/Field.java b/src/main/java/frc/reefscape/Field.java index 3cd146d9..029d2077 100644 --- a/src/main/java/frc/reefscape/Field.java +++ b/src/main/java/frc/reefscape/Field.java @@ -116,8 +116,21 @@ public static int indexOfSmallest(double[] array) { } return indexOfSmallest; } + + // TODO: Add a method to get which side of the barge the robot is on + + public static double bargeAlign(Pose2d robotPose) { + double x = Field.Barge.bargeXBlue.getX(); + double robotX = robotPose.getX(); + + if (robotX >= halfLength) { + x = bargeXRed.getX(); + } + return x; + } } + public static class CoralStation { public static final double leftFaceRobotPovDegrees = 144.011; public static final double rightFaceRobotPovDegrees = -144.011; diff --git a/src/main/java/frc/reefscape/Zones.java b/src/main/java/frc/reefscape/Zones.java index 1e43fde1..0583470b 100644 --- a/src/main/java/frc/reefscape/Zones.java +++ b/src/main/java/frc/reefscape/Zones.java @@ -39,6 +39,17 @@ public class Zones { - swerve.getConfig().getRobotLength() / 2) .and(topLeftZone); + // ------------------------------------------------------------- + // Barge Align Helper + // ------------------------------------------------------------- + + public double getBargeAlignX() { + double bargeAlignX = Field.Barge.bargeAlign(Robot.getSwerve().getRobotPose()); + + SmartDashboard.putNumber("Barge Align X", bargeAlignX); + return bargeAlignX; + } + // ------------------------------------------------------------- // Reef Offsets Helper // ------------------------------------------------------------- diff --git a/src/main/java/frc/robot/swerve/SwerveStates.java b/src/main/java/frc/robot/swerve/SwerveStates.java index 51092a0f..584dda05 100644 --- a/src/main/java/frc/robot/swerve/SwerveStates.java +++ b/src/main/java/frc/robot/swerve/SwerveStates.java @@ -110,17 +110,11 @@ public static Command reefAimDrive() { } public static Command netAimDrive() { - double poseX = Robot.getSwerve().getRobotPose().getX(); - if (poseX > Field.fieldWidth / 2) { - return alignToXDrive(() -> Field.getFieldLength() - Field.Barge.bargeXBlue.getX()); - } else { - return alignToXDrive(() -> Field.Barge.bargeXBlue.getX()); - } + return alignToXDrive(() -> zones.getBargeAlignX()); } public static Command alignToXDrive(DoubleSupplier xGoalMeters) { - double poseX = Robot.getSwerve().getRobotPose().getX(); - if (poseX > Field.fieldWidth / 2) { + if (Field.isRed()) { return resetXController() .andThen( drive( From 4212a0be3622d7548c624ce60bf46e22176056ba Mon Sep 17 00:00:00 2001 From: marcjuguilon <133045955+marcjuguilon@users.noreply.github.com> Date: Mon, 14 Apr 2025 18:39:29 -0500 Subject: [PATCH 09/12] Commit (in progress) --- src/main/java/frc/robot/swerve/SwerveStates.java | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/swerve/SwerveStates.java b/src/main/java/frc/robot/swerve/SwerveStates.java index 94a5c33a..1c6f8487 100644 --- a/src/main/java/frc/robot/swerve/SwerveStates.java +++ b/src/main/java/frc/robot/swerve/SwerveStates.java @@ -77,9 +77,8 @@ protected static void setStates() { // // vision aim // pilot.reefAim_A.whileTrue(log(reefAimDrive())); - pilot.reefVision_A.whileTrue(log(reefAimDriveVision())); // pilot.reefAim_A.and(RobotStates.netAlgae).whileTrue(log(netAimDrive())); - pilot.reefAim_A.whileTrue(log(netAimDrive())); + // pilot.reefAim_A.whileTrue(log(netAimDrive())); // pilot.reefAim_A.whileTrue(log(reefAimDrive())); // pilot.reefVision_A.whileTrue(log(reefAimDriveVisionTA())); pilot.reefVision_A.whileTrue(log(reefAimDriveVisionXY())); From 2f401b828929252c36872dff7cdea616955ea680 Mon Sep 17 00:00:00 2001 From: Spectrum3847 Date: Mon, 14 Apr 2025 19:19:21 -0500 Subject: [PATCH 10/12] resolve other merge changes --- src/main/java/frc/reefscape/Field.java | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/reefscape/Field.java b/src/main/java/frc/reefscape/Field.java index 7f7654d5..f07943a2 100644 --- a/src/main/java/frc/reefscape/Field.java +++ b/src/main/java/frc/reefscape/Field.java @@ -52,8 +52,8 @@ public static class Barge { new Translation2d(Units.inchesToMeters(325.68), Units.inchesToMeters(241.64)); public static final Translation2d bargeXRed = new Translation2d( - Field.flipXifRed(Units.inchesToMeters(325.68)), - Field.flipYifRed(Units.inchesToMeters(241.64))); + FieldHelpers.flipXifRed(Units.inchesToMeters(325.68)), + FieldHelpers.flipYifRed(Units.inchesToMeters(241.64))); // Measured from floor to bottom of cage public static final double deepHeight = Units.inchesToMeters(3.125); public static final double shallowHeight = Units.inchesToMeters(30.125); @@ -117,7 +117,7 @@ public static int indexOfSmallest(double[] array) { } // TODO: Add a method to get which side of the barge the robot is on - + public static double bargeAlign(Pose2d robotPose) { double x = Field.Barge.bargeXBlue.getX(); double robotX = robotPose.getX(); @@ -129,7 +129,6 @@ public static double bargeAlign(Pose2d robotPose) { } } - public static class CoralStation { public static final double leftFaceRobotPovDegrees = 144.011; public static final double rightFaceRobotPovDegrees = -144.011; From 46ea20010687980062ad8edac08891a34a8ddd45 Mon Sep 17 00:00:00 2001 From: ethanli1337 Date: Tue, 15 Apr 2025 16:37:59 -0500 Subject: [PATCH 11/12] Barge align fix --- simgui-window.json | 32 +++++++++---------- src/main/java/frc/reefscape/Field.java | 11 +++++-- src/main/java/frc/robot/pilot/Pilot.java | 1 + .../java/frc/robot/swerve/SwerveStates.java | 5 +-- 4 files changed, 28 insertions(+), 21 deletions(-) diff --git a/simgui-window.json b/simgui-window.json index 20e6c988..c637f91c 100644 --- a/simgui-window.json +++ b/simgui-window.json @@ -1,18 +1,18 @@ { "Docking": { "Data": [ - "DockNode ID=0x00000001 Pos=7,664 Size=905,316 Selected=0x48913727" + "DockNode ID=0x00000001 Pos=-64,877 Size=905,316 Selected=0x48913727" ] }, "MainWindow": { "GLOBAL": { "font": "Proggy Dotted", "fps": "120", - "height": "1415", + "height": "1170", "maximized": "0", "style": "0", "userScale": "2", - "width": "2256", + "width": "1924", "xpos": "0", "ypos": "29" } @@ -34,7 +34,7 @@ "###/SmartDashboard/Auto Chooser": { "Collapsed": "0", "Pos": "339,253", - "Size": "307,60" + "Size": "361,66" }, "###/SmartDashboard/Field2d": { "Collapsed": "0", @@ -48,7 +48,7 @@ }, "###/SmartDashboard/Scheduler": { "Collapsed": "0", - "Pos": "920,644", + "Pos": "414,331", "Size": "372,548" }, "###/SmartDashboard/States": { @@ -74,17 +74,17 @@ "###Addressable LEDs": { "Collapsed": "0", "Pos": "585,29", - "Size": "416,220" + "Size": "491,246" }, "###FMS": { "Collapsed": "0", - "Pos": "368,30", - "Size": "210,214" + "Pos": "562,299", + "Size": "244,244" }, "###Joysticks": { "Collapsed": "0", - "Pos": "6,384", - "Size": "976,252" + "Pos": "87,735", + "Size": "1156,287" }, "###Keyboard 0 Settings": { "Collapsed": "0", @@ -99,13 +99,13 @@ "###NetworkTables": { "Collapsed": "0", "DockId": "0x00000001,0", - "Pos": "7,664", + "Pos": "-64,877", "Size": "905,316" }, "###NetworkTables Info": { "Collapsed": "0", "DockId": "0x00000001,1", - "Pos": "7,664", + "Pos": "-64,877", "Size": "905,316" }, "###Other Devices": { @@ -126,13 +126,13 @@ }, "###System Joysticks": { "Collapsed": "0", - "Pos": "684,642", - "Size": "232,254" + "Pos": "721,421", + "Size": "273,290" }, "###Timing": { "Collapsed": "0", "Pos": "5,187", - "Size": "162,194" + "Size": "188,215" }, "Debug##Default": { "Collapsed": "0", @@ -142,7 +142,7 @@ "Robot State": { "Collapsed": "0", "Pos": "193,186", - "Size": "118,134" + "Size": "137,152" } } } diff --git a/src/main/java/frc/reefscape/Field.java b/src/main/java/frc/reefscape/Field.java index f07943a2..ae6d06ec 100644 --- a/src/main/java/frc/reefscape/Field.java +++ b/src/main/java/frc/reefscape/Field.java @@ -119,13 +119,18 @@ public static int indexOfSmallest(double[] array) { // TODO: Add a method to get which side of the barge the robot is on public static double bargeAlign(Pose2d robotPose) { - double x = Field.Barge.bargeXBlue.getX(); + double targetX = bargeXBlue.getX(); + double oppositeX = bargeXRed.getX(); + if (isRed()) { + targetX = bargeXRed.getX(); + oppositeX = bargeXBlue.getX(); + } double robotX = robotPose.getX(); if (robotX >= halfLength) { - x = bargeXRed.getX(); + targetX = Field.fieldLength - oppositeX; } - return x; + return targetX; } } diff --git a/src/main/java/frc/robot/pilot/Pilot.java b/src/main/java/frc/robot/pilot/Pilot.java index ea77b801..30ae26bf 100644 --- a/src/main/java/frc/robot/pilot/Pilot.java +++ b/src/main/java/frc/robot/pilot/Pilot.java @@ -41,6 +41,7 @@ public class Pilot extends Gamepad { public final Trigger reefVision_A = A.and(teleop); // , tagsInView); // remove tags in view public final Trigger reefAlignScore_B = B.and(teleop); // public final Trigger cageAim_B = B.and(teleop); + public final Trigger bargeAlign_A = A.and(teleop); // Drive Triggers public final Trigger upReorient = upDpad.and(fn, teleop); diff --git a/src/main/java/frc/robot/swerve/SwerveStates.java b/src/main/java/frc/robot/swerve/SwerveStates.java index 1c6f8487..e3fcf8a7 100644 --- a/src/main/java/frc/robot/swerve/SwerveStates.java +++ b/src/main/java/frc/robot/swerve/SwerveStates.java @@ -81,8 +81,9 @@ protected static void setStates() { // pilot.reefAim_A.whileTrue(log(netAimDrive())); // pilot.reefAim_A.whileTrue(log(reefAimDrive())); // pilot.reefVision_A.whileTrue(log(reefAimDriveVisionTA())); - pilot.reefVision_A.whileTrue(log(reefAimDriveVisionXY())); - pilot.reefAlignScore_B.whileTrue(log(reefAimDriveVisionXY())); + // pilot.reefVision_A.whileTrue(log(reefAimDriveVisionXY())); + // pilot.reefAlignScore_B.whileTrue(log(reefAimDriveVisionXY())); + pilot.bargeAlign_A.whileTrue(log(netAimDrive())); // Pose2d backReefOffset = Field.Reef.getOffsetPosition(21, Units.inchesToMeters(24)); // pilot.cageAim_B.whileTrue( From 0c2451a4922888d00473e02c8b8c4347f13b2732 Mon Sep 17 00:00:00 2001 From: Ethan Li Date: Tue, 15 Apr 2025 18:05:27 -0500 Subject: [PATCH 12/12] barge align state --- src/main/java/frc/robot/swerve/SwerveStates.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/swerve/SwerveStates.java b/src/main/java/frc/robot/swerve/SwerveStates.java index e3fcf8a7..440ead86 100644 --- a/src/main/java/frc/robot/swerve/SwerveStates.java +++ b/src/main/java/frc/robot/swerve/SwerveStates.java @@ -14,6 +14,7 @@ import frc.reefscape.Zones; import frc.reefscape.offsets.HomeOffsets; import frc.robot.Robot; +import frc.robot.RobotStates; import frc.robot.pilot.Pilot; import frc.spectrumLib.SpectrumState; import frc.spectrumLib.Telemetry; @@ -77,13 +78,12 @@ protected static void setStates() { // // vision aim // pilot.reefAim_A.whileTrue(log(reefAimDrive())); - // pilot.reefAim_A.and(RobotStates.netAlgae).whileTrue(log(netAimDrive())); + pilot.reefVision_A.and(RobotStates.netAlgae).whileTrue(log(netAimDrive())); // pilot.reefAim_A.whileTrue(log(netAimDrive())); // pilot.reefAim_A.whileTrue(log(reefAimDrive())); // pilot.reefVision_A.whileTrue(log(reefAimDriveVisionTA())); // pilot.reefVision_A.whileTrue(log(reefAimDriveVisionXY())); // pilot.reefAlignScore_B.whileTrue(log(reefAimDriveVisionXY())); - pilot.bargeAlign_A.whileTrue(log(netAimDrive())); // Pose2d backReefOffset = Field.Reef.getOffsetPosition(21, Units.inchesToMeters(24)); // pilot.cageAim_B.whileTrue(