diff --git a/simgui-window.json b/simgui-window.json index 82150286..c637f91c 100644 --- a/simgui-window.json +++ b/simgui-window.json @@ -1,18 +1,18 @@ { "Docking": { "Data": [ - "DockNode ID=0x00000001 Pos=34,976 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", - "maximized": "1", + "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,21 +74,21 @@ "###Addressable LEDs": { "Collapsed": "0", "Pos": "585,29", - "Size": "416,220" + "Size": "491,246" }, "###FMS": { "Collapsed": "0", - "Pos": "368,30", - "Size": "336,158" + "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", - "Pos": "910,548", + "Pos": "981,805", "Size": "300,560" }, "###Keyboard 1 Settings": { @@ -99,13 +99,13 @@ "###NetworkTables": { "Collapsed": "0", "DockId": "0x00000001,0", - "Pos": "34,976", + "Pos": "-64,877", "Size": "905,316" }, "###NetworkTables Info": { "Collapsed": "0", "DockId": "0x00000001,1", - "Pos": "34,976", + "Pos": "-64,877", "Size": "905,316" }, "###Other Devices": { @@ -126,13 +126,13 @@ }, "###System Joysticks": { "Collapsed": "0", - "Pos": "689,662", - "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 9455d34d..ae6d06ec 100644 --- a/src/main/java/frc/reefscape/Field.java +++ b/src/main/java/frc/reefscape/Field.java @@ -48,6 +48,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( + 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); @@ -97,6 +103,35 @@ public double getCageToClimb() { return Robot.getSwerve().getRobotPose().getY(); } } + + public static int indexOfSmallest(double[] array) { + int indexOfSmallest = 0; + double smallestIndex = array[indexOfSmallest]; + for (int i = 0; i < array.length; i++) { + if (array[i] <= smallestIndex) { + smallestIndex = array[i]; + indexOfSmallest = i; + } + } + return indexOfSmallest; + } + + // TODO: Add a method to get which side of the barge the robot is on + + public static double bargeAlign(Pose2d robotPose) { + double targetX = bargeXBlue.getX(); + double oppositeX = bargeXRed.getX(); + if (isRed()) { + targetX = bargeXRed.getX(); + oppositeX = bargeXBlue.getX(); + } + double robotX = robotPose.getX(); + + if (robotX >= halfLength) { + targetX = Field.fieldLength - oppositeX; + } + return targetX; + } } public static class CoralStation { diff --git a/src/main/java/frc/reefscape/Zones.java b/src/main/java/frc/reefscape/Zones.java index 1f8d2e39..80948fd8 100644 --- a/src/main/java/frc/reefscape/Zones.java +++ b/src/main/java/frc/reefscape/Zones.java @@ -62,6 +62,17 @@ public class Zones { public static final Trigger isCloseToReef = new Trigger(() -> Zones.withinReefRange(reefRangeRadius)); + // ------------------------------------------------------------- + // 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/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 5011477f..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,9 +78,12 @@ protected static void setStates() { // // vision aim // pilot.reefAim_A.whileTrue(log(reefAimDrive())); + 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.reefVision_A.whileTrue(log(reefAimDriveVisionXY())); + // pilot.reefAlignScore_B.whileTrue(log(reefAimDriveVisionXY())); // Pose2d backReefOffset = Field.Reef.getOffsetPosition(21, Units.inchesToMeters(24)); // pilot.cageAim_B.whileTrue( @@ -88,6 +92,7 @@ protected static void setStates() { // backReefOffset::getY, // () -> Math.toRadians(180))); // alignToYDrive(() -> Field.fieldWidth / // 2)); + } /** Pilot Commands ************************************************************************ */ @@ -121,13 +126,26 @@ public static Command reefAimDrive() { .withName("Swerve.reefAimDrive"); } + public static Command netAimDrive() { + return alignToXDrive(() -> zones.getBargeAlignX()); + } + 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).getAsDouble(), + pilot::getDriveLeftPositive, + pilot::getDriveCCWPositive)); + } } public static Command alignToYDrive(DoubleSupplier yGoalMeters) {