From 8c3b93df67f868fc16d013a8c8607d6aa02e2012 Mon Sep 17 00:00:00 2001 From: kirby Date: Sat, 30 Mar 2024 14:06:21 -0700 Subject: [PATCH 01/11] auto alignment --- src/main/java/frc/team2412/robot/Robot.java | 4 +- .../robot/util/auto/AutoAlignment.java | 108 ++++++++++++++++++ .../team2412/robot/util/auto/AutoLogic.java | 6 +- 3 files changed, 116 insertions(+), 2 deletions(-) create mode 100644 src/main/java/frc/team2412/robot/util/auto/AutoAlignment.java diff --git a/src/main/java/frc/team2412/robot/Robot.java b/src/main/java/frc/team2412/robot/Robot.java index 37e9f8c5..a2ce305b 100644 --- a/src/main/java/frc/team2412/robot/Robot.java +++ b/src/main/java/frc/team2412/robot/Robot.java @@ -20,6 +20,7 @@ import frc.team2412.robot.commands.diagnostic.LauncherDiagnosticCommand; import frc.team2412.robot.util.MACAddress; import frc.team2412.robot.util.MatchDashboard; +import frc.team2412.robot.util.auto.AutoAlignment; import frc.team2412.robot.util.auto.AutoLogic; public class Robot extends TimedRobot { @@ -88,7 +89,8 @@ public void robotInit() { AutoLogic.registerCommands(); if (Subsystems.SubsystemConstants.DRIVEBASE_ENABLED) { - AutoLogic.initShuffleBoard(); + AutoLogic.initShuffleboard(); + AutoAlignment.initShuffleboard(); } SmartDashboard.putString("current bot", getTypeFromAddress().toString()); diff --git a/src/main/java/frc/team2412/robot/util/auto/AutoAlignment.java b/src/main/java/frc/team2412/robot/util/auto/AutoAlignment.java new file mode 100644 index 00000000..0acedd14 --- /dev/null +++ b/src/main/java/frc/team2412/robot/util/auto/AutoAlignment.java @@ -0,0 +1,108 @@ +package frc.team2412.robot.util.auto; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; +import edu.wpi.first.wpilibj.smartdashboard.FieldObject2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc.team2412.robot.Robot; +import frc.team2412.robot.Subsystems; +import java.util.function.Supplier; + +public class AutoAlignment { + private static Robot r = Robot.getInstance(); + private static final Subsystems s = r.subsystems; + + private static Supplier currentPosition = s.drivebaseWrapper::getEstimatedPosition; + private static Supplier goalPosition = () -> AutoLogic.getSelectedAutoPath().getStartPose2d(); + + private static final double POSITION_TOLERANCE = 0.5; + private static final double ROTATION_TOLERANCE = 5; + + private static Field2d field = new Field2d(); + private static FieldObject2d robotPos; + private static FieldObject2d robotGoalPos; + private static ShuffleboardTab tab = Shuffleboard.getTab("AutoAlignment"); + + public static void initShuffleboard() { + + tab.addString("Current Position", () -> getPose2dString(currentPosition.get())) + .withPosition(0, 0) + .withSize(3, 1); + + tab.addString("Auto Starting Position", () -> getPose2dString(goalPosition.get())) + .withPosition(0, 1) + .withSize(3, 1); + + tab.addBoolean( + "Correct Horizontal Position", AutoAlignment::isRobotInCorrectHorizontalPosition) + .withPosition(3, 0) + .withSize(2, 1); + tab.addBoolean("Correct Vertical Position", AutoAlignment::isRobotInCorrectVerticalPosition) + .withPosition(3, 1) + .withSize(2, 1); + tab.addBoolean("Correct Rotation", AutoAlignment::isRobotInCorrectHorizontalPosition) + .withPosition(3, 2) + .withSize(2, 1); + + tab.addBoolean("Correct Position", AutoAlignment::isRobotInCorrectPosition) + .withPosition(5, 0) + .withSize(2, 3); + + tab.add(field).withPosition(7, 0); + + initField(); + } + + private static boolean isRobotInCorrectPosition() { + return (isRobotInCorrectHorizontalPosition() + && isRobotInCorrectVerticalPosition() + && isRobotInCorrectRotation()); + } + + private static boolean isRobotInCorrectHorizontalPosition() { + return MathUtil.isNear( + currentPosition.get().getX(), goalPosition.get().getX(), POSITION_TOLERANCE); + } + + private static boolean isRobotInCorrectVerticalPosition() { + return MathUtil.isNear( + currentPosition.get().getY(), goalPosition.get().getY(), POSITION_TOLERANCE); + } + + private static boolean isRobotInCorrectRotation() { + return MathUtil.isNear( + currentPosition.get().getRotation().getDegrees(), + goalPosition.get().getRotation().getDegrees(), + ROTATION_TOLERANCE); + } + + private static String getPose2dString(Pose2d pose) { + return "Translation: (" + + pose.getX() + + "m, " + + pose.getY() + + "m) | Rotation: (" + + pose.getRotation().getDegrees() + + " deg.)"; + } + + + private static void initField() { + field = new Field2d(); + + SmartDashboard.putData("AlignmentField", field); + tab.add(field).withPosition(7, 0).withSize(4, 3); + + robotPos = field.getObject("Robot"); + robotGoalPos = field.getObject("AutoStartPosition"); + } + + private static void updateField() { + robotPos.setPose(s.drivebaseWrapper.getEstimatedPosition()); + + robotGoalPos.setPose(AutoLogic.getSelectedAutoPath().getStartPose2d()); + } +} diff --git a/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java b/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java index e6e8e858..fbabb63b 100644 --- a/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java +++ b/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java @@ -265,7 +265,7 @@ public static Command getAutoCommand(String pathName) { return AutoBuilder.followPath(path); } - public static void initShuffleBoard() { + public static void initShuffleboard() { startPositionChooser.setDefaultOption(StartPosition.MISC.title, StartPosition.MISC); for (StartPosition startPosition : StartPosition.values()) { @@ -356,4 +356,8 @@ public static double getEstimatedAutoDuration() { } return 0; } + + public static AutoPath getSelectedAutoPath() { + return availableAutos.getSelected(); + } } From 782ac95887da13afaafd64da1ed463f2a94d7236 Mon Sep 17 00:00:00 2001 From: kirby Date: Sat, 30 Mar 2024 14:45:08 -0700 Subject: [PATCH 02/11] it workes yippee --- src/main/java/frc/team2412/robot/Robot.java | 9 +++++++++ .../java/frc/team2412/robot/util/auto/AutoAlignment.java | 6 ++++-- .../java/frc/team2412/robot/util/auto/AutoLogic.java | 2 +- 3 files changed, 14 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/team2412/robot/Robot.java b/src/main/java/frc/team2412/robot/Robot.java index a2ce305b..fb3a707e 100644 --- a/src/main/java/frc/team2412/robot/Robot.java +++ b/src/main/java/frc/team2412/robot/Robot.java @@ -1,5 +1,7 @@ package frc.team2412.robot; +import static frc.team2412.robot.Subsystems.SubsystemConstants.DRIVEBASE_ENABLED; + import com.ctre.phoenix6.SignalLogger; import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj.DriverStation; @@ -182,6 +184,13 @@ public void disabledInit() { coastCommand.schedule(); } + @Override + public void disabledPeriodic() { + if (DRIVEBASE_ENABLED) { + AutoAlignment.updateField(); + } + } + @Override public void disabledExit() { subsystems.drivebaseSubsystem.setMotorBrake(true); diff --git a/src/main/java/frc/team2412/robot/util/auto/AutoAlignment.java b/src/main/java/frc/team2412/robot/util/auto/AutoAlignment.java index 0acedd14..a96ed251 100644 --- a/src/main/java/frc/team2412/robot/util/auto/AutoAlignment.java +++ b/src/main/java/frc/team2412/robot/util/auto/AutoAlignment.java @@ -94,13 +94,15 @@ private static void initField() { field = new Field2d(); SmartDashboard.putData("AlignmentField", field); - tab.add(field).withPosition(7, 0).withSize(4, 3); + tab.add("Alignment Field", field).withPosition(7, 0).withSize(4, 3); robotPos = field.getObject("Robot"); robotGoalPos = field.getObject("AutoStartPosition"); + + updateField(); } - private static void updateField() { + public static void updateField() { robotPos.setPose(s.drivebaseWrapper.getEstimatedPosition()); robotGoalPos.setPose(AutoLogic.getSelectedAutoPath().getStartPose2d()); diff --git a/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java b/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java index fbabb63b..47834c43 100644 --- a/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java +++ b/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java @@ -358,6 +358,6 @@ public static double getEstimatedAutoDuration() { } public static AutoPath getSelectedAutoPath() { - return availableAutos.getSelected(); + return (chooserHasAutoSelected() ? availableAutos.getSelected() : defaultPath); } } From 17f48185020a7b94b30ce683bf587b3eb9da0d2f Mon Sep 17 00:00:00 2001 From: kirby Date: Sat, 30 Mar 2024 14:51:52 -0700 Subject: [PATCH 03/11] spotless boohoo --- .../java/frc/team2412/robot/util/auto/AutoAlignment.java | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/team2412/robot/util/auto/AutoAlignment.java b/src/main/java/frc/team2412/robot/util/auto/AutoAlignment.java index a96ed251..7e6e9f6c 100644 --- a/src/main/java/frc/team2412/robot/util/auto/AutoAlignment.java +++ b/src/main/java/frc/team2412/robot/util/auto/AutoAlignment.java @@ -16,7 +16,8 @@ public class AutoAlignment { private static final Subsystems s = r.subsystems; private static Supplier currentPosition = s.drivebaseWrapper::getEstimatedPosition; - private static Supplier goalPosition = () -> AutoLogic.getSelectedAutoPath().getStartPose2d(); + private static Supplier goalPosition = + () -> AutoLogic.getSelectedAutoPath().getStartPose2d(); private static final double POSITION_TOLERANCE = 0.5; private static final double ROTATION_TOLERANCE = 5; @@ -36,8 +37,7 @@ public static void initShuffleboard() { .withPosition(0, 1) .withSize(3, 1); - tab.addBoolean( - "Correct Horizontal Position", AutoAlignment::isRobotInCorrectHorizontalPosition) + tab.addBoolean("Correct Horizontal Position", AutoAlignment::isRobotInCorrectHorizontalPosition) .withPosition(3, 0) .withSize(2, 1); tab.addBoolean("Correct Vertical Position", AutoAlignment::isRobotInCorrectVerticalPosition) @@ -89,7 +89,6 @@ private static String getPose2dString(Pose2d pose) { + " deg.)"; } - private static void initField() { field = new Field2d(); From 9ea898a2d751a1b65c2ab8730e3a83e510365253 Mon Sep 17 00:00:00 2001 From: kirbt <91921906+kirbt@users.noreply.github.com> Date: Tue, 2 Apr 2024 13:46:14 -0700 Subject: [PATCH 04/11] lower alignment tolerances --- src/main/java/frc/team2412/robot/util/auto/AutoAlignment.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/team2412/robot/util/auto/AutoAlignment.java b/src/main/java/frc/team2412/robot/util/auto/AutoAlignment.java index 7e6e9f6c..a7db3aaa 100644 --- a/src/main/java/frc/team2412/robot/util/auto/AutoAlignment.java +++ b/src/main/java/frc/team2412/robot/util/auto/AutoAlignment.java @@ -19,8 +19,8 @@ public class AutoAlignment { private static Supplier goalPosition = () -> AutoLogic.getSelectedAutoPath().getStartPose2d(); - private static final double POSITION_TOLERANCE = 0.5; - private static final double ROTATION_TOLERANCE = 5; + private static final double POSITION_TOLERANCE = 0.0254; // meters + private static final double ROTATION_TOLERANCE = 3; // degrees private static Field2d field = new Field2d(); private static FieldObject2d robotPos; From 1563ec86c9bd52f1842dce07199d7b86f56fff42 Mon Sep 17 00:00:00 2001 From: kirbt <91921906+kirbt@users.noreply.github.com> Date: Tue, 2 Apr 2024 13:54:30 -0700 Subject: [PATCH 05/11] merge conflict debug + spotless --- src/main/java/frc/team2412/robot/Robot.java | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/team2412/robot/Robot.java b/src/main/java/frc/team2412/robot/Robot.java index 68b6d434..c8258d00 100644 --- a/src/main/java/frc/team2412/robot/Robot.java +++ b/src/main/java/frc/team2412/robot/Robot.java @@ -94,10 +94,9 @@ public void robotInit() { if (Subsystems.SubsystemConstants.DRIVEBASE_ENABLED) { if (autoEnabled) { - AutoLogic.initShuffleBoard(); - AutoAlignment.initShuffleboard(); + AutoLogic.initShuffleboard(); + AutoAlignment.initShuffleboard(); } - } SmartDashboard.putString("current bot", getTypeFromAddress().toString()); From b1e0732f4c6e127ef5cd57ba7bb784b5c566c7ab Mon Sep 17 00:00:00 2001 From: kirbt <91921906+kirbt@users.noreply.github.com> Date: Tue, 2 Apr 2024 13:57:05 -0700 Subject: [PATCH 06/11] enabled checker for alignment --- src/main/java/frc/team2412/robot/Robot.java | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/team2412/robot/Robot.java b/src/main/java/frc/team2412/robot/Robot.java index c8258d00..442b1fb4 100644 --- a/src/main/java/frc/team2412/robot/Robot.java +++ b/src/main/java/frc/team2412/robot/Robot.java @@ -1,5 +1,6 @@ package frc.team2412.robot; +import static frc.team2412.robot.Subsystems.SubsystemConstants.APRILTAGS_ENABLED; import static frc.team2412.robot.Subsystems.SubsystemConstants.DRIVEBASE_ENABLED; import com.ctre.phoenix6.SignalLogger; @@ -95,7 +96,9 @@ public void robotInit() { if (Subsystems.SubsystemConstants.DRIVEBASE_ENABLED) { if (autoEnabled) { AutoLogic.initShuffleboard(); - AutoAlignment.initShuffleboard(); + if (APRILTAGS_ENABLED) { + AutoAlignment.initShuffleboard(); + } } } @@ -193,7 +196,7 @@ public void disabledInit() { @Override public void disabledPeriodic() { - if (DRIVEBASE_ENABLED) { + if (DRIVEBASE_ENABLED && APRILTAGS_ENABLED && autoEnabled) { AutoAlignment.updateField(); } } From bdb3202df7eeadc1393546257eab7932050d9d21 Mon Sep 17 00:00:00 2001 From: kirbt <91921906+kirbt@users.noreply.github.com> Date: Tue, 2 Apr 2024 17:38:03 -0700 Subject: [PATCH 07/11] Update src/main/java/frc/team2412/robot/util/auto/AutoAlignment.java Co-authored-by: Joseph Eng <91924258+KangarooKoala@users.noreply.github.com> --- src/main/java/frc/team2412/robot/util/auto/AutoAlignment.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/team2412/robot/util/auto/AutoAlignment.java b/src/main/java/frc/team2412/robot/util/auto/AutoAlignment.java index a7db3aaa..87fa375a 100644 --- a/src/main/java/frc/team2412/robot/util/auto/AutoAlignment.java +++ b/src/main/java/frc/team2412/robot/util/auto/AutoAlignment.java @@ -43,7 +43,7 @@ public static void initShuffleboard() { tab.addBoolean("Correct Vertical Position", AutoAlignment::isRobotInCorrectVerticalPosition) .withPosition(3, 1) .withSize(2, 1); - tab.addBoolean("Correct Rotation", AutoAlignment::isRobotInCorrectHorizontalPosition) + tab.addBoolean("Correct Rotation", AutoAlignment::isRobotInCorrectRotation) .withPosition(3, 2) .withSize(2, 1); From 4e998232b3cf56660edcbd504c7429618347ee1b Mon Sep 17 00:00:00 2001 From: kirbt <91921906+kirbt@users.noreply.github.com> Date: Tue, 2 Apr 2024 17:39:06 -0700 Subject: [PATCH 08/11] address pr --- src/main/java/frc/team2412/robot/util/auto/AutoAlignment.java | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/main/java/frc/team2412/robot/util/auto/AutoAlignment.java b/src/main/java/frc/team2412/robot/util/auto/AutoAlignment.java index a7db3aaa..0a1de3f9 100644 --- a/src/main/java/frc/team2412/robot/util/auto/AutoAlignment.java +++ b/src/main/java/frc/team2412/robot/util/auto/AutoAlignment.java @@ -51,8 +51,6 @@ public static void initShuffleboard() { .withPosition(5, 0) .withSize(2, 3); - tab.add(field).withPosition(7, 0); - initField(); } From 081a031128a51c283e76fa0d035c99dcf340ea02 Mon Sep 17 00:00:00 2001 From: kirbt <91921906+kirbt@users.noreply.github.com> Date: Tue, 2 Apr 2024 17:42:49 -0700 Subject: [PATCH 09/11] a a --- src/main/java/frc/team2412/robot/Robot.java | 15 +++++++-------- 1 file changed, 7 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/team2412/robot/Robot.java b/src/main/java/frc/team2412/robot/Robot.java index 442b1fb4..ca91312a 100644 --- a/src/main/java/frc/team2412/robot/Robot.java +++ b/src/main/java/frc/team2412/robot/Robot.java @@ -93,12 +93,11 @@ public void robotInit() { AutoLogic.registerCommands(); } - if (Subsystems.SubsystemConstants.DRIVEBASE_ENABLED) { - if (autoEnabled) { - AutoLogic.initShuffleboard(); - if (APRILTAGS_ENABLED) { - AutoAlignment.initShuffleboard(); - } + if (Subsystems.SubsystemConstants.DRIVEBASE_ENABLED && autoEnabled) { + AutoLogic.initShuffleboard(); + + if (APRILTAGS_ENABLED) { + AutoAlignment.initShuffleboard(); } } @@ -153,8 +152,8 @@ public void autonomousInit() { // Checks if FMS is attatched and enables joystick warning if true DriverStation.silenceJoystickConnectionWarning(!DriverStation.isFMSAttached()); // System.out.println(AutoLogic.getSelected() != null); - if (autoEnabled) { - if (AutoLogic.getSelectedAuto() != null && SubsystemConstants.DRIVEBASE_ENABLED) { + if (autoEnabled && SubsystemConstants.DRIVEBASE_ENABLED) { + if (AutoLogic.getSelectedAuto() != null) { AutoLogic.getSelectedAuto().schedule(); } } From bd7fca9f72f1357930c13463b294f8d00b3b93c8 Mon Sep 17 00:00:00 2001 From: kirbt <91921906+kirbt@users.noreply.github.com> Date: Tue, 2 Apr 2024 17:49:40 -0700 Subject: [PATCH 10/11] spotless --- src/main/java/frc/team2412/robot/Robot.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/team2412/robot/Robot.java b/src/main/java/frc/team2412/robot/Robot.java index ca91312a..d5089550 100644 --- a/src/main/java/frc/team2412/robot/Robot.java +++ b/src/main/java/frc/team2412/robot/Robot.java @@ -95,7 +95,7 @@ public void robotInit() { if (Subsystems.SubsystemConstants.DRIVEBASE_ENABLED && autoEnabled) { AutoLogic.initShuffleboard(); - + if (APRILTAGS_ENABLED) { AutoAlignment.initShuffleboard(); } From ec8fa8f9a08d8f5f44d95f441b576ec3a8ca84aa Mon Sep 17 00:00:00 2001 From: kirbt <91921906+kirbt@users.noreply.github.com> Date: Tue, 2 Apr 2024 18:28:41 -0700 Subject: [PATCH 11/11] spotless + noob gaming merge --- src/main/java/frc/team2412/robot/util/auto/AutoLogic.java | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java b/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java index 9664a6c1..a370a14c 100644 --- a/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java +++ b/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java @@ -342,10 +342,9 @@ public static double getEstimatedAutoDuration() { return 0; } - public static AutoPath getSelectedAutoPath() { return (chooserHasAutoSelected() ? availableAutos.getSelected() : defaultPath); - + } // commands util public static BooleanSupplier isReadyToLaunch() { @@ -514,6 +513,5 @@ public static Command index() { public static Command stopFeeder() { return (INTAKE_ENABLED ? new FeederStopCommand(s.intakeSubsystem) : Commands.none()) .withName("Auto - StopFeeder"); - } }