From 91e94f9f923f6812ea580efaff4ac9c8dca135e9 Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Sat, 22 Feb 2025 19:22:50 -0500 Subject: [PATCH 01/99] initial commit --- .../alignment/AlignClosestBranch.java | 43 ++++++++++++++++++ .../robot/constants/AlignmentPositions.java | 44 +++++++++++++++++++ .../frc/robot/constants/BranchPositions.java | 37 ++++++++++++++++ .../frc/robot/constants/GameConstants.java | 1 + 4 files changed, 125 insertions(+) create mode 100644 src/main/java/frc/robot/commands/alignment/AlignClosestBranch.java create mode 100644 src/main/java/frc/robot/constants/AlignmentPositions.java create mode 100644 src/main/java/frc/robot/constants/BranchPositions.java diff --git a/src/main/java/frc/robot/commands/alignment/AlignClosestBranch.java b/src/main/java/frc/robot/commands/alignment/AlignClosestBranch.java new file mode 100644 index 00000000..08c826af --- /dev/null +++ b/src/main/java/frc/robot/commands/alignment/AlignClosestBranch.java @@ -0,0 +1,43 @@ +package frc.robot.commands.alignment; + +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.path.GoalEndState; +import com.pathplanner.lib.path.PathConstraints; +import com.pathplanner.lib.path.PathPlannerPath; +import com.pathplanner.lib.path.Waypoint; +import edu.wpi.first.math.geometry.Pose2d; +import frc.robot.constants.AlignmentPositions; +import frc.robot.constants.Constants; +import frc.robot.subsystems.swervev3.SwerveDrivetrain; +import frc.robot.utils.logging.commands.LoggableCommand; +import frc.robot.utils.logging.commands.LoggableCommandWrapper; +import java.util.List; + +public class AlignClosestBranch extends LoggableCommand { + private Pose2d targetPosition; + private final SwerveDrivetrain drivetrain; + + public AlignClosestBranch(SwerveDrivetrain drivetrain) { + this.drivetrain = drivetrain; + } + + @Override + public void initialize() { + targetPosition = AlignmentPositions.getClosest(drivetrain.getPose()); + List waypoints = PathPlannerPath.waypointsFromPoses(targetPosition); + PathConstraints constraints = + new PathConstraints(3.0, 3.0, 2 * Math.PI, 4 * Math.PI); // The constraints for this path. + PathPlannerPath path = + new PathPlannerPath( + waypoints, constraints, null, new GoalEndState(0.0, targetPosition.getRotation())); + LoggableCommandWrapper.wrap(AutoBuilder.followPath(path)); + } + + @Override + public boolean isFinished() { + return (Math.sqrt( + Math.pow(drivetrain.getPose().minus(targetPosition).getX(), 2) + + Math.pow(drivetrain.getPose().minus(targetPosition).getY(), 2)) + < Constants.ALIGNMENT_DISTANCE_THRESHOLD); + } +} diff --git a/src/main/java/frc/robot/constants/AlignmentPositions.java b/src/main/java/frc/robot/constants/AlignmentPositions.java new file mode 100644 index 00000000..99f2775d --- /dev/null +++ b/src/main/java/frc/robot/constants/AlignmentPositions.java @@ -0,0 +1,44 @@ +package frc.robot.constants; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import java.util.ArrayList; +import java.util.List; + +public enum AlignmentPositions { + BRANCH_A(new Pose2d(3.18849375, 4.0739863148, Rotation2d.fromDegrees(0))), + BRANCH_B(new Pose2d(3.18849375, 3.7238597608, Rotation2d.fromDegrees(0))), + BRANCH_C(new Pose2d(3.79133815262887167703, 2.93370262638828539811, Rotation2d.fromDegrees(60))), + BRANCH_D(new Pose2d(4.09455664293237573925, 2.75863934938828539811, Rotation2d.fromDegrees(60))), + BRANCH_E(new Pose2d(5.08027499562887167703, 2.88563934938828539811, Rotation2d.fromDegrees(120))), + BRANCH_F(new Pose2d(5.38349348593237573925, 3.06070262638828539811, Rotation2d.fromDegrees(120))), + BRANCH_G(new Pose2d(5.766367436, 3.9778597608, Rotation2d.fromDegrees(180))), + BRANCH_H(new Pose2d(5.766367436, 4.3279863148, Rotation2d.fromDegrees(180))), + BRANCH_I(new Pose2d(5.16352303337112832297, 5.11814344921171460189, Rotation2d.fromDegrees(240))), + BRANCH_J(new Pose2d(4.86030454306762426075, 5.29320672621171460189, Rotation2d.fromDegrees(240))), + BRANCH_K(new Pose2d(3.87458619037112832297, 5.16620672621171460189, Rotation2d.fromDegrees(300))), + BRANCH_L( + new Pose2d( + 3.57136770006762426075, + 4.99114344921171460189, + Rotation2d.fromDegrees(300))); // TODO: ALL PLACEHOLDERS + + private final Pose2d position; + + AlignmentPositions(Pose2d position) { + this.position = position; + } + + public Pose2d getPosition() { + return position; + } + + public static Pose2d getClosest(Pose2d currentPosition) { + AlignmentPositions[] listEnums = AlignmentPositions.values(); + List listPose2d = new ArrayList<>(); + for (AlignmentPositions position : listEnums) { + listPose2d.add(position.getPosition()); + } + return currentPosition.nearest(listPose2d); + } +} diff --git a/src/main/java/frc/robot/constants/BranchPositions.java b/src/main/java/frc/robot/constants/BranchPositions.java new file mode 100644 index 00000000..b1e94bda --- /dev/null +++ b/src/main/java/frc/robot/constants/BranchPositions.java @@ -0,0 +1,37 @@ +package frc.robot.constants; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; + +// Technically not used now, mainly just for checking if the math is right +public enum BranchPositions { + CENTER(new Pose2d(4.477430593, 4.0259230378, Rotation2d.fromDegrees(0.0))), + CENTER_A_B(new Pose2d(3.64569375, 4.0259230378, Rotation2d.fromDegrees(0))), + CENTER_C_D(new Pose2d(4.0615621715, 3.30561780249853074741, Rotation2d.fromDegrees(60))), + CENTER_E_F(new Pose2d(4.8932990145, 3.30561780249853074741, Rotation2d.fromDegrees(120))), + CENTER_G_H(new Pose2d(5.309167436, 4.0259230378, Rotation2d.fromDegrees(180))), + CENTER_I_J(new Pose2d(4.8932990145, 4.74622827310146925259, Rotation2d.fromDegrees(240))), + CENTER_K_L(new Pose2d(4.0615621715, 4.74622827310146925259, Rotation2d.fromDegrees(300))), + BRANCH_A(new Pose2d(3.64569375, 4.2009863148, Rotation2d.fromDegrees(0))), + BRANCH_B(new Pose2d(3.64569375, 3.8508597608, Rotation2d.fromDegrees(0))), + BRANCH_C(new Pose2d(3.90995292634824796889, 3.39314944099853074741, Rotation2d.fromDegrees(60))), + BRANCH_D(new Pose2d(4.21317141665175203111, 3.21808616399853074741, Rotation2d.fromDegrees(60))), + BRANCH_E(new Pose2d(4.74168976934824796889, 3.21808616399853074741, Rotation2d.fromDegrees(120))), + BRANCH_F(new Pose2d(5.04490825965175203111, 3.39314944099853074741, Rotation2d.fromDegrees(120))), + BRANCH_G(new Pose2d(5.309167436, 3.8508597608, Rotation2d.fromDegrees(180))), + BRANCH_H(new Pose2d(5.309167436, 4.2009863148, Rotation2d.fromDegrees(180))), + BRANCH_I(new Pose2d(5.04490825965175203111, 4.65869663460146925259, Rotation2d.fromDegrees(240))), + BRANCH_J(new Pose2d(4.74168976934824796889, 4.83375991160146925259, Rotation2d.fromDegrees(240))), + BRANCH_K(new Pose2d(4.21317141665175203111, 4.83375991160146925259, Rotation2d.fromDegrees(300))), + BRANCH_L(new Pose2d(3.90995292634824796889, 4.65869663460146925259, Rotation2d.fromDegrees(300))); + + private final Pose2d position; + + BranchPositions(Pose2d position) { + this.position = position; + } + + public Pose2d getPosition() { + return position; + } +} diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java index eceaaa95..5a3a143f 100644 --- a/src/main/java/frc/robot/constants/GameConstants.java +++ b/src/main/java/frc/robot/constants/GameConstants.java @@ -132,6 +132,7 @@ public enum Mode { public static final double MAX_ELEVATOR_HEIGHT_METERS = -200; // in m public static final double INITIAL_ELEVATOR_HEIGHT = 0; // TODO: change later public static final double HIHI_LENGTH = 0.5; // TODO: change later + public static final double ALIGNMENT_DISTANCE_THRESHOLD = 0.05; // TODO: change later // Angles public static final Rotation2d HIHI_MIN_ANGLE = From e8cde9463eb6b6a3e5a1d637990ff6841c4ffc08 Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Sat, 22 Feb 2025 19:42:40 -0500 Subject: [PATCH 02/99] fixed pr comments --- .../alignment/AlignClosestBranch.java | 15 +------ .../robot/constants/AlignmentPositions.java | 11 ++--- .../robot/utils/auto/PathPlannerUtils.java | 42 +++++++++++++++++++ 3 files changed, 48 insertions(+), 20 deletions(-) create mode 100644 src/main/java/frc/robot/utils/auto/PathPlannerUtils.java diff --git a/src/main/java/frc/robot/commands/alignment/AlignClosestBranch.java b/src/main/java/frc/robot/commands/alignment/AlignClosestBranch.java index 08c826af..1c346227 100644 --- a/src/main/java/frc/robot/commands/alignment/AlignClosestBranch.java +++ b/src/main/java/frc/robot/commands/alignment/AlignClosestBranch.java @@ -1,17 +1,12 @@ package frc.robot.commands.alignment; -import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.path.GoalEndState; -import com.pathplanner.lib.path.PathConstraints; -import com.pathplanner.lib.path.PathPlannerPath; -import com.pathplanner.lib.path.Waypoint; import edu.wpi.first.math.geometry.Pose2d; import frc.robot.constants.AlignmentPositions; import frc.robot.constants.Constants; import frc.robot.subsystems.swervev3.SwerveDrivetrain; +import frc.robot.utils.auto.PathPlannerUtils; import frc.robot.utils.logging.commands.LoggableCommand; import frc.robot.utils.logging.commands.LoggableCommandWrapper; -import java.util.List; public class AlignClosestBranch extends LoggableCommand { private Pose2d targetPosition; @@ -24,13 +19,7 @@ public AlignClosestBranch(SwerveDrivetrain drivetrain) { @Override public void initialize() { targetPosition = AlignmentPositions.getClosest(drivetrain.getPose()); - List waypoints = PathPlannerPath.waypointsFromPoses(targetPosition); - PathConstraints constraints = - new PathConstraints(3.0, 3.0, 2 * Math.PI, 4 * Math.PI); // The constraints for this path. - PathPlannerPath path = - new PathPlannerPath( - waypoints, constraints, null, new GoalEndState(0.0, targetPosition.getRotation())); - LoggableCommandWrapper.wrap(AutoBuilder.followPath(path)); + LoggableCommandWrapper.wrap(PathPlannerUtils.pathToPose(targetPosition, 0.0)); } @Override diff --git a/src/main/java/frc/robot/constants/AlignmentPositions.java b/src/main/java/frc/robot/constants/AlignmentPositions.java index 99f2775d..6a2e52a2 100644 --- a/src/main/java/frc/robot/constants/AlignmentPositions.java +++ b/src/main/java/frc/robot/constants/AlignmentPositions.java @@ -2,7 +2,7 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; -import java.util.ArrayList; +import java.util.Arrays; import java.util.List; public enum AlignmentPositions { @@ -34,11 +34,8 @@ public Pose2d getPosition() { } public static Pose2d getClosest(Pose2d currentPosition) { - AlignmentPositions[] listEnums = AlignmentPositions.values(); - List listPose2d = new ArrayList<>(); - for (AlignmentPositions position : listEnums) { - listPose2d.add(position.getPosition()); - } - return currentPosition.nearest(listPose2d); + List poseList = + Arrays.stream(AlignmentPositions.values()).map(AlignmentPositions::getPosition).toList(); + return currentPosition.nearest(poseList); } } diff --git a/src/main/java/frc/robot/utils/auto/PathPlannerUtils.java b/src/main/java/frc/robot/utils/auto/PathPlannerUtils.java new file mode 100644 index 00000000..29330bf0 --- /dev/null +++ b/src/main/java/frc/robot/utils/auto/PathPlannerUtils.java @@ -0,0 +1,42 @@ +package frc.robot.utils.auto; + +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.path.GoalEndState; +import com.pathplanner.lib.path.PathConstraints; +import com.pathplanner.lib.path.PathPlannerPath; +import com.pathplanner.lib.path.Waypoint; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.constants.Constants; +import frc.robot.utils.logging.commands.LoggableCommandWrapper; +import java.util.List; + +public class PathPlannerUtils { + private static final PathConstraints defualtPathConstraints = + new PathConstraints( + Constants.MAX_VELOCITY, + Constants.MAX_VELOCITY, + Math.toRadians(1000), + Math.toRadians(1000)); + + public static PathPlannerPath createManualPath( + Pose2d startPose, Pose2d targetPos, double endVelocity) { + List waypoints = PathPlannerPath.waypointsFromPoses(startPose, targetPos); + PathPlannerPath path = + new PathPlannerPath( + waypoints, + defualtPathConstraints, + null, + new GoalEndState(endVelocity, targetPos.getRotation())); + path.preventFlipping = true; + return path; + } + + public static LoggableCommandWrapper autoFromPath(PathPlannerPath path) { + return LoggableCommandWrapper.wrap(AutoBuilder.followPath(path)); + } + + public static Command pathToPose(Pose2d targetPos, double endVelocity) { + return AutoBuilder.pathfindToPose(targetPos, defualtPathConstraints, endVelocity); + } +} From cecb84a98d0244f01a3ffee45c1ce62a93cdb860 Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Sat, 22 Feb 2025 20:30:27 -0500 Subject: [PATCH 03/99] fixed pr comments --- .../alignment/AlignClosestBranch.java | 5 +- .../java/frc/robot/utils/math/PoseUtils.java | 186 ++++++++++++++++++ 2 files changed, 188 insertions(+), 3 deletions(-) create mode 100644 src/main/java/frc/robot/utils/math/PoseUtils.java diff --git a/src/main/java/frc/robot/commands/alignment/AlignClosestBranch.java b/src/main/java/frc/robot/commands/alignment/AlignClosestBranch.java index 1c346227..3da92512 100644 --- a/src/main/java/frc/robot/commands/alignment/AlignClosestBranch.java +++ b/src/main/java/frc/robot/commands/alignment/AlignClosestBranch.java @@ -7,6 +7,7 @@ import frc.robot.utils.auto.PathPlannerUtils; import frc.robot.utils.logging.commands.LoggableCommand; import frc.robot.utils.logging.commands.LoggableCommandWrapper; +import frc.robot.utils.math.PoseUtils; public class AlignClosestBranch extends LoggableCommand { private Pose2d targetPosition; @@ -24,9 +25,7 @@ public void initialize() { @Override public boolean isFinished() { - return (Math.sqrt( - Math.pow(drivetrain.getPose().minus(targetPosition).getX(), 2) - + Math.pow(drivetrain.getPose().minus(targetPosition).getY(), 2)) + return (PoseUtils.getDistance(drivetrain.getPose(), targetPosition) < Constants.ALIGNMENT_DISTANCE_THRESHOLD); } } diff --git a/src/main/java/frc/robot/utils/math/PoseUtils.java b/src/main/java/frc/robot/utils/math/PoseUtils.java new file mode 100644 index 00000000..57d2a240 --- /dev/null +++ b/src/main/java/frc/robot/utils/math/PoseUtils.java @@ -0,0 +1,186 @@ +package frc.robot.utils.math; + +import edu.wpi.first.math.geometry.*; + +public class PoseUtils { + /** + * Estimates a future position based on the current position and velocities + * + * @param pose current pose + * @param vx x velocity + * @param vy y velocity + * @param time time over which velocity is applied + * @return Estimated position + */ + public static Pose2d getFieldEstimatedFuturePose(Pose2d pose, double vx, double vy, double time) { + return getFieldEstimatedFuturePose(pose, vx, vy, 0.0, time); + } + + /** + * Estimates a future position based on the current position and velocities + * + * @param pose current pose + * @param vx x velocity + * @param vy y velocity + * @param time time over which velocity is applied + * @return Estimated position + */ + public static Transform2d getFieldEstimatedFuturePose(Transform2d pose, double vx, double vy, double time) { + return getFieldEstimatedFuturePose(pose, vx, vy, 0.0, time); + } + + /** + * Estimates a future position based on the current position and velocities + * + * @param pose current pose + * @param vx x velocity + * @param vy y velocity + * @param time time over which velocity is applied + * @return Estimated position + */ + public static Translation2d getFieldEstimatedFuturePose(Translation2d pose, double vx, double vy, double time) { + return pose.plus(new Translation2d(vx * time, vy * time)); + } + + /** + * Estimates a future position based on the current position and velocities + * + * @param pose current pose + * @param vx x velocity + * @param vy y velocity + * @param vz z velocity + * @param time time over which velocity is applied + * @return Estimated position + */ + public static Pose3d getFieldEstimatedFuturePose(Pose3d pose, double vx, double vy, double vz, double time) { + return getFieldEstimatedFuturePose(pose, vx, vy, vz, 0.0, 0.0, 0.0, time); + } + + /** + * Estimates a future position based on the current position and velocities + * + * @param pose current pose + * @param vx x velocity + * @param vy y velocity + * @param vz z velocity + * @param time time over which velocity is applied + * @return Estimated position + */ + public static Transform3d getFieldEstimatedFuturePose(Transform3d pose, double vx, double vy, double vz, double time) { + return getFieldEstimatedFuturePose(pose, vx, vy, vz, 0.0, 0.0, 0.0, time); + } + + /** + * Estimates a future position based on the current position and velocities + * + * @param pose current pose + * @param vx x velocity + * @param vy y velocity + * @param vz z velocity + * @param time time over which velocity is applied + * @return Estimated position + */ + public static Translation3d getFieldEstimatedFuturePose(Translation3d pose, double vx, double vy, double vz, double time) { + return pose.plus(new Translation3d(vx * time, vy * time, vz * time)); + } + + /** + * Estimates a future position based on the current position and velocities + * + * @param pose current pose + * @param vx x velocity + * @param vy y velocity + * @param vYaw yaw velocity + * @param time time over which velocity is applied + * @return Estimated position + */ + public static Pose2d getFieldEstimatedFuturePose(Pose2d pose, double vx, double vy, double vYaw, double time) { + return pose.transformBy(new Transform2d(vx * time, vy * time, new Rotation2d(vYaw * time))); + } + + /** + * Estimates a future position based on the current position and velocities + * + * @param pose current pose + * @param vx x velocity + * @param vy y velocity + * @param vYaw yaw velocity + * @param time time over which velocity is applied + * @return Estimated position + */ + public static Transform2d getFieldEstimatedFuturePose(Transform2d pose, double vx, double vy, double vYaw, double time) { + return pose.plus(new Transform2d(vx * time, vy * time, Rotation2d.fromDegrees(vYaw * time))); + } + + /** + * Estimates a future position based on the current position and velocities + * + * @param pose current pose + * @param vx x velocity + * @param vy y velocity + * @param vz z velocity + * @param vYaw yaw velocity + * @param vRoll roll velocity + * @param vPitch pitch velocity + * @param time time over which velocity is applied + * @return Estimated position + */ + public static Pose3d getFieldEstimatedFuturePose(Pose3d pose, double vx, double vy, double vz, double vYaw, double vRoll, double vPitch, double time) { + return pose.transformBy(new Transform3d(vx * time, vy * time, vz * time, new Rotation3d(vRoll * time, vPitch * time, vYaw * time))); + } + + /** + * Estimates a future position based on the current position and velocities + * + * @param pose current pose + * @param vx x velocity + * @param vy y velocity + * @param vz z velocity + * @param vYaw yaw velocity + * @param vRoll roll velocity + * @param vPitch pitch velocity + * @param time time over which velocity is applied + * @return Estimated position + */ + public static Transform3d getFieldEstimatedFuturePose(Transform3d pose, double vx, double vy, double vz, double vYaw, double vRoll, double vPitch, double time) { + return pose.plus(new Transform3d(vx * time, vy * time, vz * time, new Rotation3d(vRoll * time, vPitch * time, vYaw * time))); + } + + + /** + * @param pose to modify + * @param z to add + * @param pitch to add + * @param roll to add + * @return 3D pose that inherits 2D attributes + */ + public static Pose3d addDimension(Pose2d pose, double z, double pitch, double roll) { + return new Pose3d(addDimension(pose.getTranslation(), z), addDimension(pose.getRotation(), pitch, roll)); + } + + /** + * @param pose to modify + * @param z to add + * @return 3D pose that inherits 2D attributes + */ + public static Translation3d addDimension(Translation2d pose, double z) { + return new Translation3d(pose.getX(), pose.getY(), z); + } + + /** + * @param rotation to modify + * @param pitch to add + * @param roll to add + * @return 3D rotation that inherits yaw of 2D rotation + */ + public static Rotation3d addDimension(Rotation2d rotation, double pitch, double roll) { + return new Rotation3d(roll, pitch, rotation.getRadians()); + } + public static double slope(Translation2d startPose, Translation2d endPose){ + return (endPose.getY() - startPose.getY()) / (endPose.getX() - startPose.getX()); + } + + public static double getDistance (Pose2d pose1, Pose2d pose2){ + return Math.sqrt(Math.pow(pose1.getX() - pose2.getX(), 2) + Math.pow(pose1.getY() - pose2.getY(), 2)); + } +} \ No newline at end of file From 88e786666266fc34e68f5fc75dec525d482d1143 Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Sat, 22 Feb 2025 20:56:19 -0500 Subject: [PATCH 04/99] fixed pr comments --- .../java/frc/robot/commands/alignment/AlignClosestBranch.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/commands/alignment/AlignClosestBranch.java b/src/main/java/frc/robot/commands/alignment/AlignClosestBranch.java index 3da92512..8e17016d 100644 --- a/src/main/java/frc/robot/commands/alignment/AlignClosestBranch.java +++ b/src/main/java/frc/robot/commands/alignment/AlignClosestBranch.java @@ -20,7 +20,7 @@ public AlignClosestBranch(SwerveDrivetrain drivetrain) { @Override public void initialize() { targetPosition = AlignmentPositions.getClosest(drivetrain.getPose()); - LoggableCommandWrapper.wrap(PathPlannerUtils.pathToPose(targetPosition, 0.0)); + LoggableCommandWrapper.wrap(PathPlannerUtils.pathToPose(targetPosition, 0.0)).schedule(); } @Override From 40c723bae9b1da0a6e1a0e3d01ff30ebda876494 Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Sat, 22 Feb 2025 21:01:10 -0500 Subject: [PATCH 05/99] added sum --- src/main/java/frc/robot/RobotContainer.java | 3 + .../java/frc/robot/utils/math/PoseUtils.java | 397 ++++++++++-------- 2 files changed, 219 insertions(+), 181 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 0cb10ff1..87764376 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -18,6 +18,7 @@ import frc.robot.apriltags.TCPApriltag; import frc.robot.commands.CancelAll; import frc.robot.commands.RollAlgae; +import frc.robot.commands.alignment.AlignClosestBranch; import frc.robot.commands.byebye.ByeByeToFwrLimit; import frc.robot.commands.byebye.ByeByeToRevLimit; import frc.robot.commands.coral.IntakeCoral; @@ -293,6 +294,8 @@ public SwerveDrivetrain getDrivetrain() { public void putShuffleboardCommands() { + SmartShuffleboard.putCommand("Align", "Align to Reef", new AlignClosestBranch(drivetrain)); + if (Constants.CORAL_DEBUG) { SmartShuffleboard.putCommand( "Commands", "Shoot Coral", new ShootCoral(coralSubsystem, Constants.CORAL_SHOOTER_SPEED)); diff --git a/src/main/java/frc/robot/utils/math/PoseUtils.java b/src/main/java/frc/robot/utils/math/PoseUtils.java index 57d2a240..8a12926c 100644 --- a/src/main/java/frc/robot/utils/math/PoseUtils.java +++ b/src/main/java/frc/robot/utils/math/PoseUtils.java @@ -3,184 +3,219 @@ import edu.wpi.first.math.geometry.*; public class PoseUtils { - /** - * Estimates a future position based on the current position and velocities - * - * @param pose current pose - * @param vx x velocity - * @param vy y velocity - * @param time time over which velocity is applied - * @return Estimated position - */ - public static Pose2d getFieldEstimatedFuturePose(Pose2d pose, double vx, double vy, double time) { - return getFieldEstimatedFuturePose(pose, vx, vy, 0.0, time); - } - - /** - * Estimates a future position based on the current position and velocities - * - * @param pose current pose - * @param vx x velocity - * @param vy y velocity - * @param time time over which velocity is applied - * @return Estimated position - */ - public static Transform2d getFieldEstimatedFuturePose(Transform2d pose, double vx, double vy, double time) { - return getFieldEstimatedFuturePose(pose, vx, vy, 0.0, time); - } - - /** - * Estimates a future position based on the current position and velocities - * - * @param pose current pose - * @param vx x velocity - * @param vy y velocity - * @param time time over which velocity is applied - * @return Estimated position - */ - public static Translation2d getFieldEstimatedFuturePose(Translation2d pose, double vx, double vy, double time) { - return pose.plus(new Translation2d(vx * time, vy * time)); - } - - /** - * Estimates a future position based on the current position and velocities - * - * @param pose current pose - * @param vx x velocity - * @param vy y velocity - * @param vz z velocity - * @param time time over which velocity is applied - * @return Estimated position - */ - public static Pose3d getFieldEstimatedFuturePose(Pose3d pose, double vx, double vy, double vz, double time) { - return getFieldEstimatedFuturePose(pose, vx, vy, vz, 0.0, 0.0, 0.0, time); - } - - /** - * Estimates a future position based on the current position and velocities - * - * @param pose current pose - * @param vx x velocity - * @param vy y velocity - * @param vz z velocity - * @param time time over which velocity is applied - * @return Estimated position - */ - public static Transform3d getFieldEstimatedFuturePose(Transform3d pose, double vx, double vy, double vz, double time) { - return getFieldEstimatedFuturePose(pose, vx, vy, vz, 0.0, 0.0, 0.0, time); - } - - /** - * Estimates a future position based on the current position and velocities - * - * @param pose current pose - * @param vx x velocity - * @param vy y velocity - * @param vz z velocity - * @param time time over which velocity is applied - * @return Estimated position - */ - public static Translation3d getFieldEstimatedFuturePose(Translation3d pose, double vx, double vy, double vz, double time) { - return pose.plus(new Translation3d(vx * time, vy * time, vz * time)); - } - - /** - * Estimates a future position based on the current position and velocities - * - * @param pose current pose - * @param vx x velocity - * @param vy y velocity - * @param vYaw yaw velocity - * @param time time over which velocity is applied - * @return Estimated position - */ - public static Pose2d getFieldEstimatedFuturePose(Pose2d pose, double vx, double vy, double vYaw, double time) { - return pose.transformBy(new Transform2d(vx * time, vy * time, new Rotation2d(vYaw * time))); - } - - /** - * Estimates a future position based on the current position and velocities - * - * @param pose current pose - * @param vx x velocity - * @param vy y velocity - * @param vYaw yaw velocity - * @param time time over which velocity is applied - * @return Estimated position - */ - public static Transform2d getFieldEstimatedFuturePose(Transform2d pose, double vx, double vy, double vYaw, double time) { - return pose.plus(new Transform2d(vx * time, vy * time, Rotation2d.fromDegrees(vYaw * time))); - } - - /** - * Estimates a future position based on the current position and velocities - * - * @param pose current pose - * @param vx x velocity - * @param vy y velocity - * @param vz z velocity - * @param vYaw yaw velocity - * @param vRoll roll velocity - * @param vPitch pitch velocity - * @param time time over which velocity is applied - * @return Estimated position - */ - public static Pose3d getFieldEstimatedFuturePose(Pose3d pose, double vx, double vy, double vz, double vYaw, double vRoll, double vPitch, double time) { - return pose.transformBy(new Transform3d(vx * time, vy * time, vz * time, new Rotation3d(vRoll * time, vPitch * time, vYaw * time))); - } - - /** - * Estimates a future position based on the current position and velocities - * - * @param pose current pose - * @param vx x velocity - * @param vy y velocity - * @param vz z velocity - * @param vYaw yaw velocity - * @param vRoll roll velocity - * @param vPitch pitch velocity - * @param time time over which velocity is applied - * @return Estimated position - */ - public static Transform3d getFieldEstimatedFuturePose(Transform3d pose, double vx, double vy, double vz, double vYaw, double vRoll, double vPitch, double time) { - return pose.plus(new Transform3d(vx * time, vy * time, vz * time, new Rotation3d(vRoll * time, vPitch * time, vYaw * time))); - } - - - /** - * @param pose to modify - * @param z to add - * @param pitch to add - * @param roll to add - * @return 3D pose that inherits 2D attributes - */ - public static Pose3d addDimension(Pose2d pose, double z, double pitch, double roll) { - return new Pose3d(addDimension(pose.getTranslation(), z), addDimension(pose.getRotation(), pitch, roll)); - } - - /** - * @param pose to modify - * @param z to add - * @return 3D pose that inherits 2D attributes - */ - public static Translation3d addDimension(Translation2d pose, double z) { - return new Translation3d(pose.getX(), pose.getY(), z); - } - - /** - * @param rotation to modify - * @param pitch to add - * @param roll to add - * @return 3D rotation that inherits yaw of 2D rotation - */ - public static Rotation3d addDimension(Rotation2d rotation, double pitch, double roll) { - return new Rotation3d(roll, pitch, rotation.getRadians()); - } - public static double slope(Translation2d startPose, Translation2d endPose){ - return (endPose.getY() - startPose.getY()) / (endPose.getX() - startPose.getX()); - } - - public static double getDistance (Pose2d pose1, Pose2d pose2){ - return Math.sqrt(Math.pow(pose1.getX() - pose2.getX(), 2) + Math.pow(pose1.getY() - pose2.getY(), 2)); - } -} \ No newline at end of file + /** + * Estimates a future position based on the current position and velocities + * + * @param pose current pose + * @param vx x velocity + * @param vy y velocity + * @param time time over which velocity is applied + * @return Estimated position + */ + public static Pose2d getFieldEstimatedFuturePose(Pose2d pose, double vx, double vy, double time) { + return getFieldEstimatedFuturePose(pose, vx, vy, 0.0, time); + } + + /** + * Estimates a future position based on the current position and velocities + * + * @param pose current pose + * @param vx x velocity + * @param vy y velocity + * @param time time over which velocity is applied + * @return Estimated position + */ + public static Transform2d getFieldEstimatedFuturePose( + Transform2d pose, double vx, double vy, double time) { + return getFieldEstimatedFuturePose(pose, vx, vy, 0.0, time); + } + + /** + * Estimates a future position based on the current position and velocities + * + * @param pose current pose + * @param vx x velocity + * @param vy y velocity + * @param time time over which velocity is applied + * @return Estimated position + */ + public static Translation2d getFieldEstimatedFuturePose( + Translation2d pose, double vx, double vy, double time) { + return pose.plus(new Translation2d(vx * time, vy * time)); + } + + /** + * Estimates a future position based on the current position and velocities + * + * @param pose current pose + * @param vx x velocity + * @param vy y velocity + * @param vz z velocity + * @param time time over which velocity is applied + * @return Estimated position + */ + public static Pose3d getFieldEstimatedFuturePose( + Pose3d pose, double vx, double vy, double vz, double time) { + return getFieldEstimatedFuturePose(pose, vx, vy, vz, 0.0, 0.0, 0.0, time); + } + + /** + * Estimates a future position based on the current position and velocities + * + * @param pose current pose + * @param vx x velocity + * @param vy y velocity + * @param vz z velocity + * @param time time over which velocity is applied + * @return Estimated position + */ + public static Transform3d getFieldEstimatedFuturePose( + Transform3d pose, double vx, double vy, double vz, double time) { + return getFieldEstimatedFuturePose(pose, vx, vy, vz, 0.0, 0.0, 0.0, time); + } + + /** + * Estimates a future position based on the current position and velocities + * + * @param pose current pose + * @param vx x velocity + * @param vy y velocity + * @param vz z velocity + * @param time time over which velocity is applied + * @return Estimated position + */ + public static Translation3d getFieldEstimatedFuturePose( + Translation3d pose, double vx, double vy, double vz, double time) { + return pose.plus(new Translation3d(vx * time, vy * time, vz * time)); + } + + /** + * Estimates a future position based on the current position and velocities + * + * @param pose current pose + * @param vx x velocity + * @param vy y velocity + * @param vYaw yaw velocity + * @param time time over which velocity is applied + * @return Estimated position + */ + public static Pose2d getFieldEstimatedFuturePose( + Pose2d pose, double vx, double vy, double vYaw, double time) { + return pose.transformBy(new Transform2d(vx * time, vy * time, new Rotation2d(vYaw * time))); + } + + /** + * Estimates a future position based on the current position and velocities + * + * @param pose current pose + * @param vx x velocity + * @param vy y velocity + * @param vYaw yaw velocity + * @param time time over which velocity is applied + * @return Estimated position + */ + public static Transform2d getFieldEstimatedFuturePose( + Transform2d pose, double vx, double vy, double vYaw, double time) { + return pose.plus(new Transform2d(vx * time, vy * time, Rotation2d.fromDegrees(vYaw * time))); + } + + /** + * Estimates a future position based on the current position and velocities + * + * @param pose current pose + * @param vx x velocity + * @param vy y velocity + * @param vz z velocity + * @param vYaw yaw velocity + * @param vRoll roll velocity + * @param vPitch pitch velocity + * @param time time over which velocity is applied + * @return Estimated position + */ + public static Pose3d getFieldEstimatedFuturePose( + Pose3d pose, + double vx, + double vy, + double vz, + double vYaw, + double vRoll, + double vPitch, + double time) { + return pose.transformBy( + new Transform3d( + vx * time, + vy * time, + vz * time, + new Rotation3d(vRoll * time, vPitch * time, vYaw * time))); + } + + /** + * Estimates a future position based on the current position and velocities + * + * @param pose current pose + * @param vx x velocity + * @param vy y velocity + * @param vz z velocity + * @param vYaw yaw velocity + * @param vRoll roll velocity + * @param vPitch pitch velocity + * @param time time over which velocity is applied + * @return Estimated position + */ + public static Transform3d getFieldEstimatedFuturePose( + Transform3d pose, + double vx, + double vy, + double vz, + double vYaw, + double vRoll, + double vPitch, + double time) { + return pose.plus( + new Transform3d( + vx * time, + vy * time, + vz * time, + new Rotation3d(vRoll * time, vPitch * time, vYaw * time))); + } + + /** + * @param pose to modify + * @param z to add + * @param pitch to add + * @param roll to add + * @return 3D pose that inherits 2D attributes + */ + public static Pose3d addDimension(Pose2d pose, double z, double pitch, double roll) { + return new Pose3d( + addDimension(pose.getTranslation(), z), addDimension(pose.getRotation(), pitch, roll)); + } + + /** + * @param pose to modify + * @param z to add + * @return 3D pose that inherits 2D attributes + */ + public static Translation3d addDimension(Translation2d pose, double z) { + return new Translation3d(pose.getX(), pose.getY(), z); + } + + /** + * @param rotation to modify + * @param pitch to add + * @param roll to add + * @return 3D rotation that inherits yaw of 2D rotation + */ + public static Rotation3d addDimension(Rotation2d rotation, double pitch, double roll) { + return new Rotation3d(roll, pitch, rotation.getRadians()); + } + + public static double slope(Translation2d startPose, Translation2d endPose) { + return (endPose.getY() - startPose.getY()) / (endPose.getX() - startPose.getX()); + } + + public static double getDistance(Pose2d pose1, Pose2d pose2) { + return Math.sqrt( + Math.pow(pose1.getX() - pose2.getX(), 2) + Math.pow(pose1.getY() - pose2.getY(), 2)); + } +} From ec71fe8fc11afce90537f3a97c42d282afb48679 Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Sat, 22 Feb 2025 22:03:23 -0500 Subject: [PATCH 06/99] wow --- src/main/java/frc/robot/RobotContainer.java | 33 +++++++++++++++++++ .../frc/robot/constants/GameConstants.java | 10 ++++++ 2 files changed, 43 insertions(+) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 241109fc..16cff106 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,6 +4,10 @@ package frc.robot; +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.config.PIDConstants; +import com.pathplanner.lib.config.RobotConfig; +import com.pathplanner.lib.controllers.PPHolonomicDriveController; import com.studica.frc.AHRS; import com.studica.frc.AHRS.NavXComType; import edu.wpi.first.math.trajectory.TrapezoidProfile; @@ -96,6 +100,7 @@ public class RobotContainer { new CommandXboxController(Constants.XBOX_CONTROLLER_ID); private final Joystick joyleft = new Joystick(Constants.LEFT_JOYSTICK_ID); private final Joystick joyright = new Joystick(Constants.RIGHT_JOYSTICK_ID); + private RobotConfig config; public RobotContainer() { switch (Constants.currentMode) { @@ -136,6 +141,7 @@ public RobotContainer() { setupDriveTrain(); configureBindings(); putShuffleboardCommands(); + setupPathPlanning(); } private void configureBindings() { @@ -288,6 +294,33 @@ public SwerveDrivetrain getDrivetrain() { return drivetrain; } + private void setupPathPlanning() { + try { + config = RobotConfig.fromGUISettings(); + } catch (Exception e) { + // Handle exception as needed + e.printStackTrace(); + } + AutoBuilder.configure( + drivetrain::getPose, + drivetrain::resetOdometry, + drivetrain::speedsFromStates, + drivetrain::drive, + new PPHolonomicDriveController( + new PIDConstants( + Constants.PATH_PLANNER_TRANSLATION_PID_P, + Constants.PATH_PLANNER_TRANSLATION_PID_I, + Constants.PATH_PLANNER_TRANSLATION_PID_D), // Translation PID constants + new PIDConstants( + Constants.PATH_PLANNER_ROTATION_PID_P, + Constants.PATH_PLANNER_ROTATION_PID_I, + Constants.PATH_PLANNER_ROTATION_PID_D) // Rotation PID constants + ), + config, + RobotContainer::isRedAlliance, + drivetrain); + } + public void putShuffleboardCommands() { SmartShuffleboard.putCommand("Align", "Align to Reef", new AlignClosestBranch(drivetrain)); diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java index e31b07b2..e68e3570 100644 --- a/src/main/java/frc/robot/constants/GameConstants.java +++ b/src/main/java/frc/robot/constants/GameConstants.java @@ -123,6 +123,11 @@ public enum Mode { public static final double DRIVE_PID_FF_S = 1; // TODO: change later public static final double DRIVE_PID_FF_V = 2.8; // TODO: change later + // PathPlanner Drive PID + public static final double PATH_PLANNER_TRANSLATION_PID_P = 1; + public static final double PATH_PLANNER_TRANSLATION_PID_I = 0; + public static final double PATH_PLANNER_TRANSLATION_PID_D = 0; + // Steer PID public static final double STEER_PID_P = 0.3; // TODO: change later public static final double STEER_PID_I = 0; // TODO: change later @@ -130,6 +135,11 @@ public enum Mode { public static final double STEER_PID_FF_S = 0; // 0.2; //TODO: change later public static final double STEER_PID_FF_V = 0; // 0.8; //TODO: change later + // PathPlanner Steer PID + public static final double PATH_PLANNER_ROTATION_PID_P = 0.3; + public static final double PATH_PLANNER_ROTATION_PID_I = 0; + public static final double PATH_PLANNER_ROTATION_PID_D = 0.005; + // Lengths public static final double ELEVATOR_DRUM_RADIUS = Units.inchesToMeters(1); // In M(in), change later From 929fbf35f77f1d9e25fc3f74d8c1993623c8fa1a Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Sat, 22 Feb 2025 22:15:02 -0500 Subject: [PATCH 07/99] fixed completely --- src/main/deploy/pathplanner/settings.json | 46 +++++++++++++++++++++++ 1 file changed, 46 insertions(+) create mode 100644 src/main/deploy/pathplanner/settings.json diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json new file mode 100644 index 00000000..066a195b --- /dev/null +++ b/src/main/deploy/pathplanner/settings.json @@ -0,0 +1,46 @@ +{ + "robotWidth": 0.8636, + "robotLength": 0.864, + "holonomicMode": true, + "pathFolders": [ + "Cross", + "Do Nothing", + "Posts to Station (4)", + "Push", + "Robot to Post (4)", + "Station 1 to Posts (12)", + "Station 2 to Posts (12)" + ], + "autoFolders": [ + "RAINBOW AUTO", + "Robot 1 to Posts (12)", + "Robot 2 to Posts (12)", + "Robot 3 to Reefs (12)", + "Robot 4 to Reefs (12)" + ], + "defaultMaxVel": 4.0, + "defaultMaxAccel": 4.0, + "defaultMaxAngVel": 540.0, + "defaultMaxAngAccel": 720.0, + "defaultNominalVoltage": 12.0, + "robotMass": 54.5, + "robotMOI": 4.342, + "robotTrackwidth": 0.546, + "driveWheelRadius": 0.048, + "driveGearing": 6.75, + "maxDriveSpeed": 3.9, + "driveMotorType": "NEO", + "driveCurrentLimit": 60.0, + "wheelCOF": 1.2, + "flModuleX": 0.29845, + "flModuleY": 0.29845, + "frModuleX": 0.29845, + "frModuleY": -0.29845, + "blModuleX": -0.29845, + "blModuleY": 0.29845, + "brModuleX": -0.29845, + "brModuleY": -0.29845, + "bumperOffsetX": 0.0, + "bumperOffsetY": 0.0, + "robotFeatures": [] + } \ No newline at end of file From bf3399faade7bca4a47d146b60f43aee9e11b4f8 Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Sun, 23 Feb 2025 13:26:04 -0500 Subject: [PATCH 08/99] wow --- src/main/deploy/pathplanner/navgrid.json | 1 + 1 file changed, 1 insertion(+) create mode 100644 src/main/deploy/pathplanner/navgrid.json diff --git a/src/main/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/navgrid.json new file mode 100644 index 00000000..23e0db94 --- /dev/null +++ b/src/main/deploy/pathplanner/navgrid.json @@ -0,0 +1 @@ +{"field_size":{"x":17.548,"y":8.052},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]} \ No newline at end of file From 50706fe8994ecb7be45c079d2387068dc35a9807 Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Sun, 23 Feb 2025 13:28:57 -0500 Subject: [PATCH 09/99] fixed pathplanner settings? --- src/main/deploy/pathplanner/settings.json | 90 +++++++++++------------ 1 file changed, 45 insertions(+), 45 deletions(-) diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index 066a195b..00ae684b 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -1,46 +1,46 @@ { - "robotWidth": 0.8636, - "robotLength": 0.864, - "holonomicMode": true, - "pathFolders": [ - "Cross", - "Do Nothing", - "Posts to Station (4)", - "Push", - "Robot to Post (4)", - "Station 1 to Posts (12)", - "Station 2 to Posts (12)" - ], - "autoFolders": [ - "RAINBOW AUTO", - "Robot 1 to Posts (12)", - "Robot 2 to Posts (12)", - "Robot 3 to Reefs (12)", - "Robot 4 to Reefs (12)" - ], - "defaultMaxVel": 4.0, - "defaultMaxAccel": 4.0, - "defaultMaxAngVel": 540.0, - "defaultMaxAngAccel": 720.0, - "defaultNominalVoltage": 12.0, - "robotMass": 54.5, - "robotMOI": 4.342, - "robotTrackwidth": 0.546, - "driveWheelRadius": 0.048, - "driveGearing": 6.75, - "maxDriveSpeed": 3.9, - "driveMotorType": "NEO", - "driveCurrentLimit": 60.0, - "wheelCOF": 1.2, - "flModuleX": 0.29845, - "flModuleY": 0.29845, - "frModuleX": 0.29845, - "frModuleY": -0.29845, - "blModuleX": -0.29845, - "blModuleY": 0.29845, - "brModuleX": -0.29845, - "brModuleY": -0.29845, - "bumperOffsetX": 0.0, - "bumperOffsetY": 0.0, - "robotFeatures": [] - } \ No newline at end of file + "robotWidth": 0.9271, + "robotLength": 0.927, + "holonomicMode": true, + "pathFolders": [ + "Cross", + "Do Nothing", + "Posts to Station (4)", + "Push", + "Robot to Post (4)", + "Station 1 to Posts (12)", + "Station 2 to Posts (12)" + ], + "autoFolders": [ + "RAINBOW AUTO", + "Robot 1 to Posts (12)", + "Robot 2 to Posts (12)", + "Robot 3 to Reefs (12)", + "Robot 4 to Reefs (12)" + ], + "defaultMaxVel": 4.0, + "defaultMaxAccel": 4.0, + "defaultMaxAngVel": 540.0, + "defaultMaxAngAccel": 720.0, + "defaultNominalVoltage": 12.0, + "robotMass": 54.5, + "robotMOI": 4.342, + "robotTrackwidth": 0.546, + "driveWheelRadius": 0.048, + "driveGearing": 8.142857, + "maxDriveSpeed": 3.5, + "driveMotorType": "NEO", + "driveCurrentLimit": 40.0, + "wheelCOF": 1.2, + "flModuleX": 0.318, + "flModuleY": 0.3175, + "frModuleX": 0.318, + "frModuleY": -0.318, + "blModuleX": -0.318, + "blModuleY": 0.318, + "brModuleX": -0.318, + "brModuleY": -0.318, + "bumperOffsetX": 0.0, + "bumperOffsetY": 0.0, + "robotFeatures": [] +} \ No newline at end of file From 143dafe834cf28fda0e280be4bb2502534ce1a54 Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Sun, 23 Feb 2025 13:31:01 -0500 Subject: [PATCH 10/99] even more fixed pathplanner settings --- src/main/deploy/pathplanner/settings.json | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index 00ae684b..1cb21a36 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -18,7 +18,7 @@ "Robot 3 to Reefs (12)", "Robot 4 to Reefs (12)" ], - "defaultMaxVel": 4.0, + "defaultMaxVel": 3.5, "defaultMaxAccel": 4.0, "defaultMaxAngVel": 540.0, "defaultMaxAngAccel": 720.0, From cdc89398074d0c4fc63b798b4084da391b7d63cb Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Sun, 23 Feb 2025 13:57:11 -0500 Subject: [PATCH 11/99] fixed pr comments? --- src/main/java/frc/robot/RobotContainer.java | 10 ++--- .../alignment/AlignClosestBranch.java | 1 + .../frc/robot/constants/BranchPositions.java | 2 +- .../frc/robot/constants/GameConstants.java | 16 ++++--- .../robot/utils/auto/PathPlannerUtils.java | 43 +++++++++++++++---- 5 files changed, 50 insertions(+), 22 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 16cff106..4beddb3d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -80,6 +80,7 @@ import frc.robot.subsystems.swervev3.io.steer.MockSteerMotorIO; import frc.robot.utils.BlinkinPattern; import frc.robot.utils.ModulePosition; +import frc.robot.utils.auto.PathPlannerUtils; import frc.robot.utils.logging.LoggableIO; import frc.robot.utils.motor.Gain; import frc.robot.utils.motor.PID; @@ -295,12 +296,7 @@ public SwerveDrivetrain getDrivetrain() { } private void setupPathPlanning() { - try { - config = RobotConfig.fromGUISettings(); - } catch (Exception e) { - // Handle exception as needed - e.printStackTrace(); - } + PathPlannerUtils.generateConfiguration(); AutoBuilder.configure( drivetrain::getPose, drivetrain::resetOdometry, @@ -316,7 +312,7 @@ private void setupPathPlanning() { Constants.PATH_PLANNER_ROTATION_PID_I, Constants.PATH_PLANNER_ROTATION_PID_D) // Rotation PID constants ), - config, + PathPlannerUtils.getConfig(), RobotContainer::isRedAlliance, drivetrain); } diff --git a/src/main/java/frc/robot/commands/alignment/AlignClosestBranch.java b/src/main/java/frc/robot/commands/alignment/AlignClosestBranch.java index 8e17016d..c22c4f24 100644 --- a/src/main/java/frc/robot/commands/alignment/AlignClosestBranch.java +++ b/src/main/java/frc/robot/commands/alignment/AlignClosestBranch.java @@ -15,6 +15,7 @@ public class AlignClosestBranch extends LoggableCommand { public AlignClosestBranch(SwerveDrivetrain drivetrain) { this.drivetrain = drivetrain; + addRequirements(drivetrain); } @Override diff --git a/src/main/java/frc/robot/constants/BranchPositions.java b/src/main/java/frc/robot/constants/BranchPositions.java index b1e94bda..5a59d1eb 100644 --- a/src/main/java/frc/robot/constants/BranchPositions.java +++ b/src/main/java/frc/robot/constants/BranchPositions.java @@ -1,9 +1,9 @@ +/** Technically not used now, mainly just for checking if the math is right */ package frc.robot.constants; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; -// Technically not used now, mainly just for checking if the math is right public enum BranchPositions { CENTER(new Pose2d(4.477430593, 4.0259230378, Rotation2d.fromDegrees(0.0))), CENTER_A_B(new Pose2d(3.64569375, 4.0259230378, Rotation2d.fromDegrees(0))), diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java index e68e3570..7cb4c3b2 100644 --- a/src/main/java/frc/robot/constants/GameConstants.java +++ b/src/main/java/frc/robot/constants/GameConstants.java @@ -45,6 +45,10 @@ public class GameConstants { public static final double CLIMBER_RISE_SPEED = 0.5; public static final double HIHI_INTAKE_BASE_VELOCITY = 7000.0; + // Accelerations + public static final double MAX_PATHPLANNER_ACCEL = 11.8; + public static final double MAX_PATHPLANNER_ANGULAR_ACCEL = 3570; + // Timeouts public static final int SERVER_SOCKET_CONNECTION_TIMEOUT = 2000; public static final int ELEVATOR_TIMEOUT = 10; @@ -123,11 +127,6 @@ public enum Mode { public static final double DRIVE_PID_FF_S = 1; // TODO: change later public static final double DRIVE_PID_FF_V = 2.8; // TODO: change later - // PathPlanner Drive PID - public static final double PATH_PLANNER_TRANSLATION_PID_P = 1; - public static final double PATH_PLANNER_TRANSLATION_PID_I = 0; - public static final double PATH_PLANNER_TRANSLATION_PID_D = 0; - // Steer PID public static final double STEER_PID_P = 0.3; // TODO: change later public static final double STEER_PID_I = 0; // TODO: change later @@ -135,6 +134,11 @@ public enum Mode { public static final double STEER_PID_FF_S = 0; // 0.2; //TODO: change later public static final double STEER_PID_FF_V = 0; // 0.8; //TODO: change later + // PathPlanner Drive PID + public static final double PATH_PLANNER_TRANSLATION_PID_P = 1; + public static final double PATH_PLANNER_TRANSLATION_PID_I = 0; + public static final double PATH_PLANNER_TRANSLATION_PID_D = 0; + // PathPlanner Steer PID public static final double PATH_PLANNER_ROTATION_PID_P = 0.3; public static final double PATH_PLANNER_ROTATION_PID_I = 0; @@ -147,7 +151,6 @@ public enum Mode { public static final double MAX_ELEVATOR_HEIGHT_METERS = -200; // in m public static final double INITIAL_ELEVATOR_HEIGHT = 0; // TODO: change later public static final double HIHI_LENGTH = 0.5; // TODO: change later - public static final double ALIGNMENT_DISTANCE_THRESHOLD = 0.05; // TODO: change later // Angles public static final Rotation2d HIHI_MIN_ANGLE = @@ -186,4 +189,5 @@ public enum Mode { public static final boolean HI_HI_SIMULATE_GRAVITY = true; public static final int MAX_VALID_TICKS_INTAKE = 15; // TODO: Change Later public static final int MAX_VALID_TICKS_ELEVATOR = 10; // TODO: Change Later + public static final double ALIGNMENT_DISTANCE_THRESHOLD = 0.05; // TODO: change later } diff --git a/src/main/java/frc/robot/utils/auto/PathPlannerUtils.java b/src/main/java/frc/robot/utils/auto/PathPlannerUtils.java index 29330bf0..b211f848 100644 --- a/src/main/java/frc/robot/utils/auto/PathPlannerUtils.java +++ b/src/main/java/frc/robot/utils/auto/PathPlannerUtils.java @@ -1,23 +1,50 @@ package frc.robot.utils.auto; import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.config.RobotConfig; import com.pathplanner.lib.path.GoalEndState; import com.pathplanner.lib.path.PathConstraints; import com.pathplanner.lib.path.PathPlannerPath; import com.pathplanner.lib.path.Waypoint; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.constants.Constants; import frc.robot.utils.logging.commands.LoggableCommandWrapper; +import java.io.IOException; import java.util.List; +import org.json.simple.parser.ParseException; public class PathPlannerUtils { - private static final PathConstraints defualtPathConstraints = - new PathConstraints( - Constants.MAX_VELOCITY, - Constants.MAX_VELOCITY, - Math.toRadians(1000), - Math.toRadians(1000)); + public static PathConstraints defaultPathConstraints; + public static RobotConfig config; + + public static void generateConfiguration() { + RobotConfig tempConf = null; + PathConstraints tempConstraints = null; + try { + tempConf = RobotConfig.fromGUISettings(); + tempConstraints = + new PathConstraints( + tempConf.moduleConfig.maxDriveVelocityMPS, + Constants.MAX_PATHPLANNER_ACCEL, + tempConf.moduleConfig.maxDriveVelocityRadPerSec, + Constants.MAX_PATHPLANNER_ANGULAR_ACCEL); + } catch (IOException e) { + DriverStation.reportError( + "IO Exception while opening PathPlanner config from GUISettings", true); + } catch (ParseException e) { + DriverStation.reportError("Could not parse PathPlanner config from GUISettings", true); + throw new RuntimeException(e); + } finally { + config = tempConf; + defaultPathConstraints = tempConstraints; + } + } + + public static RobotConfig getConfig() { + return config; + } public static PathPlannerPath createManualPath( Pose2d startPose, Pose2d targetPos, double endVelocity) { @@ -25,7 +52,7 @@ public static PathPlannerPath createManualPath( PathPlannerPath path = new PathPlannerPath( waypoints, - defualtPathConstraints, + defaultPathConstraints, null, new GoalEndState(endVelocity, targetPos.getRotation())); path.preventFlipping = true; @@ -37,6 +64,6 @@ public static LoggableCommandWrapper autoFromPath(PathPlannerPath path) { } public static Command pathToPose(Pose2d targetPos, double endVelocity) { - return AutoBuilder.pathfindToPose(targetPos, defualtPathConstraints, endVelocity); + return AutoBuilder.pathfindToPose(targetPos, defaultPathConstraints, endVelocity); } } From c9e12d30d3bb1da79a2fba8284878e8a42172f8d Mon Sep 17 00:00:00 2001 From: codetoad Date: Sun, 23 Feb 2025 14:17:53 -0500 Subject: [PATCH 12/99] made it build --- src/main/java/frc/robot/RobotContainer.java | 3 +-- src/main/java/frc/robot/constants/Constants2025.java | 6 ++++-- src/main/java/frc/robot/constants/GameConstants.java | 4 ++-- .../robot/subsystems/swervev3/SwerveDrivetrain.java | 8 ++++---- .../java/frc/robot/utils/auto/PathPlannerUtils.java | 10 +++------- 5 files changed, 14 insertions(+), 17 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 75d62451..421dd0e5 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -300,7 +300,6 @@ public SwerveDrivetrain getDrivetrain() { } private void setupPathPlanning() { - PathPlannerUtils.generateConfiguration(); AutoBuilder.configure( drivetrain::getPose, drivetrain::resetOdometry, @@ -316,7 +315,7 @@ private void setupPathPlanning() { Constants.PATH_PLANNER_ROTATION_PID_I, Constants.PATH_PLANNER_ROTATION_PID_D) // Rotation PID constants ), - PathPlannerUtils.getConfig(), + PathPlannerUtils.config, RobotContainer::isRedAlliance, drivetrain); } diff --git a/src/main/java/frc/robot/constants/Constants2025.java b/src/main/java/frc/robot/constants/Constants2025.java index 56b0460a..f95d57fe 100644 --- a/src/main/java/frc/robot/constants/Constants2025.java +++ b/src/main/java/frc/robot/constants/Constants2025.java @@ -44,6 +44,8 @@ public class Constants2025 extends GameConstants { public static final int DRIVE_FRONT_LEFT_S = 55; public static final int DRIVE_BACK_LEFT_S = 49; - public static final double DRIVE_BASE_WIDTH = 0.635; // Measured distance between the center of the wheels - public static final double DRIVE_BASE_LENGTH = 0.635; // Measured distance between the center of the wheels + public static final double DRIVE_BASE_WIDTH = + 0.635; // Measured distance between the center of the wheels + public static final double DRIVE_BASE_LENGTH = + 0.635; // Measured distance between the center of the wheels } diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java index f6442b39..2a50de8f 100644 --- a/src/main/java/frc/robot/constants/GameConstants.java +++ b/src/main/java/frc/robot/constants/GameConstants.java @@ -46,8 +46,8 @@ public class GameConstants { public static final double HIHI_INTAKE_BASE_VELOCITY = 7000.0; // Accelerations - public static final double MAX_PATHPLANNER_ACCEL = 11.8; - public static final double MAX_PATHPLANNER_ANGULAR_ACCEL = 3570; + public static final double MAX_PATHPLANNER_ACCEL = 11.7; + public static final double MAX_PATHPLANNER_ANGULAR_ACCEL = 3797; // Timeouts public static final int SERVER_SOCKET_CONNECTION_TIMEOUT = 2000; diff --git a/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java index a58f061b..2eaef416 100644 --- a/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java @@ -27,13 +27,13 @@ public class SwerveDrivetrain extends SubsystemBase { private final SwerveModule backLeft; private final SwerveModule backRight; private final Translation2d frontLeftLocation = - new Translation2d(Constants.DRIVE_BASE_WIDTH/2, Constants.DRIVE_BASE_LENGTH/2); + new Translation2d(Constants.DRIVE_BASE_WIDTH / 2, Constants.DRIVE_BASE_LENGTH / 2); private final Translation2d frontRightLocation = - new Translation2d(Constants.DRIVE_BASE_WIDTH/2, -Constants.DRIVE_BASE_LENGTH/2); + new Translation2d(Constants.DRIVE_BASE_WIDTH / 2, -Constants.DRIVE_BASE_LENGTH / 2); private final Translation2d backLeftLocation = - new Translation2d(-Constants.DRIVE_BASE_WIDTH/2, Constants.DRIVE_BASE_LENGTH/2); + new Translation2d(-Constants.DRIVE_BASE_WIDTH / 2, Constants.DRIVE_BASE_LENGTH / 2); private final Translation2d backRightLocation = - new Translation2d(-Constants.DRIVE_BASE_WIDTH/2, -Constants.DRIVE_BASE_LENGTH/2); + new Translation2d(-Constants.DRIVE_BASE_WIDTH / 2, -Constants.DRIVE_BASE_LENGTH / 2); private final SwerveDriveKinematics kinematics = new SwerveDriveKinematics( frontLeftLocation, frontRightLocation, backLeftLocation, backRightLocation); diff --git a/src/main/java/frc/robot/utils/auto/PathPlannerUtils.java b/src/main/java/frc/robot/utils/auto/PathPlannerUtils.java index b211f848..188f91d3 100644 --- a/src/main/java/frc/robot/utils/auto/PathPlannerUtils.java +++ b/src/main/java/frc/robot/utils/auto/PathPlannerUtils.java @@ -16,10 +16,10 @@ import org.json.simple.parser.ParseException; public class PathPlannerUtils { - public static PathConstraints defaultPathConstraints; - public static RobotConfig config; + public static final PathConstraints defaultPathConstraints; + public static final RobotConfig config; - public static void generateConfiguration() { + static { RobotConfig tempConf = null; PathConstraints tempConstraints = null; try { @@ -42,10 +42,6 @@ public static void generateConfiguration() { } } - public static RobotConfig getConfig() { - return config; - } - public static PathPlannerPath createManualPath( Pose2d startPose, Pose2d targetPos, double endVelocity) { List waypoints = PathPlannerPath.waypointsFromPoses(startPose, targetPos); From 9da95dfe181654655ad91956ede18d1830f1e4bc Mon Sep 17 00:00:00 2001 From: codetoad Date: Sun, 23 Feb 2025 15:09:47 -0500 Subject: [PATCH 13/99] reset wheels on auto init --- src/main/java/frc/robot/Robot.java | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index becbdce7..78ba246b 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -114,12 +114,16 @@ public void disabledExit() {} @Override public void autonomousInit() { + mode.set(RobotMode.AUTONOMOUS); m_autonomousCommand = m_robotContainer.getAutonomousCommand(); if (m_autonomousCommand != null) { m_autonomousCommand.schedule(); } - mode.set(RobotMode.AUTONOMOUS); + new SequentialCommandGroup( + new WheelAlign(m_robotContainer.getDrivetrain()), + new ResetGyro(m_robotContainer.getDrivetrain())) + .schedule(); } @Override @@ -130,11 +134,11 @@ public void autonomousExit() {} @Override public void teleopInit() { + mode.set(RobotMode.TELEOP); diagnostics.reset(); if (m_autonomousCommand != null) { m_autonomousCommand.cancel(); } - mode.set(RobotMode.TELEOP); } @Override @@ -145,9 +149,9 @@ public void teleopExit() {} @Override public void testInit() { + mode.set(RobotMode.TEST); diagnostics.reset(); CommandScheduler.getInstance().cancelAll(); - mode.set(RobotMode.TEST); } @Override From bc154f4d0ccd2472aa7fde0c6c2664e20e5f372e Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Sun, 23 Feb 2025 19:24:46 -0500 Subject: [PATCH 14/99] fixed pr comments? --- src/main/java/frc/robot/utils/auto/PathPlannerUtils.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/utils/auto/PathPlannerUtils.java b/src/main/java/frc/robot/utils/auto/PathPlannerUtils.java index 188f91d3..7365cc43 100644 --- a/src/main/java/frc/robot/utils/auto/PathPlannerUtils.java +++ b/src/main/java/frc/robot/utils/auto/PathPlannerUtils.java @@ -26,7 +26,7 @@ public class PathPlannerUtils { tempConf = RobotConfig.fromGUISettings(); tempConstraints = new PathConstraints( - tempConf.moduleConfig.maxDriveVelocityMPS, + 1.0, //tempConf.moduleConfig.maxDriveVelocityMPS, Constants.MAX_PATHPLANNER_ACCEL, tempConf.moduleConfig.maxDriveVelocityRadPerSec, Constants.MAX_PATHPLANNER_ANGULAR_ACCEL); From 31a6d0c10a5a511342c3d0f744a924f971b5866a Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Sun, 23 Feb 2025 20:04:16 -0500 Subject: [PATCH 15/99] hopefully fixed --- .../commands/alignment/AlignClosestBranch.java | 15 +++++++++++++-- .../java/frc/robot/constants/GameConstants.java | 2 +- 2 files changed, 14 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/commands/alignment/AlignClosestBranch.java b/src/main/java/frc/robot/commands/alignment/AlignClosestBranch.java index c22c4f24..44f6aa51 100644 --- a/src/main/java/frc/robot/commands/alignment/AlignClosestBranch.java +++ b/src/main/java/frc/robot/commands/alignment/AlignClosestBranch.java @@ -12,6 +12,7 @@ public class AlignClosestBranch extends LoggableCommand { private Pose2d targetPosition; private final SwerveDrivetrain drivetrain; + private int counter = 0; public AlignClosestBranch(SwerveDrivetrain drivetrain) { this.drivetrain = drivetrain; @@ -20,13 +21,23 @@ public AlignClosestBranch(SwerveDrivetrain drivetrain) { @Override public void initialize() { + counter = 0; targetPosition = AlignmentPositions.getClosest(drivetrain.getPose()); LoggableCommandWrapper.wrap(PathPlannerUtils.pathToPose(targetPosition, 0.0)).schedule(); } + @Override + public void execute() { + if (PoseUtils.getDistance(drivetrain.getPose(), targetPosition) + < Constants.ALIGNMENT_DISTANCE_THRESHOLD) { + counter++; + } else { + counter = 0; + } + } + @Override public boolean isFinished() { - return (PoseUtils.getDistance(drivetrain.getPose(), targetPosition) - < Constants.ALIGNMENT_DISTANCE_THRESHOLD); + return (counter > 20); } } diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java index 2970d337..cb8f524d 100644 --- a/src/main/java/frc/robot/constants/GameConstants.java +++ b/src/main/java/frc/robot/constants/GameConstants.java @@ -195,7 +195,7 @@ public enum Mode { public static final boolean HI_HI_SIMULATE_GRAVITY = true; public static final int MAX_VALID_TICKS_INTAKE = 15; // TODO: Change Later public static final int MAX_VALID_TICKS_ELEVATOR = 10; // TODO: Change Later - public static final double ALIGNMENT_DISTANCE_THRESHOLD = 0.05; // TODO: change later + public static final double ALIGNMENT_DISTANCE_THRESHOLD = 0.005; // TODO: change later // ELEVATOR CONSTANTS public static final double ELEVATOR_MANUAL_DEADBAND = 0.2; From c487ccacfdb8f4ce21116898f3c91ba3f443343d Mon Sep 17 00:00:00 2001 From: codetoad Date: Sun, 23 Feb 2025 22:23:40 -0500 Subject: [PATCH 16/99] made pathplanner go to pose with fine adjustments :) --- .../alignment/AlignClosestBranch.java | 26 +- .../robot/commands/alignment/FineAlign.java | 34 ++ .../subsystems/swervev3/SwerveDrivetrain.java | 382 +++++++++--------- 3 files changed, 250 insertions(+), 192 deletions(-) create mode 100644 src/main/java/frc/robot/commands/alignment/FineAlign.java diff --git a/src/main/java/frc/robot/commands/alignment/AlignClosestBranch.java b/src/main/java/frc/robot/commands/alignment/AlignClosestBranch.java index 44f6aa51..e4333ebf 100644 --- a/src/main/java/frc/robot/commands/alignment/AlignClosestBranch.java +++ b/src/main/java/frc/robot/commands/alignment/AlignClosestBranch.java @@ -1,18 +1,22 @@ package frc.robot.commands.alignment; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.CommandScheduler; import frc.robot.constants.AlignmentPositions; import frc.robot.constants.Constants; import frc.robot.subsystems.swervev3.SwerveDrivetrain; import frc.robot.utils.auto.PathPlannerUtils; import frc.robot.utils.logging.commands.LoggableCommand; import frc.robot.utils.logging.commands.LoggableCommandWrapper; +import frc.robot.utils.logging.commands.LoggableSequentialCommandGroup; import frc.robot.utils.math.PoseUtils; public class AlignClosestBranch extends LoggableCommand { private Pose2d targetPosition; private final SwerveDrivetrain drivetrain; - private int counter = 0; + private LoggableSequentialCommandGroup driveSequence; + private final Timer timer = new Timer(); public AlignClosestBranch(SwerveDrivetrain drivetrain) { this.drivetrain = drivetrain; @@ -21,23 +25,25 @@ public AlignClosestBranch(SwerveDrivetrain drivetrain) { @Override public void initialize() { - counter = 0; + timer.restart(); targetPosition = AlignmentPositions.getClosest(drivetrain.getPose()); - LoggableCommandWrapper.wrap(PathPlannerUtils.pathToPose(targetPosition, 0.0)).schedule(); + LoggableSequentialCommandGroup sequence = new LoggableSequentialCommandGroup( + LoggableCommandWrapper.wrap(PathPlannerUtils.pathToPose(targetPosition, 0.0)).withBasicName("GeneralAlign"), + new FineAlign(drivetrain, targetPosition) + ).withBasicName("PathPlannerToBranch"); + sequence.setParent(this); + sequence.schedule(); } @Override - public void execute() { - if (PoseUtils.getDistance(drivetrain.getPose(), targetPosition) - < Constants.ALIGNMENT_DISTANCE_THRESHOLD) { - counter++; - } else { - counter = 0; + public void end(boolean interrupted) { + if (CommandScheduler.getInstance().isScheduled(driveSequence)){ + CommandScheduler.getInstance().cancel(driveSequence); } } @Override public boolean isFinished() { - return (counter > 20); + return driveSequence.isFinished() | timer.hasElapsed(10); } } diff --git a/src/main/java/frc/robot/commands/alignment/FineAlign.java b/src/main/java/frc/robot/commands/alignment/FineAlign.java new file mode 100644 index 00000000..8248bf09 --- /dev/null +++ b/src/main/java/frc/robot/commands/alignment/FineAlign.java @@ -0,0 +1,34 @@ +package frc.robot.commands.alignment; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj.Timer; +import frc.robot.subsystems.swervev3.SwerveDrivetrain; +import frc.robot.utils.logging.commands.LoggableCommand; + +import java.util.function.Supplier; + +public class FineAlign extends LoggableCommand { + private final SwerveDrivetrain drivetrain; + private Pose2d targetPose; + private final Timer timer = new Timer(); + + public FineAlign(SwerveDrivetrain drivetrain, Pose2d targetPose){ + this.drivetrain = drivetrain; + this.targetPose = targetPose; + } + + @Override + public void initialize() { + timer.restart(); + } + + @Override + public void execute() { + drivetrain.drive(drivetrain.calculateSpeedsTowardsPoint(targetPose)); + } + + @Override + public boolean isFinished() { + return drivetrain.atTarget(targetPose) || timer.hasElapsed(5); + } +} diff --git a/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java index 2eaef416..48e7f566 100644 --- a/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java @@ -1,5 +1,8 @@ package frc.robot.subsystems.swervev3; +import edu.wpi.first.math.controller.HolonomicDriveController; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; @@ -7,6 +10,7 @@ import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.apriltags.ApriltagInputs; import frc.robot.constants.Constants; @@ -22,186 +26,200 @@ import org.littletonrobotics.junction.Logger; public class SwerveDrivetrain extends SubsystemBase { - private final SwerveModule frontLeft; - private final SwerveModule frontRight; - private final SwerveModule backLeft; - private final SwerveModule backRight; - private final Translation2d frontLeftLocation = - new Translation2d(Constants.DRIVE_BASE_WIDTH / 2, Constants.DRIVE_BASE_LENGTH / 2); - private final Translation2d frontRightLocation = - new Translation2d(Constants.DRIVE_BASE_WIDTH / 2, -Constants.DRIVE_BASE_LENGTH / 2); - private final Translation2d backLeftLocation = - new Translation2d(-Constants.DRIVE_BASE_WIDTH / 2, Constants.DRIVE_BASE_LENGTH / 2); - private final Translation2d backRightLocation = - new Translation2d(-Constants.DRIVE_BASE_WIDTH / 2, -Constants.DRIVE_BASE_LENGTH / 2); - private final SwerveDriveKinematics kinematics = - new SwerveDriveKinematics( - frontLeftLocation, frontRightLocation, backLeftLocation, backRightLocation); - private final LoggableSystem gyroSystem; - private DriveMode driveMode = DriveMode.FIELD_CENTRIC; - private final PoseEstimator poseEstimator; - private boolean facingTarget = false; - - public SwerveDrivetrain( - SwerveModule frontLeftModule, - SwerveModule frontRightModule, - SwerveModule backLeftModule, - SwerveModule backRightModule, - GyroIO gyroIO, - LoggableIO apriltagIO) { - this.frontLeft = frontLeftModule; - this.frontRight = frontRightModule; - this.backLeft = backLeftModule; - this.backRight = backRightModule; - this.gyroSystem = new LoggableSystem<>(gyroIO, new GyroInputs("Gyro")); - this.poseEstimator = - new PoseEstimator( - frontLeft, frontRight, backLeft, backRight, apriltagIO, kinematics, getLastGyro()); - } - - @Override - public void periodic() { - poseEstimator.updateInputs(); - processInputs(); - OdometryMeasurement odom = - new OdometryMeasurement( - new SwerveModulePosition[] { - frontLeft.getPosition(), - frontRight.getPosition(), - backLeft.getPosition(), - backRight.getPosition() - }, - getLastGyro()); - Logger.recordOutput("LastOdomModPoses", odom.modulePosition()); - poseEstimator.updatePosition(odom); - poseEstimator.updateVision(); - Logger.recordOutput( - "realSwerveStates", - frontLeft.getLatestState(), - frontRight.getLatestState(), - backLeft.getLatestState(), - backRight.getLatestState()); - if (Constants.SWERVE_DEBUG) { - SmartShuffleboard.put("Drive", "FL ABS Pos", frontLeft.getAbsPosition()); - SmartShuffleboard.put("Drive", "FR ABS Pos", frontRight.getAbsPosition()); - SmartShuffleboard.put("Drive", "BL ABS Pos", backLeft.getAbsPosition()); - SmartShuffleboard.put("Drive", "BR ABS Pos", backRight.getAbsPosition()); - } - } - - private void processInputs() { - frontLeft.updateInputs(); - frontRight.updateInputs(); - backLeft.updateInputs(); - backRight.updateInputs(); - gyroSystem.updateInputs(); - } - - public ChassisSpeeds createChassisSpeeds( - double xSpeed, double ySpeed, double rotation, DriveMode driveMode) { - return driveMode.equals(DriveMode.FIELD_CENTRIC) - ? ChassisSpeeds.fromFieldRelativeSpeeds( - xSpeed, ySpeed, rotation, Rotation2d.fromDegrees(getLastGyro())) - : new ChassisSpeeds(xSpeed, ySpeed, rotation); - } - - public void drive(ChassisSpeeds speeds) { - SwerveModuleState[] swerveModuleStates = kinematics.toSwerveModuleStates(speeds); - SwerveDriveKinematics.desaturateWheelSpeeds(swerveModuleStates, Constants.MAX_VELOCITY); - setModuleStates(swerveModuleStates); - } - - public ChassisSpeeds speedsFromStates() { - return kinematics.toChassisSpeeds( - frontLeft.getLatestState(), - frontRight.getLatestState(), - backLeft.getLatestState(), - backRight.getLatestState()); - } - - private void setModuleStates(SwerveModuleState[] desiredStates) { - Logger.recordOutput("desiredStates", desiredStates); - frontLeft.setState(desiredStates[0]); - frontRight.setState(desiredStates[1]); - backLeft.setState(desiredStates[2]); - backRight.setState(desiredStates[3]); - } - - public void stopMotor() { - frontLeft.stop(); - frontRight.stop(); - backLeft.stop(); - backRight.stop(); - } - - public void zeroRelativeEncoders() { - frontLeft.resetRelativeEnc(); - frontRight.resetRelativeEnc(); - backLeft.resetRelativeEnc(); - backRight.resetRelativeEnc(); - } - - public void setSteerOffset( - double absEncoderZeroFL, - double absEncoderZeroFR, - double absEncoderZeroBL, - double absEncoderZeroBR) { - frontLeft.setSteerOffset(absEncoderZeroFL); - frontRight.setSteerOffset(absEncoderZeroFR); - backLeft.setSteerOffset(absEncoderZeroBL); - backRight.setSteerOffset(absEncoderZeroBR); - } - - public void resetGyro() { - gyroSystem.getIO().resetGyro(); - } - - public double getLastGyro() { - return gyroSystem.getInputs().anglesInDeg; - } - - public void setDriveMode(DriveMode driveMode) { - this.driveMode = driveMode; - } - - public DriveMode getDriveMode() { - return driveMode; - } - - public Pose2d getPose() { - return poseEstimator.getEstimatedPose(); - } - - public void setGyroOffset(double offset) { - gyroSystem.getIO().setAngleOffset(offset); - } - - public void resetOdometry(Pose2d startingPosition) { - poseEstimator.resetOdometry( - startingPosition.getRotation().getRadians(), startingPosition.getTranslation()); - } - - public Rotation2d getGyroAngle() { - return Rotation2d.fromDegrees(getLastGyro()); - } - - public ChassisSpeeds getChassisSpeeds() { - return kinematics.toChassisSpeeds( - frontLeft.getLatestState(), - frontRight.getLatestState(), - backLeft.getLatestState(), - backRight.getLatestState()); - } - - public ChassisSpeeds getFieldChassisSpeeds() { - return ChassisSpeeds.fromRobotRelativeSpeeds(getChassisSpeeds(), getPose().getRotation()); - } - - public void setFacingTarget(boolean facingTarget) { - this.facingTarget = facingTarget; - } - - public boolean isFacingTarget() { - return facingTarget; - } + private final SwerveModule frontLeft; + private final SwerveModule frontRight; + private final SwerveModule backLeft; + private final SwerveModule backRight; + private final Translation2d frontLeftLocation = + new Translation2d(Constants.DRIVE_BASE_WIDTH / 2, Constants.DRIVE_BASE_LENGTH / 2); + private final Translation2d frontRightLocation = + new Translation2d(Constants.DRIVE_BASE_WIDTH / 2, -Constants.DRIVE_BASE_LENGTH / 2); + private final Translation2d backLeftLocation = + new Translation2d(-Constants.DRIVE_BASE_WIDTH / 2, Constants.DRIVE_BASE_LENGTH / 2); + private final Translation2d backRightLocation = + new Translation2d(-Constants.DRIVE_BASE_WIDTH / 2, -Constants.DRIVE_BASE_LENGTH / 2); + private final SwerveDriveKinematics kinematics = + new SwerveDriveKinematics( + frontLeftLocation, frontRightLocation, backLeftLocation, backRightLocation); + private final LoggableSystem gyroSystem; + private DriveMode driveMode = DriveMode.FIELD_CENTRIC; + private final PoseEstimator poseEstimator; + private boolean facingTarget = false; + //controller will add an additional meter per second in the x direction for every meter of error in the x direction + private final HolonomicDriveController finePathController = new HolonomicDriveController( + new PIDController(1, 0, 0), new PIDController(1, 0, 0), new ProfiledPIDController(1, 0, 0, + new TrapezoidProfile.Constraints(3.5, 1)) + ); + + public SwerveDrivetrain( + SwerveModule frontLeftModule, + SwerveModule frontRightModule, + SwerveModule backLeftModule, + SwerveModule backRightModule, + GyroIO gyroIO, + LoggableIO apriltagIO) { + this.frontLeft = frontLeftModule; + this.frontRight = frontRightModule; + this.backLeft = backLeftModule; + this.backRight = backRightModule; + this.gyroSystem = new LoggableSystem<>(gyroIO, new GyroInputs("Gyro")); + this.poseEstimator = + new PoseEstimator( + frontLeft, frontRight, backLeft, backRight, apriltagIO, kinematics, getLastGyro()); + finePathController.setTolerance(new Pose2d(0.05, 0.05, Rotation2d.fromDegrees(5))); + } + + @Override + public void periodic() { + poseEstimator.updateInputs(); + processInputs(); + OdometryMeasurement odom = + new OdometryMeasurement( + new SwerveModulePosition[]{ + frontLeft.getPosition(), + frontRight.getPosition(), + backLeft.getPosition(), + backRight.getPosition() + }, + getLastGyro()); + Logger.recordOutput("LastOdomModPoses", odom.modulePosition()); + poseEstimator.updatePosition(odom); + poseEstimator.updateVision(); + Logger.recordOutput( + "realSwerveStates", + frontLeft.getLatestState(), + frontRight.getLatestState(), + backLeft.getLatestState(), + backRight.getLatestState()); + if (Constants.SWERVE_DEBUG) { + SmartShuffleboard.put("Drive", "FL ABS Pos", frontLeft.getAbsPosition()); + SmartShuffleboard.put("Drive", "FR ABS Pos", frontRight.getAbsPosition()); + SmartShuffleboard.put("Drive", "BL ABS Pos", backLeft.getAbsPosition()); + SmartShuffleboard.put("Drive", "BR ABS Pos", backRight.getAbsPosition()); + } + } + + private void processInputs() { + frontLeft.updateInputs(); + frontRight.updateInputs(); + backLeft.updateInputs(); + backRight.updateInputs(); + gyroSystem.updateInputs(); + } + + public ChassisSpeeds createChassisSpeeds( + double xSpeed, double ySpeed, double rotation, DriveMode driveMode) { + return driveMode.equals(DriveMode.FIELD_CENTRIC) + ? ChassisSpeeds.fromFieldRelativeSpeeds( + xSpeed, ySpeed, rotation, Rotation2d.fromDegrees(getLastGyro())) + : new ChassisSpeeds(xSpeed, ySpeed, rotation); + } + + public void drive(ChassisSpeeds speeds) { + SwerveModuleState[] swerveModuleStates = kinematics.toSwerveModuleStates(speeds); + SwerveDriveKinematics.desaturateWheelSpeeds(swerveModuleStates, Constants.MAX_VELOCITY); + setModuleStates(swerveModuleStates); + } + + public ChassisSpeeds speedsFromStates() { + return kinematics.toChassisSpeeds( + frontLeft.getLatestState(), + frontRight.getLatestState(), + backLeft.getLatestState(), + backRight.getLatestState()); + } + + private void setModuleStates(SwerveModuleState[] desiredStates) { + Logger.recordOutput("desiredStates", desiredStates); + frontLeft.setState(desiredStates[0]); + frontRight.setState(desiredStates[1]); + backLeft.setState(desiredStates[2]); + backRight.setState(desiredStates[3]); + } + + public void stopMotor() { + frontLeft.stop(); + frontRight.stop(); + backLeft.stop(); + backRight.stop(); + } + + public void zeroRelativeEncoders() { + frontLeft.resetRelativeEnc(); + frontRight.resetRelativeEnc(); + backLeft.resetRelativeEnc(); + backRight.resetRelativeEnc(); + } + + public void setSteerOffset( + double absEncoderZeroFL, + double absEncoderZeroFR, + double absEncoderZeroBL, + double absEncoderZeroBR) { + frontLeft.setSteerOffset(absEncoderZeroFL); + frontRight.setSteerOffset(absEncoderZeroFR); + backLeft.setSteerOffset(absEncoderZeroBL); + backRight.setSteerOffset(absEncoderZeroBR); + } + + public void resetGyro() { + gyroSystem.getIO().resetGyro(); + } + + public double getLastGyro() { + return gyroSystem.getInputs().anglesInDeg; + } + + public void setDriveMode(DriveMode driveMode) { + this.driveMode = driveMode; + } + + public DriveMode getDriveMode() { + return driveMode; + } + + public Pose2d getPose() { + return poseEstimator.getEstimatedPose(); + } + + public void setGyroOffset(double offset) { + gyroSystem.getIO().setAngleOffset(offset); + } + + public void resetOdometry(Pose2d startingPosition) { + poseEstimator.resetOdometry( + startingPosition.getRotation().getRadians(), startingPosition.getTranslation()); + } + + public Rotation2d getGyroAngle() { + return Rotation2d.fromDegrees(getLastGyro()); + } + + public ChassisSpeeds getChassisSpeeds() { + return kinematics.toChassisSpeeds( + frontLeft.getLatestState(), + frontRight.getLatestState(), + backLeft.getLatestState(), + backRight.getLatestState()); + } + + public ChassisSpeeds getFieldChassisSpeeds() { + return ChassisSpeeds.fromRobotRelativeSpeeds(getChassisSpeeds(), getPose().getRotation()); + } + + public void setFacingTarget(boolean facingTarget) { + this.facingTarget = facingTarget; + } + + public boolean isFacingTarget() { + return facingTarget; + } + + public ChassisSpeeds calculateSpeedsTowardsPoint(Pose2d targetPose) { + return finePathController.calculate(getPose(), targetPose, 0, targetPose.getRotation()); + } + + public boolean atTarget(Pose2d targetPose) { + return finePathController.atReference(); + } } From 120a9b53c0785047e895a93549f32de413ce7f4a Mon Sep 17 00:00:00 2001 From: codetoad Date: Sun, 23 Feb 2025 22:24:00 -0500 Subject: [PATCH 17/99] added comment --- .../java/frc/robot/commands/alignment/AlignClosestBranch.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/commands/alignment/AlignClosestBranch.java b/src/main/java/frc/robot/commands/alignment/AlignClosestBranch.java index e4333ebf..c5424095 100644 --- a/src/main/java/frc/robot/commands/alignment/AlignClosestBranch.java +++ b/src/main/java/frc/robot/commands/alignment/AlignClosestBranch.java @@ -31,7 +31,7 @@ public void initialize() { LoggableCommandWrapper.wrap(PathPlannerUtils.pathToPose(targetPosition, 0.0)).withBasicName("GeneralAlign"), new FineAlign(drivetrain, targetPosition) ).withBasicName("PathPlannerToBranch"); - sequence.setParent(this); + sequence.setParent(this); //for advantage scope logging sequence.schedule(); } From 73f03be11657019b08d48be94c60b3017181f5ce Mon Sep 17 00:00:00 2001 From: codetoad Date: Sun, 23 Feb 2025 22:24:19 -0500 Subject: [PATCH 18/99] I forgot to build --- src/main/java/frc/robot/Robot.java | 2 +- .../alignment/AlignClosestBranch.java | 16 +- .../robot/commands/alignment/FineAlign.java | 40 +- .../subsystems/swervev3/SwerveDrivetrain.java | 394 +++++++++--------- .../robot/utils/auto/PathPlannerUtils.java | 2 +- 5 files changed, 227 insertions(+), 227 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index a079ca30..24cac122 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -126,7 +126,7 @@ public void autonomousInit() { new SequentialCommandGroup( new WheelAlign(m_robotContainer.getDrivetrain()), new ResetGyro(m_robotContainer.getDrivetrain())) - .schedule(); + .schedule(); } @Override diff --git a/src/main/java/frc/robot/commands/alignment/AlignClosestBranch.java b/src/main/java/frc/robot/commands/alignment/AlignClosestBranch.java index c5424095..c33b867c 100644 --- a/src/main/java/frc/robot/commands/alignment/AlignClosestBranch.java +++ b/src/main/java/frc/robot/commands/alignment/AlignClosestBranch.java @@ -4,13 +4,11 @@ import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.CommandScheduler; import frc.robot.constants.AlignmentPositions; -import frc.robot.constants.Constants; import frc.robot.subsystems.swervev3.SwerveDrivetrain; import frc.robot.utils.auto.PathPlannerUtils; import frc.robot.utils.logging.commands.LoggableCommand; import frc.robot.utils.logging.commands.LoggableCommandWrapper; import frc.robot.utils.logging.commands.LoggableSequentialCommandGroup; -import frc.robot.utils.math.PoseUtils; public class AlignClosestBranch extends LoggableCommand { private Pose2d targetPosition; @@ -27,17 +25,19 @@ public AlignClosestBranch(SwerveDrivetrain drivetrain) { public void initialize() { timer.restart(); targetPosition = AlignmentPositions.getClosest(drivetrain.getPose()); - LoggableSequentialCommandGroup sequence = new LoggableSequentialCommandGroup( - LoggableCommandWrapper.wrap(PathPlannerUtils.pathToPose(targetPosition, 0.0)).withBasicName("GeneralAlign"), - new FineAlign(drivetrain, targetPosition) - ).withBasicName("PathPlannerToBranch"); - sequence.setParent(this); //for advantage scope logging + LoggableSequentialCommandGroup sequence = + new LoggableSequentialCommandGroup( + LoggableCommandWrapper.wrap(PathPlannerUtils.pathToPose(targetPosition, 0.0)) + .withBasicName("GeneralAlign"), + new FineAlign(drivetrain, targetPosition)) + .withBasicName("PathPlannerToBranch"); + sequence.setParent(this); // for advantage scope logging sequence.schedule(); } @Override public void end(boolean interrupted) { - if (CommandScheduler.getInstance().isScheduled(driveSequence)){ + if (CommandScheduler.getInstance().isScheduled(driveSequence)) { CommandScheduler.getInstance().cancel(driveSequence); } } diff --git a/src/main/java/frc/robot/commands/alignment/FineAlign.java b/src/main/java/frc/robot/commands/alignment/FineAlign.java index 8248bf09..62694ee6 100644 --- a/src/main/java/frc/robot/commands/alignment/FineAlign.java +++ b/src/main/java/frc/robot/commands/alignment/FineAlign.java @@ -5,30 +5,28 @@ import frc.robot.subsystems.swervev3.SwerveDrivetrain; import frc.robot.utils.logging.commands.LoggableCommand; -import java.util.function.Supplier; - public class FineAlign extends LoggableCommand { - private final SwerveDrivetrain drivetrain; - private Pose2d targetPose; - private final Timer timer = new Timer(); + private final SwerveDrivetrain drivetrain; + private Pose2d targetPose; + private final Timer timer = new Timer(); - public FineAlign(SwerveDrivetrain drivetrain, Pose2d targetPose){ - this.drivetrain = drivetrain; - this.targetPose = targetPose; - } + public FineAlign(SwerveDrivetrain drivetrain, Pose2d targetPose) { + this.drivetrain = drivetrain; + this.targetPose = targetPose; + } - @Override - public void initialize() { - timer.restart(); - } + @Override + public void initialize() { + timer.restart(); + } - @Override - public void execute() { - drivetrain.drive(drivetrain.calculateSpeedsTowardsPoint(targetPose)); - } + @Override + public void execute() { + drivetrain.drive(drivetrain.calculateSpeedsTowardsPoint(targetPose)); + } - @Override - public boolean isFinished() { - return drivetrain.atTarget(targetPose) || timer.hasElapsed(5); - } + @Override + public boolean isFinished() { + return drivetrain.atTarget(targetPose) || timer.hasElapsed(5); + } } diff --git a/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java index 48e7f566..a13c07ce 100644 --- a/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java @@ -26,200 +26,202 @@ import org.littletonrobotics.junction.Logger; public class SwerveDrivetrain extends SubsystemBase { - private final SwerveModule frontLeft; - private final SwerveModule frontRight; - private final SwerveModule backLeft; - private final SwerveModule backRight; - private final Translation2d frontLeftLocation = - new Translation2d(Constants.DRIVE_BASE_WIDTH / 2, Constants.DRIVE_BASE_LENGTH / 2); - private final Translation2d frontRightLocation = - new Translation2d(Constants.DRIVE_BASE_WIDTH / 2, -Constants.DRIVE_BASE_LENGTH / 2); - private final Translation2d backLeftLocation = - new Translation2d(-Constants.DRIVE_BASE_WIDTH / 2, Constants.DRIVE_BASE_LENGTH / 2); - private final Translation2d backRightLocation = - new Translation2d(-Constants.DRIVE_BASE_WIDTH / 2, -Constants.DRIVE_BASE_LENGTH / 2); - private final SwerveDriveKinematics kinematics = - new SwerveDriveKinematics( - frontLeftLocation, frontRightLocation, backLeftLocation, backRightLocation); - private final LoggableSystem gyroSystem; - private DriveMode driveMode = DriveMode.FIELD_CENTRIC; - private final PoseEstimator poseEstimator; - private boolean facingTarget = false; - //controller will add an additional meter per second in the x direction for every meter of error in the x direction - private final HolonomicDriveController finePathController = new HolonomicDriveController( - new PIDController(1, 0, 0), new PIDController(1, 0, 0), new ProfiledPIDController(1, 0, 0, - new TrapezoidProfile.Constraints(3.5, 1)) - ); - - public SwerveDrivetrain( - SwerveModule frontLeftModule, - SwerveModule frontRightModule, - SwerveModule backLeftModule, - SwerveModule backRightModule, - GyroIO gyroIO, - LoggableIO apriltagIO) { - this.frontLeft = frontLeftModule; - this.frontRight = frontRightModule; - this.backLeft = backLeftModule; - this.backRight = backRightModule; - this.gyroSystem = new LoggableSystem<>(gyroIO, new GyroInputs("Gyro")); - this.poseEstimator = - new PoseEstimator( - frontLeft, frontRight, backLeft, backRight, apriltagIO, kinematics, getLastGyro()); - finePathController.setTolerance(new Pose2d(0.05, 0.05, Rotation2d.fromDegrees(5))); - } - - @Override - public void periodic() { - poseEstimator.updateInputs(); - processInputs(); - OdometryMeasurement odom = - new OdometryMeasurement( - new SwerveModulePosition[]{ - frontLeft.getPosition(), - frontRight.getPosition(), - backLeft.getPosition(), - backRight.getPosition() - }, - getLastGyro()); - Logger.recordOutput("LastOdomModPoses", odom.modulePosition()); - poseEstimator.updatePosition(odom); - poseEstimator.updateVision(); - Logger.recordOutput( - "realSwerveStates", - frontLeft.getLatestState(), - frontRight.getLatestState(), - backLeft.getLatestState(), - backRight.getLatestState()); - if (Constants.SWERVE_DEBUG) { - SmartShuffleboard.put("Drive", "FL ABS Pos", frontLeft.getAbsPosition()); - SmartShuffleboard.put("Drive", "FR ABS Pos", frontRight.getAbsPosition()); - SmartShuffleboard.put("Drive", "BL ABS Pos", backLeft.getAbsPosition()); - SmartShuffleboard.put("Drive", "BR ABS Pos", backRight.getAbsPosition()); - } - } - - private void processInputs() { - frontLeft.updateInputs(); - frontRight.updateInputs(); - backLeft.updateInputs(); - backRight.updateInputs(); - gyroSystem.updateInputs(); - } - - public ChassisSpeeds createChassisSpeeds( - double xSpeed, double ySpeed, double rotation, DriveMode driveMode) { - return driveMode.equals(DriveMode.FIELD_CENTRIC) - ? ChassisSpeeds.fromFieldRelativeSpeeds( - xSpeed, ySpeed, rotation, Rotation2d.fromDegrees(getLastGyro())) - : new ChassisSpeeds(xSpeed, ySpeed, rotation); - } - - public void drive(ChassisSpeeds speeds) { - SwerveModuleState[] swerveModuleStates = kinematics.toSwerveModuleStates(speeds); - SwerveDriveKinematics.desaturateWheelSpeeds(swerveModuleStates, Constants.MAX_VELOCITY); - setModuleStates(swerveModuleStates); - } - - public ChassisSpeeds speedsFromStates() { - return kinematics.toChassisSpeeds( - frontLeft.getLatestState(), - frontRight.getLatestState(), - backLeft.getLatestState(), - backRight.getLatestState()); - } - - private void setModuleStates(SwerveModuleState[] desiredStates) { - Logger.recordOutput("desiredStates", desiredStates); - frontLeft.setState(desiredStates[0]); - frontRight.setState(desiredStates[1]); - backLeft.setState(desiredStates[2]); - backRight.setState(desiredStates[3]); - } - - public void stopMotor() { - frontLeft.stop(); - frontRight.stop(); - backLeft.stop(); - backRight.stop(); - } - - public void zeroRelativeEncoders() { - frontLeft.resetRelativeEnc(); - frontRight.resetRelativeEnc(); - backLeft.resetRelativeEnc(); - backRight.resetRelativeEnc(); - } - - public void setSteerOffset( - double absEncoderZeroFL, - double absEncoderZeroFR, - double absEncoderZeroBL, - double absEncoderZeroBR) { - frontLeft.setSteerOffset(absEncoderZeroFL); - frontRight.setSteerOffset(absEncoderZeroFR); - backLeft.setSteerOffset(absEncoderZeroBL); - backRight.setSteerOffset(absEncoderZeroBR); - } - - public void resetGyro() { - gyroSystem.getIO().resetGyro(); - } - - public double getLastGyro() { - return gyroSystem.getInputs().anglesInDeg; - } - - public void setDriveMode(DriveMode driveMode) { - this.driveMode = driveMode; - } - - public DriveMode getDriveMode() { - return driveMode; - } - - public Pose2d getPose() { - return poseEstimator.getEstimatedPose(); - } - - public void setGyroOffset(double offset) { - gyroSystem.getIO().setAngleOffset(offset); - } - - public void resetOdometry(Pose2d startingPosition) { - poseEstimator.resetOdometry( - startingPosition.getRotation().getRadians(), startingPosition.getTranslation()); - } - - public Rotation2d getGyroAngle() { - return Rotation2d.fromDegrees(getLastGyro()); - } - - public ChassisSpeeds getChassisSpeeds() { - return kinematics.toChassisSpeeds( - frontLeft.getLatestState(), - frontRight.getLatestState(), - backLeft.getLatestState(), - backRight.getLatestState()); - } - - public ChassisSpeeds getFieldChassisSpeeds() { - return ChassisSpeeds.fromRobotRelativeSpeeds(getChassisSpeeds(), getPose().getRotation()); - } - - public void setFacingTarget(boolean facingTarget) { - this.facingTarget = facingTarget; - } - - public boolean isFacingTarget() { - return facingTarget; - } - - public ChassisSpeeds calculateSpeedsTowardsPoint(Pose2d targetPose) { - return finePathController.calculate(getPose(), targetPose, 0, targetPose.getRotation()); - } - - public boolean atTarget(Pose2d targetPose) { - return finePathController.atReference(); - } + private final SwerveModule frontLeft; + private final SwerveModule frontRight; + private final SwerveModule backLeft; + private final SwerveModule backRight; + private final Translation2d frontLeftLocation = + new Translation2d(Constants.DRIVE_BASE_WIDTH / 2, Constants.DRIVE_BASE_LENGTH / 2); + private final Translation2d frontRightLocation = + new Translation2d(Constants.DRIVE_BASE_WIDTH / 2, -Constants.DRIVE_BASE_LENGTH / 2); + private final Translation2d backLeftLocation = + new Translation2d(-Constants.DRIVE_BASE_WIDTH / 2, Constants.DRIVE_BASE_LENGTH / 2); + private final Translation2d backRightLocation = + new Translation2d(-Constants.DRIVE_BASE_WIDTH / 2, -Constants.DRIVE_BASE_LENGTH / 2); + private final SwerveDriveKinematics kinematics = + new SwerveDriveKinematics( + frontLeftLocation, frontRightLocation, backLeftLocation, backRightLocation); + private final LoggableSystem gyroSystem; + private DriveMode driveMode = DriveMode.FIELD_CENTRIC; + private final PoseEstimator poseEstimator; + private boolean facingTarget = false; + // controller will add an additional meter per second in the x direction for every meter of error + // in the x direction + private final HolonomicDriveController finePathController = + new HolonomicDriveController( + new PIDController(1, 0, 0), + new PIDController(1, 0, 0), + new ProfiledPIDController(1, 0, 0, new TrapezoidProfile.Constraints(3.5, 1))); + + public SwerveDrivetrain( + SwerveModule frontLeftModule, + SwerveModule frontRightModule, + SwerveModule backLeftModule, + SwerveModule backRightModule, + GyroIO gyroIO, + LoggableIO apriltagIO) { + this.frontLeft = frontLeftModule; + this.frontRight = frontRightModule; + this.backLeft = backLeftModule; + this.backRight = backRightModule; + this.gyroSystem = new LoggableSystem<>(gyroIO, new GyroInputs("Gyro")); + this.poseEstimator = + new PoseEstimator( + frontLeft, frontRight, backLeft, backRight, apriltagIO, kinematics, getLastGyro()); + finePathController.setTolerance(new Pose2d(0.05, 0.05, Rotation2d.fromDegrees(5))); + } + + @Override + public void periodic() { + poseEstimator.updateInputs(); + processInputs(); + OdometryMeasurement odom = + new OdometryMeasurement( + new SwerveModulePosition[] { + frontLeft.getPosition(), + frontRight.getPosition(), + backLeft.getPosition(), + backRight.getPosition() + }, + getLastGyro()); + Logger.recordOutput("LastOdomModPoses", odom.modulePosition()); + poseEstimator.updatePosition(odom); + poseEstimator.updateVision(); + Logger.recordOutput( + "realSwerveStates", + frontLeft.getLatestState(), + frontRight.getLatestState(), + backLeft.getLatestState(), + backRight.getLatestState()); + if (Constants.SWERVE_DEBUG) { + SmartShuffleboard.put("Drive", "FL ABS Pos", frontLeft.getAbsPosition()); + SmartShuffleboard.put("Drive", "FR ABS Pos", frontRight.getAbsPosition()); + SmartShuffleboard.put("Drive", "BL ABS Pos", backLeft.getAbsPosition()); + SmartShuffleboard.put("Drive", "BR ABS Pos", backRight.getAbsPosition()); + } + } + + private void processInputs() { + frontLeft.updateInputs(); + frontRight.updateInputs(); + backLeft.updateInputs(); + backRight.updateInputs(); + gyroSystem.updateInputs(); + } + + public ChassisSpeeds createChassisSpeeds( + double xSpeed, double ySpeed, double rotation, DriveMode driveMode) { + return driveMode.equals(DriveMode.FIELD_CENTRIC) + ? ChassisSpeeds.fromFieldRelativeSpeeds( + xSpeed, ySpeed, rotation, Rotation2d.fromDegrees(getLastGyro())) + : new ChassisSpeeds(xSpeed, ySpeed, rotation); + } + + public void drive(ChassisSpeeds speeds) { + SwerveModuleState[] swerveModuleStates = kinematics.toSwerveModuleStates(speeds); + SwerveDriveKinematics.desaturateWheelSpeeds(swerveModuleStates, Constants.MAX_VELOCITY); + setModuleStates(swerveModuleStates); + } + + public ChassisSpeeds speedsFromStates() { + return kinematics.toChassisSpeeds( + frontLeft.getLatestState(), + frontRight.getLatestState(), + backLeft.getLatestState(), + backRight.getLatestState()); + } + + private void setModuleStates(SwerveModuleState[] desiredStates) { + Logger.recordOutput("desiredStates", desiredStates); + frontLeft.setState(desiredStates[0]); + frontRight.setState(desiredStates[1]); + backLeft.setState(desiredStates[2]); + backRight.setState(desiredStates[3]); + } + + public void stopMotor() { + frontLeft.stop(); + frontRight.stop(); + backLeft.stop(); + backRight.stop(); + } + + public void zeroRelativeEncoders() { + frontLeft.resetRelativeEnc(); + frontRight.resetRelativeEnc(); + backLeft.resetRelativeEnc(); + backRight.resetRelativeEnc(); + } + + public void setSteerOffset( + double absEncoderZeroFL, + double absEncoderZeroFR, + double absEncoderZeroBL, + double absEncoderZeroBR) { + frontLeft.setSteerOffset(absEncoderZeroFL); + frontRight.setSteerOffset(absEncoderZeroFR); + backLeft.setSteerOffset(absEncoderZeroBL); + backRight.setSteerOffset(absEncoderZeroBR); + } + + public void resetGyro() { + gyroSystem.getIO().resetGyro(); + } + + public double getLastGyro() { + return gyroSystem.getInputs().anglesInDeg; + } + + public void setDriveMode(DriveMode driveMode) { + this.driveMode = driveMode; + } + + public DriveMode getDriveMode() { + return driveMode; + } + + public Pose2d getPose() { + return poseEstimator.getEstimatedPose(); + } + + public void setGyroOffset(double offset) { + gyroSystem.getIO().setAngleOffset(offset); + } + + public void resetOdometry(Pose2d startingPosition) { + poseEstimator.resetOdometry( + startingPosition.getRotation().getRadians(), startingPosition.getTranslation()); + } + + public Rotation2d getGyroAngle() { + return Rotation2d.fromDegrees(getLastGyro()); + } + + public ChassisSpeeds getChassisSpeeds() { + return kinematics.toChassisSpeeds( + frontLeft.getLatestState(), + frontRight.getLatestState(), + backLeft.getLatestState(), + backRight.getLatestState()); + } + + public ChassisSpeeds getFieldChassisSpeeds() { + return ChassisSpeeds.fromRobotRelativeSpeeds(getChassisSpeeds(), getPose().getRotation()); + } + + public void setFacingTarget(boolean facingTarget) { + this.facingTarget = facingTarget; + } + + public boolean isFacingTarget() { + return facingTarget; + } + + public ChassisSpeeds calculateSpeedsTowardsPoint(Pose2d targetPose) { + return finePathController.calculate(getPose(), targetPose, 0, targetPose.getRotation()); + } + + public boolean atTarget(Pose2d targetPose) { + return finePathController.atReference(); + } } diff --git a/src/main/java/frc/robot/utils/auto/PathPlannerUtils.java b/src/main/java/frc/robot/utils/auto/PathPlannerUtils.java index 7365cc43..6fbbee0e 100644 --- a/src/main/java/frc/robot/utils/auto/PathPlannerUtils.java +++ b/src/main/java/frc/robot/utils/auto/PathPlannerUtils.java @@ -26,7 +26,7 @@ public class PathPlannerUtils { tempConf = RobotConfig.fromGUISettings(); tempConstraints = new PathConstraints( - 1.0, //tempConf.moduleConfig.maxDriveVelocityMPS, + 1.0, // tempConf.moduleConfig.maxDriveVelocityMPS, Constants.MAX_PATHPLANNER_ACCEL, tempConf.moduleConfig.maxDriveVelocityRadPerSec, Constants.MAX_PATHPLANNER_ANGULAR_ACCEL); From bab2f46d6e88601f7bdc797d421b2cd2ccdd5fa9 Mon Sep 17 00:00:00 2001 From: codetoad Date: Sun, 23 Feb 2025 22:28:48 -0500 Subject: [PATCH 19/99] adjusted constant --- .../java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java index a13c07ce..b6d327cd 100644 --- a/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java @@ -51,7 +51,7 @@ public class SwerveDrivetrain extends SubsystemBase { new HolonomicDriveController( new PIDController(1, 0, 0), new PIDController(1, 0, 0), - new ProfiledPIDController(1, 0, 0, new TrapezoidProfile.Constraints(3.5, 1))); + new ProfiledPIDController(1, 0, 0, new TrapezoidProfile.Constraints(0.5, 1))); public SwerveDrivetrain( SwerveModule frontLeftModule, From 0634b7e56750a538f1c547f08b1f427842aa0a0e Mon Sep 17 00:00:00 2001 From: codetoad Date: Mon, 24 Feb 2025 20:41:06 -0500 Subject: [PATCH 20/99] use swerve drive trajectory --- .../alignment/AlignClosestBranch.java | 23 ++++++------- .../robot/commands/alignment/FineAlign.java | 32 ------------------- .../frc/robot/constants/GameConstants.java | 20 ++++++------ .../subsystems/swervev3/SwerveDrivetrain.java | 31 +++++++++++++----- 4 files changed, 42 insertions(+), 64 deletions(-) delete mode 100644 src/main/java/frc/robot/commands/alignment/FineAlign.java diff --git a/src/main/java/frc/robot/commands/alignment/AlignClosestBranch.java b/src/main/java/frc/robot/commands/alignment/AlignClosestBranch.java index c33b867c..9ac7de72 100644 --- a/src/main/java/frc/robot/commands/alignment/AlignClosestBranch.java +++ b/src/main/java/frc/robot/commands/alignment/AlignClosestBranch.java @@ -5,15 +5,14 @@ import edu.wpi.first.wpilibj2.command.CommandScheduler; import frc.robot.constants.AlignmentPositions; import frc.robot.subsystems.swervev3.SwerveDrivetrain; -import frc.robot.utils.auto.PathPlannerUtils; import frc.robot.utils.logging.commands.LoggableCommand; import frc.robot.utils.logging.commands.LoggableCommandWrapper; -import frc.robot.utils.logging.commands.LoggableSequentialCommandGroup; +import org.littletonrobotics.junction.Logger; public class AlignClosestBranch extends LoggableCommand { private Pose2d targetPosition; private final SwerveDrivetrain drivetrain; - private LoggableSequentialCommandGroup driveSequence; + private LoggableCommand followTrajectory; private final Timer timer = new Timer(); public AlignClosestBranch(SwerveDrivetrain drivetrain) { @@ -25,25 +24,21 @@ public AlignClosestBranch(SwerveDrivetrain drivetrain) { public void initialize() { timer.restart(); targetPosition = AlignmentPositions.getClosest(drivetrain.getPose()); - LoggableSequentialCommandGroup sequence = - new LoggableSequentialCommandGroup( - LoggableCommandWrapper.wrap(PathPlannerUtils.pathToPose(targetPosition, 0.0)) - .withBasicName("GeneralAlign"), - new FineAlign(drivetrain, targetPosition)) - .withBasicName("PathPlannerToBranch"); - sequence.setParent(this); // for advantage scope logging - sequence.schedule(); + Logger.recordOutput("TargetReefPose", targetPosition); + followTrajectory = LoggableCommandWrapper.wrap(drivetrain.generateTrajectoryCommand(targetPosition)); + followTrajectory.setParent(this); + followTrajectory.schedule(); } @Override public void end(boolean interrupted) { - if (CommandScheduler.getInstance().isScheduled(driveSequence)) { - CommandScheduler.getInstance().cancel(driveSequence); + if (CommandScheduler.getInstance().isScheduled(followTrajectory)) { + CommandScheduler.getInstance().cancel(followTrajectory); } } @Override public boolean isFinished() { - return driveSequence.isFinished() | timer.hasElapsed(10); + return followTrajectory.isFinished() | timer.hasElapsed(10); } } diff --git a/src/main/java/frc/robot/commands/alignment/FineAlign.java b/src/main/java/frc/robot/commands/alignment/FineAlign.java deleted file mode 100644 index 62694ee6..00000000 --- a/src/main/java/frc/robot/commands/alignment/FineAlign.java +++ /dev/null @@ -1,32 +0,0 @@ -package frc.robot.commands.alignment; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.wpilibj.Timer; -import frc.robot.subsystems.swervev3.SwerveDrivetrain; -import frc.robot.utils.logging.commands.LoggableCommand; - -public class FineAlign extends LoggableCommand { - private final SwerveDrivetrain drivetrain; - private Pose2d targetPose; - private final Timer timer = new Timer(); - - public FineAlign(SwerveDrivetrain drivetrain, Pose2d targetPose) { - this.drivetrain = drivetrain; - this.targetPose = targetPose; - } - - @Override - public void initialize() { - timer.restart(); - } - - @Override - public void execute() { - drivetrain.drive(drivetrain.calculateSpeedsTowardsPoint(targetPose)); - } - - @Override - public boolean isFinished() { - return drivetrain.atTarget(targetPose) || timer.hasElapsed(5); - } -} diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java index 83345661..a155b086 100644 --- a/src/main/java/frc/robot/constants/GameConstants.java +++ b/src/main/java/frc/robot/constants/GameConstants.java @@ -21,16 +21,16 @@ public class GameConstants { public static final double GYRO_DIAGS_ANGLE = 30; // Debug - public static final boolean SWERVE_DEBUG = true; - public static final boolean INTAKE_DEBUG = true; - public static final boolean CLIMBER_DEBUG = true; - public static final boolean ELEVATOR_DEBUG = true; - public static final boolean CORAL_DEBUG = true; - public static final boolean HIHI_DEBUG = true; - public static final boolean BYEBYE_DEBUG = true; - public static final boolean COMMAND_DEBUG = true; - public static final boolean INPUTS_DEBUG = true; - public static final boolean TUNING_MODE = true; + public static final boolean SWERVE_DEBUG = false; + public static final boolean INTAKE_DEBUG = false; + public static final boolean CLIMBER_DEBUG = false; + public static final boolean ELEVATOR_DEBUG = false; + public static final boolean CORAL_DEBUG = false; + public static final boolean HIHI_DEBUG = false; + public static final boolean BYEBYE_DEBUG = false; + public static final boolean COMMAND_DEBUG = false; + public static final boolean INPUTS_DEBUG = false; + public static final boolean TUNING_MODE = false; // Speeds public static final double MAX_AUTO_ALIGN_SPEED = 0.9; diff --git a/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java index b6d327cd..8965cdec 100644 --- a/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java @@ -10,8 +10,13 @@ import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.math.trajectory.Trajectory; +import edu.wpi.first.math.trajectory.TrajectoryConfig; +import edu.wpi.first.math.trajectory.TrajectoryGenerator; import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.SwerveControllerCommand; import frc.robot.apriltags.ApriltagInputs; import frc.robot.constants.Constants; import frc.robot.subsystems.gyro.GyroIO; @@ -21,8 +26,11 @@ import frc.robot.subsystems.swervev3.io.SwerveModule; import frc.robot.utils.DriveMode; import frc.robot.utils.logging.LoggableIO; +import frc.robot.utils.logging.commands.LoggableCommand; +import frc.robot.utils.logging.commands.LoggableCommandWrapper; import frc.robot.utils.logging.subsystem.LoggableSystem; import frc.robot.utils.shuffleboard.SmartShuffleboard; +import java.util.Collections; import org.littletonrobotics.junction.Logger; public class SwerveDrivetrain extends SubsystemBase { @@ -45,13 +53,14 @@ public class SwerveDrivetrain extends SubsystemBase { private DriveMode driveMode = DriveMode.FIELD_CENTRIC; private final PoseEstimator poseEstimator; private boolean facingTarget = false; + private TrajectoryConfig trajectoryConfig; // controller will add an additional meter per second in the x direction for every meter of error // in the x direction private final HolonomicDriveController finePathController = new HolonomicDriveController( new PIDController(1, 0, 0), new PIDController(1, 0, 0), - new ProfiledPIDController(1, 0, 0, new TrapezoidProfile.Constraints(0.5, 1))); + new ProfiledPIDController(1, 0, 0, new TrapezoidProfile.Constraints(6.28, 3.14))); public SwerveDrivetrain( SwerveModule frontLeftModule, @@ -68,7 +77,8 @@ public SwerveDrivetrain( this.poseEstimator = new PoseEstimator( frontLeft, frontRight, backLeft, backRight, apriltagIO, kinematics, getLastGyro()); - finePathController.setTolerance(new Pose2d(0.05, 0.05, Rotation2d.fromDegrees(5))); + finePathController.setTolerance(new Pose2d(0.025, 0.025, Rotation2d.fromDegrees(5))); + trajectoryConfig = new TrajectoryConfig(0.25, 0.25); } @Override @@ -217,11 +227,16 @@ public boolean isFacingTarget() { return facingTarget; } - public ChassisSpeeds calculateSpeedsTowardsPoint(Pose2d targetPose) { - return finePathController.calculate(getPose(), targetPose, 0, targetPose.getRotation()); - } - - public boolean atTarget(Pose2d targetPose) { - return finePathController.atReference(); + public Command generateTrajectoryCommand(Pose2d targetPose) { + Trajectory trajectory = + TrajectoryGenerator.generateTrajectory( + getPose(), Collections.emptyList(), targetPose, trajectoryConfig); + return new SwerveControllerCommand( + trajectory, + this::getPose, + kinematics, // Position controllers + finePathController, + this::setModuleStates, + this); } } From fea62fdc5d142014e6c6c7c92101d69c8919a0df Mon Sep 17 00:00:00 2001 From: codetoad Date: Wed, 26 Feb 2025 20:18:40 -0500 Subject: [PATCH 21/99] update --- .../commands/alignment/AlignClosestBranch.java | 3 ++- .../java/frc/robot/constants/Constants2025.java | 4 ++-- .../java/frc/robot/constants/GameConstants.java | 8 +++++--- .../subsystems/swervev3/SwerveDrivetrain.java | 14 ++++++-------- 4 files changed, 15 insertions(+), 14 deletions(-) diff --git a/src/main/java/frc/robot/commands/alignment/AlignClosestBranch.java b/src/main/java/frc/robot/commands/alignment/AlignClosestBranch.java index 9ac7de72..aeec2b03 100644 --- a/src/main/java/frc/robot/commands/alignment/AlignClosestBranch.java +++ b/src/main/java/frc/robot/commands/alignment/AlignClosestBranch.java @@ -25,7 +25,8 @@ public void initialize() { timer.restart(); targetPosition = AlignmentPositions.getClosest(drivetrain.getPose()); Logger.recordOutput("TargetReefPose", targetPosition); - followTrajectory = LoggableCommandWrapper.wrap(drivetrain.generateTrajectoryCommand(targetPosition)); + followTrajectory = + LoggableCommandWrapper.wrap(drivetrain.generateTrajectoryCommand(targetPosition)); followTrajectory.setParent(this); followTrajectory.schedule(); } diff --git a/src/main/java/frc/robot/constants/Constants2025.java b/src/main/java/frc/robot/constants/Constants2025.java index c6de3d9f..3b195f5e 100644 --- a/src/main/java/frc/robot/constants/Constants2025.java +++ b/src/main/java/frc/robot/constants/Constants2025.java @@ -45,8 +45,8 @@ public class Constants2025 extends GameConstants { public static final int DRIVE_BACK_LEFT_S = 49; // Measured distance between the center of the wheels - public static final double DRIVE_BASE_WIDTH = 0.635; + public static final double DRIVE_BASE_WIDTH = 0.61595; // Measured distance between the center of the wheels - public static final double DRIVE_BASE_LENGTH = 0.635; + public static final double DRIVE_BASE_LENGTH = 0.61595; } diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java index a155b086..c2be298e 100644 --- a/src/main/java/frc/robot/constants/GameConstants.java +++ b/src/main/java/frc/robot/constants/GameConstants.java @@ -130,11 +130,13 @@ public enum Mode { public static final boolean ELEVATOR_USE_MAX_MOTION = true; // Drive PID - public static final double DRIVE_PID_P = 1; // TODO: change later + public static final double DRIVE_PID_P = 2; // TODO: change later public static final double DRIVE_PID_I = 0; // TODO: change later public static final double DRIVE_PID_D = 0; // TODO: change later - public static final double DRIVE_PID_FF_S = 1; // TODO: change later - public static final double DRIVE_PID_FF_V = 2.8; // TODO: change later + public static final double DRIVE_PID_FF_S = 0.19; + public static final double DRIVE_PID_FF_V = 3.3; + public static final double DRIVE_PID_I_ZONE = 0; // TODO: change later + public static final double DRIVE_PID_ALLOWED_ERROR = 0; // Steer PID public static final double STEER_PID_P = 0.3; // TODO: change later diff --git a/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java index 8965cdec..38a368b6 100644 --- a/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java @@ -26,8 +26,6 @@ import frc.robot.subsystems.swervev3.io.SwerveModule; import frc.robot.utils.DriveMode; import frc.robot.utils.logging.LoggableIO; -import frc.robot.utils.logging.commands.LoggableCommand; -import frc.robot.utils.logging.commands.LoggableCommandWrapper; import frc.robot.utils.logging.subsystem.LoggableSystem; import frc.robot.utils.shuffleboard.SmartShuffleboard; import java.util.Collections; @@ -232,11 +230,11 @@ public Command generateTrajectoryCommand(Pose2d targetPose) { TrajectoryGenerator.generateTrajectory( getPose(), Collections.emptyList(), targetPose, trajectoryConfig); return new SwerveControllerCommand( - trajectory, - this::getPose, - kinematics, // Position controllers - finePathController, - this::setModuleStates, - this); + trajectory, + this::getPose, + kinematics, // Position controllers + finePathController, + this::setModuleStates, + this); } } From dfc59ca7d20e56755738c697ca1c7dc2b68a7d94 Mon Sep 17 00:00:00 2001 From: codetoad Date: Tue, 4 Mar 2025 21:53:31 -0500 Subject: [PATCH 22/99] stuff --- src/main/java/frc/robot/RobotContainer.java | 11 +++-------- .../java/frc/robot/autochooser/FieldLocation.java | 1 - .../java/frc/robot/constants/GameConstants.java | 2 +- .../subsystems/swervev3/SwerveDrivetrain.java | 15 +++++++-------- 4 files changed, 11 insertions(+), 18 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 25acdb0b..fe3b630a 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -5,10 +5,10 @@ package frc.robot; import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.auto.NamedCommands; import com.pathplanner.lib.config.PIDConstants; import com.pathplanner.lib.config.RobotConfig; import com.pathplanner.lib.controllers.PPHolonomicDriveController; -import com.pathplanner.lib.auto.NamedCommands; import com.studica.frc.AHRS; import com.studica.frc.AHRS.NavXComType; import edu.wpi.first.math.trajectory.TrapezoidProfile; @@ -45,13 +45,7 @@ import frc.robot.commands.hihi.ShootHiHiRollerOut; import frc.robot.commands.lightStrip.SetLedFromElevatorPosition; import frc.robot.commands.lightStrip.SetLedPattern; -import frc.robot.commands.sequences.ByeByeAllDone; -import frc.robot.commands.sequences.CancelAll; -import frc.robot.commands.sequences.IntakeAlgae; -import frc.robot.commands.sequences.LowerElevator; -import frc.robot.commands.sequences.PickUpCoral; -import frc.robot.commands.sequences.RemoveAlgaeFromReef; -import frc.robot.commands.sequences.ShootAlgae; +import frc.robot.commands.sequences.*; import frc.robot.constants.Constants; import frc.robot.constants.ElevatorPosition; import frc.robot.subsystems.algaebyebyeroller.AlgaeByeByeRollerSubsystem; @@ -101,6 +95,7 @@ import frc.robot.utils.logging.LoggableIO; import frc.robot.utils.motor.Gain; import frc.robot.utils.motor.PID; +import frc.robot.utils.shuffleboard.SmartShuffleboard; import java.util.Optional; public class RobotContainer { diff --git a/src/main/java/frc/robot/autochooser/FieldLocation.java b/src/main/java/frc/robot/autochooser/FieldLocation.java index a9aeba32..0777987a 100644 --- a/src/main/java/frc/robot/autochooser/FieldLocation.java +++ b/src/main/java/frc/robot/autochooser/FieldLocation.java @@ -1,6 +1,5 @@ package frc.robot.autochooser; -import edu.wpi.first.apriltag.AprilTag; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.util.Units; diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java index b009c0cf..517fa09b 100644 --- a/src/main/java/frc/robot/constants/GameConstants.java +++ b/src/main/java/frc/robot/constants/GameConstants.java @@ -44,7 +44,7 @@ public class GameConstants { public static final double BYEBYE_REVERSE_SPEED = -0.7; // TODO: change later public static final double INTAKE_MOTOR_SPEED = 0.25; public static final double INTAKE_TILT_VELOCITY = 0.5; - public static final double CORAL_SHOOTER_SPEED = 0.7; + public static final double CORAL_SHOOTER_SPEED = 0.5; public static final double HIHI_EXTEND_SPEED = 0.4; public static final double HIHI_RETRACT_SPEED = -0.15; public static final double HIHI_INTAKE_SPEED = 0.7; diff --git a/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java index 231a0b72..fcbf9063 100644 --- a/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java @@ -1,13 +1,13 @@ package frc.robot.subsystems.swervev3; -import edu.wpi.first.math.controller.HolonomicDriveController; -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.controller.ProfiledPIDController; import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.config.PIDConstants; import com.pathplanner.lib.config.RobotConfig; import com.pathplanner.lib.controllers.PPHolonomicDriveController; import edu.wpi.first.math.Vector; +import edu.wpi.first.math.controller.HolonomicDriveController; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; @@ -15,13 +15,13 @@ import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.math.numbers.N3; import edu.wpi.first.math.trajectory.Trajectory; import edu.wpi.first.math.trajectory.TrajectoryConfig; import edu.wpi.first.math.trajectory.TrajectoryGenerator; import edu.wpi.first.math.trajectory.TrapezoidProfile; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.math.numbers.N3; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.SwerveControllerCommand; import frc.robot.apriltags.ApriltagInputs; @@ -35,7 +35,6 @@ import frc.robot.utils.DriveMode; import frc.robot.utils.logging.LoggableIO; import frc.robot.utils.logging.subsystem.LoggableSystem; -import frc.robot.utils.shuffleboard.SmartShuffleboard; import java.util.Collections; import org.littletonrobotics.junction.Logger; @@ -83,8 +82,8 @@ public SwerveDrivetrain( this.poseEstimator = new PoseEstimator( frontLeft, frontRight, backLeft, backRight, apriltagIO, kinematics, getLastGyro()); - finePathController.setTolerance(new Pose2d(0.025, 0.025, Rotation2d.fromDegrees(5))); - trajectoryConfig = new TrajectoryConfig(0.25, 0.25); + finePathController.setTolerance(new Pose2d(0.02, 0.02, Rotation2d.fromDegrees(5))); + trajectoryConfig = new TrajectoryConfig(0.5, 0.5); RobotConfig config = null; try { From 61520906a417e82ba70ffb31d4930f961d59c175 Mon Sep 17 00:00:00 2001 From: jamthepersonj Date: Wed, 5 Mar 2025 18:35:22 -0500 Subject: [PATCH 23/99] mapped controls --- src/main/java/frc/robot/RobotContainer.java | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 646d10dd..30f7c376 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -208,6 +208,7 @@ private void configureBindings() { JoystickButton joyLeft2 = new JoystickButton(joyleft, 2); JoystickButton joyRight1 = new JoystickButton(joyright, 1); + JoystickButton joyLeft8 = new JoystickButton(joyleft, 8); RobotSlide robotSlide = new RobotSlide(drivetrain, joyleft::getX, joyleft::getY); joyLeft2.whileTrue(robotSlide); @@ -246,6 +247,7 @@ private void configureBindings() { .back() .onTrue(new CancelAll(byebyeTilt, byebyeRoller, elevatorSubsystem, hihiExtender)); joyRight1.onTrue(new ShootCoral(coralSubsystem, Constants.CORAL_SHOOTER_SPEED)); + joyLeft8.onTrue(new AlignClosestBranch(drivetrain)); // climber on Right Trigger if (Constants.COMMAND_DEBUG) { SmartDashboard.putData("Roll Algae", new RollAlgae(hihiRoller, 0.5)); From 1365552dd32f6a22e4ae89d84225a47277a7ef36 Mon Sep 17 00:00:00 2001 From: Michael Kovalev <134699174+michaelk036@users.noreply.github.com> Date: Sun, 16 Mar 2025 16:29:08 -0400 Subject: [PATCH 24/99] fixed branch / robot align coordinates + angle with data from the google sheet --- .../robot/constants/AlignmentPositions.java | 28 ++++++------- .../frc/robot/constants/BranchPositions.java | 39 ++++++++++--------- 2 files changed, 32 insertions(+), 35 deletions(-) diff --git a/src/main/java/frc/robot/constants/AlignmentPositions.java b/src/main/java/frc/robot/constants/AlignmentPositions.java index 6a2e52a2..3ded1c50 100644 --- a/src/main/java/frc/robot/constants/AlignmentPositions.java +++ b/src/main/java/frc/robot/constants/AlignmentPositions.java @@ -6,22 +6,18 @@ import java.util.List; public enum AlignmentPositions { - BRANCH_A(new Pose2d(3.18849375, 4.0739863148, Rotation2d.fromDegrees(0))), - BRANCH_B(new Pose2d(3.18849375, 3.7238597608, Rotation2d.fromDegrees(0))), - BRANCH_C(new Pose2d(3.79133815262887167703, 2.93370262638828539811, Rotation2d.fromDegrees(60))), - BRANCH_D(new Pose2d(4.09455664293237573925, 2.75863934938828539811, Rotation2d.fromDegrees(60))), - BRANCH_E(new Pose2d(5.08027499562887167703, 2.88563934938828539811, Rotation2d.fromDegrees(120))), - BRANCH_F(new Pose2d(5.38349348593237573925, 3.06070262638828539811, Rotation2d.fromDegrees(120))), - BRANCH_G(new Pose2d(5.766367436, 3.9778597608, Rotation2d.fromDegrees(180))), - BRANCH_H(new Pose2d(5.766367436, 4.3279863148, Rotation2d.fromDegrees(180))), - BRANCH_I(new Pose2d(5.16352303337112832297, 5.11814344921171460189, Rotation2d.fromDegrees(240))), - BRANCH_J(new Pose2d(4.86030454306762426075, 5.29320672621171460189, Rotation2d.fromDegrees(240))), - BRANCH_K(new Pose2d(3.87458619037112832297, 5.16620672621171460189, Rotation2d.fromDegrees(300))), - BRANCH_L( - new Pose2d( - 3.57136770006762426075, - 4.99114344921171460189, - Rotation2d.fromDegrees(300))); // TODO: ALL PLACEHOLDERS + BRANCH_A(new Pose2d(-0.3424, 4.068154, Rotation2d.fromRadians(3.141592654))), + BRANCH_B(new Pose2d(-0.3424, 3.739154, Rotation2d.fromRadians(3.141592654))), + BRANCH_C(new Pose2d(2.021430073, -0.09755978616, Rotation2d.fromRadians(4.188790205))), + BRANCH_D(new Pose2d(2.30635243, -0.2620597862, Rotation2d.fromRadians(4.188790205))), + BRANCH_E(new Pose2d(6.810959073, -0.1332797862, Rotation2d.fromRadians(5.235987756))), + BRANCH_F(new Pose2d(7.09588143, 0.03122021384, Rotation2d.fromRadians(5.235987756))), + BRANCH_G(new Pose2d(9.236658, 3.996714, Rotation2d.fromRadians(6.283185307))), + BRANCH_H(new Pose2d(9.236658, 4.325714, Rotation2d.fromRadians(6.283185307))), + BRANCH_I(new Pose2d(6.872827927, 8.162427786, Rotation2d.fromRadians(7.330382858))), + BRANCH_J(new Pose2d(6.58790557, 8.326927786, Rotation2d.fromRadians(7.330382858))), + BRANCH_K(new Pose2d(2.083298927, 8.198147786, Rotation2d.fromRadians(8.37758041))), + BRANCH_L(new Pose2d(1.79837657, 8.033647786, Rotation2d.fromRadians(8.37758041))); private final Pose2d position; diff --git a/src/main/java/frc/robot/constants/BranchPositions.java b/src/main/java/frc/robot/constants/BranchPositions.java index 5a59d1eb..6b76ef7b 100644 --- a/src/main/java/frc/robot/constants/BranchPositions.java +++ b/src/main/java/frc/robot/constants/BranchPositions.java @@ -5,25 +5,26 @@ import edu.wpi.first.math.geometry.Rotation2d; public enum BranchPositions { - CENTER(new Pose2d(4.477430593, 4.0259230378, Rotation2d.fromDegrees(0.0))), - CENTER_A_B(new Pose2d(3.64569375, 4.0259230378, Rotation2d.fromDegrees(0))), - CENTER_C_D(new Pose2d(4.0615621715, 3.30561780249853074741, Rotation2d.fromDegrees(60))), - CENTER_E_F(new Pose2d(4.8932990145, 3.30561780249853074741, Rotation2d.fromDegrees(120))), - CENTER_G_H(new Pose2d(5.309167436, 4.0259230378, Rotation2d.fromDegrees(180))), - CENTER_I_J(new Pose2d(4.8932990145, 4.74622827310146925259, Rotation2d.fromDegrees(240))), - CENTER_K_L(new Pose2d(4.0615621715, 4.74622827310146925259, Rotation2d.fromDegrees(300))), - BRANCH_A(new Pose2d(3.64569375, 4.2009863148, Rotation2d.fromDegrees(0))), - BRANCH_B(new Pose2d(3.64569375, 3.8508597608, Rotation2d.fromDegrees(0))), - BRANCH_C(new Pose2d(3.90995292634824796889, 3.39314944099853074741, Rotation2d.fromDegrees(60))), - BRANCH_D(new Pose2d(4.21317141665175203111, 3.21808616399853074741, Rotation2d.fromDegrees(60))), - BRANCH_E(new Pose2d(4.74168976934824796889, 3.21808616399853074741, Rotation2d.fromDegrees(120))), - BRANCH_F(new Pose2d(5.04490825965175203111, 3.39314944099853074741, Rotation2d.fromDegrees(120))), - BRANCH_G(new Pose2d(5.309167436, 3.8508597608, Rotation2d.fromDegrees(180))), - BRANCH_H(new Pose2d(5.309167436, 4.2009863148, Rotation2d.fromDegrees(180))), - BRANCH_I(new Pose2d(5.04490825965175203111, 4.65869663460146925259, Rotation2d.fromDegrees(240))), - BRANCH_J(new Pose2d(4.74168976934824796889, 4.83375991160146925259, Rotation2d.fromDegrees(240))), - BRANCH_K(new Pose2d(4.21317141665175203111, 4.83375991160146925259, Rotation2d.fromDegrees(300))), - BRANCH_L(new Pose2d(3.90995292634824796889, 4.65869663460146925259, Rotation2d.fromDegrees(300))); + CENTER(new Pose2d(0, 0, Rotation2d.fromRadians(0))), //TODO: add number for center of the reef + CENTER_A_B(new Pose2d(3.6576, 4.032434, Rotation2d.fromRadians(3.141592654))), + CENTER_C_D(new Pose2d(4.0523645, 3.348681829, Rotation2d.fromRadians(4.188790205))), + CENTER_E_F(new Pose2d(4.8418935, 3.348681829, Rotation2d.fromRadians(5.235987756))), + CENTER_G_H(new Pose2d(5.236658, 4.032434, Rotation2d.fromRadians(6.283185307))), + CENTER_I_J(new Pose2d(4.8418935, 4.716186171, Rotation2d.fromRadians(7.330382858))), + CENTER_K_L(new Pose2d(4.0523645, 4.716186171, Rotation2d.fromRadians(8.37758041))), + + BRANCH_A(new Pose2d(3.6576, 4.196934, Rotation2d.fromRadians(3.141592654))), + BRANCH_B(new Pose2d(3.6576, 3.867934, Rotation2d.fromRadians(3.141592654))), + BRANCH_C(new Pose2d(3.909903321, 3.430931829, Rotation2d.fromRadians(4.188790205))), + BRANCH_D(new Pose2d(4.194825679, 3.266431829, Rotation2d.fromRadians(4.188790205))), + BRANCH_E(new Pose2d(4.699432321, 3.266431829, Rotation2d.fromRadians(5.235987756))), + BRANCH_F(new Pose2d(4.984354679, 3.430931829, Rotation2d.fromRadians(5.235987756))), + BRANCH_G(new Pose2d(5.236658, 3.867934, Rotation2d.fromRadians(6.283185307))), + BRANCH_H(new Pose2d(5.236658, 4.196934, Rotation2d.fromRadians(6.283185307))), + BRANCH_I(new Pose2d(4.984354679, 4.633936171, Rotation2d.fromRadians(7.330382858))), + BRANCH_J(new Pose2d(4.699432321, 4.798436171, Rotation2d.fromRadians(7.330382858))), + BRANCH_K(new Pose2d(4.194825679, 4.798436171, Rotation2d.fromRadians(8.37758041))), + BRANCH_L(new Pose2d(3.909903321, 4.633936171, Rotation2d.fromRadians(8.37758041))); private final Pose2d position; From 41a9652339e9a7be80a12fd3f0b490a9f276d32c Mon Sep 17 00:00:00 2001 From: codetoad Date: Mon, 17 Mar 2025 19:59:55 -0400 Subject: [PATCH 25/99] removed autobuilder config duplicate --- .../subsystems/swervev3/SwerveDrivetrain.java | 37 ------------------- 1 file changed, 37 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java index fcbf9063..451fa67e 100644 --- a/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java @@ -84,43 +84,6 @@ public SwerveDrivetrain( frontLeft, frontRight, backLeft, backRight, apriltagIO, kinematics, getLastGyro()); finePathController.setTolerance(new Pose2d(0.02, 0.02, Rotation2d.fromDegrees(5))); trajectoryConfig = new TrajectoryConfig(0.5, 0.5); - - RobotConfig config = null; - try { - config = RobotConfig.fromGUISettings(); - } catch (Exception e) { - // Handle exception as needed - e.printStackTrace(); - } - - AutoBuilder.configure( - this::getPose, - this::resetOdometry, - this::getChassisSpeeds, - this::drive, - new PPHolonomicDriveController( - new PIDConstants( - Constants.PATH_PLANNER_TRANSLATION_PID_P, - Constants.PATH_PLANNER_TRANSLATION_PID_I, - Constants.PATH_PLANNER_TRANSLATION_PID_D), // Translation PID constants - new PIDConstants( - Constants.PATH_PLANNER_ROTATION_PID_P, - Constants.PATH_PLANNER_ROTATION_PID_I, - Constants.PATH_PLANNER_ROTATION_PID_D) // Rotation PID constants - ), - config, - () -> { - // Boolean supplier that controls when the path will be mirrored for the red alliance - // This will flip the path being followed to the red side of the field. - // THE ORIGIN WILL REMAIN ON THE BLUE SIDE - - var alliance = DriverStation.getAlliance(); - if (alliance.isPresent()) { - return alliance.get() == DriverStation.Alliance.Red; - } - return false; - }, - this); } @Override From 5a2d08926dd09d053f4c7955580652305bd7bad1 Mon Sep 17 00:00:00 2001 From: codetoad Date: Mon, 17 Mar 2025 20:01:45 -0400 Subject: [PATCH 26/99] removed null pointer exception --- src/main/java/frc/robot/RobotContainer.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 0287b50e..f0f05820 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -400,7 +400,7 @@ private void setupPathPlanning() { Constants.PATH_PLANNER_ROTATION_PID_D) // Rotation PID constants ), PathPlannerUtils.config, - () -> Robot.getAllianceColor().orElse(null) == Alliance.Red, + () -> Robot.getAllianceColor().orElse(Alliance.Blue) == Alliance.Red, drivetrain); } From 6070d88490831c3efc6faaabea6cd8492ca7d9ff Mon Sep 17 00:00:00 2001 From: codetoad Date: Tue, 18 Mar 2025 10:43:20 -0400 Subject: [PATCH 27/99] added apriltag focus mode for auto aligning --- src/main/java/frc/robot/Robot.java | 30 ++--- src/main/java/frc/robot/RobotContainer.java | 5 +- .../alignment/AlignClosestBranch.java | 22 ++-- .../robot/constants/AlignmentPosition.java | 114 ++++++++++++++++++ .../robot/constants/AlignmentPositions.java | 37 ------ .../frc/robot/constants/BranchPositions.java | 6 +- .../frc/robot/constants/GameConstants.java | 7 +- .../elevator/ElevatorSimulator.java | 30 +++-- .../subsystems/elevator/SimElevatorIO.java | 3 +- .../subsystems/swervev3/SwerveDrivetrain.java | 17 ++- .../subsystems/swervev3/io/SwerveModule.java | 10 +- .../robot/utils/simulation/ArmSimulator.java | 3 +- .../utils/simulation/RobotVisualizer.java | 36 +++--- 13 files changed, 214 insertions(+), 106 deletions(-) create mode 100644 src/main/java/frc/robot/constants/AlignmentPosition.java delete mode 100644 src/main/java/frc/robot/constants/AlignmentPositions.java diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 60093daa..2cf7d410 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -91,29 +91,29 @@ public static RobotMode getMode() { return mode.get(); } - @Override - public void robotPeriodic() { - if (getMode() != RobotMode.TEST) { - CommandScheduler.getInstance().run(); - if (DriverStation.isDSAttached() && allianceColor.isEmpty()) { + @Override + public void robotPeriodic() { + if (getMode() != RobotMode.TEST) { + CommandScheduler.getInstance().run(); + if (DriverStation.isDSAttached() && allianceColor.isEmpty()) { allianceColor = DriverStation.getAlliance(); if (allianceColor.isPresent()) { robotContainer.getAutoChooser().getProvider().forceRefresh(); } } if (counter == 0) { - actualInit(); - } - if (Constants.currentMode.equals(GameConstants.Mode.SIM)) { - robotContainer.getRobotVisualizer().logMechanism(); - } - counter++; - } + actualInit(); + } + if (Constants.currentMode.equals(GameConstants.Mode.SIM)) { + robotContainer.getRobotVisualizer().logMechanism(); + } + counter++; + } - if (Constants.ENABLE_LOGGING) { - CommandLogger.get().log(); - } + if (Constants.ENABLE_LOGGING) { + CommandLogger.get().log(); } + } /** Use this instead of robot init. */ private void actualInit() { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index f0f05820..223bce3f 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -11,8 +11,8 @@ import com.studica.frc.AHRS; import com.studica.frc.AHRS.NavXComType; import edu.wpi.first.math.trajectory.TrapezoidProfile; -import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; @@ -28,7 +28,6 @@ import frc.robot.commands.alignment.AlignClosestBranch; import frc.robot.commands.byebye.ByeByeToFwrLimit; import frc.robot.commands.byebye.ByeByeToRevLimit; -import frc.robot.commands.byebye.SpinByeByeRoller; import frc.robot.commands.coral.IntakeCoral; import frc.robot.commands.coral.ShootCoral; import frc.robot.commands.drivetrain.Drive; @@ -98,8 +97,6 @@ import frc.robot.utils.motor.PID; import frc.robot.utils.shuffleboard.SmartShuffleboard; import frc.robot.utils.simulation.RobotVisualizer; -import java.util.Optional; -import frc.robot.utils.shuffleboard.SmartShuffleboard; public class RobotContainer { private AutoChooser2025 autoChooser; diff --git a/src/main/java/frc/robot/commands/alignment/AlignClosestBranch.java b/src/main/java/frc/robot/commands/alignment/AlignClosestBranch.java index aeec2b03..2d21e13e 100644 --- a/src/main/java/frc/robot/commands/alignment/AlignClosestBranch.java +++ b/src/main/java/frc/robot/commands/alignment/AlignClosestBranch.java @@ -1,16 +1,16 @@ package frc.robot.commands.alignment; -import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.CommandScheduler; -import frc.robot.constants.AlignmentPositions; +import frc.robot.constants.AlignmentPosition; +import frc.robot.constants.Constants; import frc.robot.subsystems.swervev3.SwerveDrivetrain; import frc.robot.utils.logging.commands.LoggableCommand; import frc.robot.utils.logging.commands.LoggableCommandWrapper; import org.littletonrobotics.junction.Logger; public class AlignClosestBranch extends LoggableCommand { - private Pose2d targetPosition; + private AlignmentPosition alignmentTarget; private final SwerveDrivetrain drivetrain; private LoggableCommand followTrajectory; private final Timer timer = new Timer(); @@ -23,10 +23,15 @@ public AlignClosestBranch(SwerveDrivetrain drivetrain) { @Override public void initialize() { timer.restart(); - targetPosition = AlignmentPositions.getClosest(drivetrain.getPose()); - Logger.recordOutput("TargetReefPose", targetPosition); - followTrajectory = - LoggableCommandWrapper.wrap(drivetrain.generateTrajectoryCommand(targetPosition)); + alignmentTarget = AlignmentPosition.getClosest(drivetrain.getPose().getTranslation()); + Logger.recordOutput("TargetReefAlignment", alignmentTarget); + Logger.recordOutput("TargetReefPose", alignmentTarget.getPosition()); + drivetrain.setFocusedApriltag(alignmentTarget.getTag()); + // spotless:off (shhhh don't tell anyone about the linter bypass) + followTrajectory = LoggableCommandWrapper.wrap( + drivetrain.generateTrajectoryCommand(alignmentTarget.getPosition()) + ).withBasicName("FollowAlignToBranchTrajectory"); + // spotless:on followTrajectory.setParent(this); followTrajectory.schedule(); } @@ -36,10 +41,11 @@ public void end(boolean interrupted) { if (CommandScheduler.getInstance().isScheduled(followTrajectory)) { CommandScheduler.getInstance().cancel(followTrajectory); } + drivetrain.exitFocusMode(); } @Override public boolean isFinished() { - return followTrajectory.isFinished() | timer.hasElapsed(10); + return followTrajectory.isFinished() || timer.hasElapsed(Constants.AUTO_ALIGN_TIMEOUT); } } diff --git a/src/main/java/frc/robot/constants/AlignmentPosition.java b/src/main/java/frc/robot/constants/AlignmentPosition.java new file mode 100644 index 00000000..07ed7365 --- /dev/null +++ b/src/main/java/frc/robot/constants/AlignmentPosition.java @@ -0,0 +1,114 @@ +package frc.robot.constants; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj.DriverStation; +import frc.robot.Robot; +import frc.robot.utils.Apriltag; +import java.util.Optional; + +public enum AlignmentPosition { + BRANCH_A( + new Pose2d(-0.3424, 4.068154, Rotation2d.fromRadians(3.141592654)), + Apriltag.EIGHTEEN, + Apriltag.SEVEN + ), + BRANCH_B( + new Pose2d(-0.3424, 3.739154, Rotation2d.fromRadians(3.141592654)), + Apriltag.EIGHTEEN, + Apriltag.SEVEN + ), + BRANCH_C( + new Pose2d(2.021430073, -0.09755978616, Rotation2d.fromRadians(4.188790205)), + Apriltag.SEVENTEEN, + Apriltag.SIX + ), + BRANCH_D( + new Pose2d(2.30635243, -0.2620597862, Rotation2d.fromRadians(4.188790205)), + Apriltag.SEVENTEEN, + Apriltag.SIX + ), + BRANCH_E( + new Pose2d(6.810959073, -0.1332797862, Rotation2d.fromRadians(5.235987756)), + Apriltag.TWENTY_TWO, + Apriltag.ELEVEN + ), + BRANCH_F( + new Pose2d(7.09588143, 0.03122021384, Rotation2d.fromRadians(5.235987756)), + Apriltag.TWENTY_TWO, + Apriltag.ELEVEN + ), + BRANCH_G( + new Pose2d(9.236658, 3.996714, Rotation2d.fromRadians(6.283185307)), + Apriltag.TWENTY_ONE, + Apriltag.TEN + ), + BRANCH_H( + new Pose2d(9.236658, 4.325714, Rotation2d.fromRadians(6.283185307)), + Apriltag.TWENTY_ONE, + Apriltag.TEN + ), + BRANCH_I( + new Pose2d(6.872827927, 8.162427786, Rotation2d.fromRadians(7.330382858)), + Apriltag.TWENTY, + Apriltag.NINE + ), + BRANCH_J( + new Pose2d(6.58790557, 8.326927786, Rotation2d.fromRadians(7.330382858)), + Apriltag.TWENTY, + Apriltag.NINE + ), + BRANCH_K( + new Pose2d(2.083298927, 8.198147786, Rotation2d.fromRadians(8.37758041)), + Apriltag.NINETEEN, + Apriltag.EIGHT + ), + BRANCH_L( + new Pose2d(1.79837657, 8.033647786, Rotation2d.fromRadians(8.37758041)), + Apriltag.NINETEEN, + Apriltag.EIGHT + ); + + private final Pose2d position; + private final Apriltag blueTag; + private final Apriltag redTag; + + AlignmentPosition(Pose2d position, Apriltag blueTag, Apriltag redTag) { + this.position = position; + this.blueTag = blueTag; + this.redTag = redTag; + } + + public Pose2d getPosition() { + return position; + } + + public static AlignmentPosition getClosest(Translation2d currentPosition) { + int closestIndex = 0; + double closestDistance = values()[0].position.getTranslation().getDistance(currentPosition); + for (int i = 1; i < values().length; i++) { + double distance = values()[i].position.getTranslation().getDistance(currentPosition); + if (distance < closestDistance) { + closestDistance = distance; + closestIndex = i; + } + } + return values()[closestIndex]; + } + + public Apriltag getBlueTag() { + return blueTag; + } + + public Apriltag getRedTag() { + return redTag; + } + + public Apriltag getTag() { + Optional allianceColor = Robot.getAllianceColor(); + return allianceColor + .map(alliance -> alliance == DriverStation.Alliance.Blue ? getBlueTag() : getRedTag()) + .orElse(null); + } +} diff --git a/src/main/java/frc/robot/constants/AlignmentPositions.java b/src/main/java/frc/robot/constants/AlignmentPositions.java deleted file mode 100644 index 3ded1c50..00000000 --- a/src/main/java/frc/robot/constants/AlignmentPositions.java +++ /dev/null @@ -1,37 +0,0 @@ -package frc.robot.constants; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import java.util.Arrays; -import java.util.List; - -public enum AlignmentPositions { - BRANCH_A(new Pose2d(-0.3424, 4.068154, Rotation2d.fromRadians(3.141592654))), - BRANCH_B(new Pose2d(-0.3424, 3.739154, Rotation2d.fromRadians(3.141592654))), - BRANCH_C(new Pose2d(2.021430073, -0.09755978616, Rotation2d.fromRadians(4.188790205))), - BRANCH_D(new Pose2d(2.30635243, -0.2620597862, Rotation2d.fromRadians(4.188790205))), - BRANCH_E(new Pose2d(6.810959073, -0.1332797862, Rotation2d.fromRadians(5.235987756))), - BRANCH_F(new Pose2d(7.09588143, 0.03122021384, Rotation2d.fromRadians(5.235987756))), - BRANCH_G(new Pose2d(9.236658, 3.996714, Rotation2d.fromRadians(6.283185307))), - BRANCH_H(new Pose2d(9.236658, 4.325714, Rotation2d.fromRadians(6.283185307))), - BRANCH_I(new Pose2d(6.872827927, 8.162427786, Rotation2d.fromRadians(7.330382858))), - BRANCH_J(new Pose2d(6.58790557, 8.326927786, Rotation2d.fromRadians(7.330382858))), - BRANCH_K(new Pose2d(2.083298927, 8.198147786, Rotation2d.fromRadians(8.37758041))), - BRANCH_L(new Pose2d(1.79837657, 8.033647786, Rotation2d.fromRadians(8.37758041))); - - private final Pose2d position; - - AlignmentPositions(Pose2d position) { - this.position = position; - } - - public Pose2d getPosition() { - return position; - } - - public static Pose2d getClosest(Pose2d currentPosition) { - List poseList = - Arrays.stream(AlignmentPositions.values()).map(AlignmentPositions::getPosition).toList(); - return currentPosition.nearest(poseList); - } -} diff --git a/src/main/java/frc/robot/constants/BranchPositions.java b/src/main/java/frc/robot/constants/BranchPositions.java index 6b76ef7b..53906dfd 100644 --- a/src/main/java/frc/robot/constants/BranchPositions.java +++ b/src/main/java/frc/robot/constants/BranchPositions.java @@ -5,10 +5,10 @@ import edu.wpi.first.math.geometry.Rotation2d; public enum BranchPositions { - CENTER(new Pose2d(0, 0, Rotation2d.fromRadians(0))), //TODO: add number for center of the reef + CENTER(new Pose2d(0, 0, Rotation2d.fromRadians(0))), // TODO: add number for center of the reef CENTER_A_B(new Pose2d(3.6576, 4.032434, Rotation2d.fromRadians(3.141592654))), - CENTER_C_D(new Pose2d(4.0523645, 3.348681829, Rotation2d.fromRadians(4.188790205))), - CENTER_E_F(new Pose2d(4.8418935, 3.348681829, Rotation2d.fromRadians(5.235987756))), + CENTER_C_D(new Pose2d(4.0523645, 3.348681829, Rotation2d.fromRadians(4.188790205))), + CENTER_E_F(new Pose2d(4.8418935, 3.348681829, Rotation2d.fromRadians(5.235987756))), CENTER_G_H(new Pose2d(5.236658, 4.032434, Rotation2d.fromRadians(6.283185307))), CENTER_I_J(new Pose2d(4.8418935, 4.716186171, Rotation2d.fromRadians(7.330382858))), CENTER_K_L(new Pose2d(4.0523645, 4.716186171, Rotation2d.fromRadians(8.37758041))), diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java index 2edb3de2..340535ea 100644 --- a/src/main/java/frc/robot/constants/GameConstants.java +++ b/src/main/java/frc/robot/constants/GameConstants.java @@ -59,6 +59,7 @@ public class GameConstants { public static final double MAX_PATHPLANNER_ANGULAR_ACCEL = 3797; // Timeouts + public static final int AUTO_ALIGN_TIMEOUT = 10; public static final int SERVER_SOCKET_CONNECTION_TIMEOUT = 2000; public static final int ELEVATOR_TIMEOUT = 10; public static final int ELEVATOR_RESET_TIMEOUT = 10; @@ -166,8 +167,10 @@ public enum Mode { // Angles public static final Rotation2d HIHI_MIN_ANGLE = Rotation2d.fromDegrees(0); public static final Rotation2d HIHI_MAX_ANGLE = Rotation2d.fromDegrees(90); - public static final Rotation2d BYEBYE_TILT_MIN_ANGLE = Rotation2d.fromDegrees(45); // TODO: change later - public static final Rotation2d BYEBYE_TILT_MAX_ANGLE = Rotation2d.fromDegrees(90); // TODO: change later + public static final Rotation2d BYEBYE_TILT_MIN_ANGLE = + Rotation2d.fromDegrees(45); // TODO: change later + public static final Rotation2d BYEBYE_TILT_MAX_ANGLE = + Rotation2d.fromDegrees(90); // TODO: change later public static final double BYEBYE_TILT_INERTIA = 0.5; // TODO: change later public static final double BYEBYE_TILT_GEARING = 45.0; // TODO: change later public static final boolean BYEBYE_TILT_SIMULATE_GRAVITY = true; diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorSimulator.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorSimulator.java index 532f0a4d..7f993457 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorSimulator.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorSimulator.java @@ -61,7 +61,9 @@ public class ElevatorSimulator { public ElevatorSimulator(SparkMax motor, LoggedMechanismLigament2d elevatorLigament) { this(motor, elevatorLigament, false); } - public ElevatorSimulator(SparkMax motor, LoggedMechanismLigament2d elevatorLigament, boolean inverted) { + + public ElevatorSimulator( + SparkMax motor, LoggedMechanismLigament2d elevatorLigament, boolean inverted) { this.motor = motor; motorSim = new SparkMaxSim(motor, elevatorGearbox); this.elevatorLigament = elevatorLigament; @@ -76,7 +78,8 @@ public void stepSimulation() { final double direction = inverted ? -1.0 : 1.0; // In this method, we update our simulation of what our elevator is doing // First, we set our "inputs" (voltages) - double motorOut = direction * motorSim.getAppliedOutput() * 12.0; // * RoboRioSim.getVInVoltage(); + double motorOut = + direction * motorSim.getAppliedOutput() * 12.0; // * RoboRioSim.getVInVoltage(); m_elevatorSim.setInput(motorOut); // Next, we update it. The standard loop time is 20ms. m_elevatorSim.update(0.020); @@ -84,27 +87,34 @@ public void stepSimulation() { // Finally, we set our simulated encoder's readings and simulated battery voltage double velocityMetersPerSecond = m_elevatorSim.getVelocityMetersPerSecond(); double rpm = convertDistanceToRotations(Meters.of(velocityMetersPerSecond)).per(Second).in(RPM); - motorSim.iterate(direction * rpm, 12,0.020); + motorSim.iterate(direction * rpm, 12, 0.020); // SimBattery estimates loaded battery voltages - RoboRioSim.setVInVoltage(BatterySim.calculateDefaultBatteryLoadedVoltage(m_elevatorSim.getCurrentDrawAmps())); + RoboRioSim.setVInVoltage( + BatterySim.calculateDefaultBatteryLoadedVoltage(m_elevatorSim.getCurrentDrawAmps())); // Update elevator visualization with position double positionMeters = m_elevatorSim.getPositionMeters(); if (inverted) { - forwardSwitchSim.setPressed(MathUtil.isNear(Constants.MIN_ELEVATOR_HEIGHT_METERS, positionMeters, 0.1)); - reverseSwitchSim.setPressed(MathUtil.isNear(Constants.MAX_ELEVATOR_HEIGHT_METERS, positionMeters, 0.1)); + forwardSwitchSim.setPressed( + MathUtil.isNear(Constants.MIN_ELEVATOR_HEIGHT_METERS, positionMeters, 0.1)); + reverseSwitchSim.setPressed( + MathUtil.isNear(Constants.MAX_ELEVATOR_HEIGHT_METERS, positionMeters, 0.1)); } else { - forwardSwitchSim.setPressed(MathUtil.isNear(Constants.MAX_ELEVATOR_HEIGHT_METERS, positionMeters, 0.1)); - reverseSwitchSim.setPressed(MathUtil.isNear(Constants.MIN_ELEVATOR_HEIGHT_METERS, positionMeters, 0.1)); + forwardSwitchSim.setPressed( + MathUtil.isNear(Constants.MAX_ELEVATOR_HEIGHT_METERS, positionMeters, 0.1)); + reverseSwitchSim.setPressed( + MathUtil.isNear(Constants.MIN_ELEVATOR_HEIGHT_METERS, positionMeters, 0.1)); } if (Constants.ELEVATOR_DEBUG) { Logger.recordOutput("ElevatorSubsystem/MotorCommandedVoltage", motorOut); Logger.recordOutput("ElevatorSubsystem/VelocityMPS", velocityMetersPerSecond); - Logger.recordOutput("ElevatorSubsystem/ElevatorActualPosition", m_elevatorSim.getPositionMeters()); + Logger.recordOutput( + "ElevatorSubsystem/ElevatorActualPosition", m_elevatorSim.getPositionMeters()); if (elevatorLigament != null) { - Logger.recordOutput("ElevatorSubsystem/ElevatorMechanismLength", elevatorLigament.getLength()); + Logger.recordOutput( + "ElevatorSubsystem/ElevatorMechanismLength", elevatorLigament.getLength()); } } diff --git a/src/main/java/frc/robot/subsystems/elevator/SimElevatorIO.java b/src/main/java/frc/robot/subsystems/elevator/SimElevatorIO.java index 7b5136cd..5459fbab 100644 --- a/src/main/java/frc/robot/subsystems/elevator/SimElevatorIO.java +++ b/src/main/java/frc/robot/subsystems/elevator/SimElevatorIO.java @@ -9,7 +9,8 @@ public class SimElevatorIO extends RealElevatorIO { public SimElevatorIO(LoggedMechanismLigament2d elevatorLigament) { super(); - this.elevatorSimulator = new ElevatorSimulator(elevatorMotor.getNeoMotor(), elevatorLigament, true); + this.elevatorSimulator = + new ElevatorSimulator(elevatorMotor.getNeoMotor(), elevatorLigament, true); } @Override diff --git a/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java index 451fa67e..9f076780 100644 --- a/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java @@ -1,9 +1,5 @@ package frc.robot.subsystems.swervev3; -import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.config.PIDConstants; -import com.pathplanner.lib.config.RobotConfig; -import com.pathplanner.lib.controllers.PPHolonomicDriveController; import edu.wpi.first.math.Vector; import edu.wpi.first.math.controller.HolonomicDriveController; import edu.wpi.first.math.controller.PIDController; @@ -20,7 +16,6 @@ import edu.wpi.first.math.trajectory.TrajectoryConfig; import edu.wpi.first.math.trajectory.TrajectoryGenerator; import edu.wpi.first.math.trajectory.TrapezoidProfile; -import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.SwerveControllerCommand; @@ -32,6 +27,7 @@ import frc.robot.subsystems.swervev3.estimation.PoseEstimator; import frc.robot.subsystems.swervev3.io.SwerveModule; import frc.robot.subsystems.swervev3.vision.DistanceVisionTruster; +import frc.robot.utils.Apriltag; import frc.robot.utils.DriveMode; import frc.robot.utils.logging.LoggableIO; import frc.robot.utils.logging.subsystem.LoggableSystem; @@ -59,6 +55,8 @@ public class SwerveDrivetrain extends SubsystemBase { private final PoseEstimator poseEstimator; private boolean facingTarget = false; private TrajectoryConfig trajectoryConfig; + private boolean focusTagMade = false; + private Apriltag focusedApriltag = Apriltag.ONE; // controller will add an additional meter per second in the x direction for every meter of error // in the x direction private final HolonomicDriveController finePathController = @@ -245,4 +243,13 @@ public Command generateTrajectoryCommand(Pose2d targetPose) { this::setModuleStates, this); } + + public void setFocusedApriltag(Apriltag tagToFocus) { + focusedApriltag = tagToFocus; + focusTagMade = true; + } + + public void exitFocusMode() { + focusTagMade = false; + } } diff --git a/src/main/java/frc/robot/subsystems/swervev3/io/SwerveModule.java b/src/main/java/frc/robot/subsystems/swervev3/io/SwerveModule.java index dc563b46..f59f49ac 100644 --- a/src/main/java/frc/robot/subsystems/swervev3/io/SwerveModule.java +++ b/src/main/java/frc/robot/subsystems/swervev3/io/SwerveModule.java @@ -46,9 +46,15 @@ public SwerveModule( TrapezoidProfile.Constraints goalConstraint, String moduleName) { MotorInputs driveInputs = - new MotorInputBuilder<>("Drivetrain/" + moduleName + "/Drive").addEncoder().motorCurrent().build(); + new MotorInputBuilder<>("Drivetrain/" + moduleName + "/Drive") + .addEncoder() + .motorCurrent() + .build(); MotorInputs steerInputs = - new MotorInputBuilder<>("Drivetrain/" + moduleName + "/Steer").addEncoder().motorCurrent().build(); + new MotorInputBuilder<>("Drivetrain/" + moduleName + "/Steer") + .addEncoder() + .motorCurrent() + .build(); this.driveSystem = new LoggableSystem<>(driveMotorIO, driveInputs); this.steerSystem = new LoggableSystem<>(steerMotorIO, steerInputs); this.absSystem = new LoggableSystem<>(absIO, new SwerveAbsInput("Drivetrain/" + moduleName)); diff --git a/src/main/java/frc/robot/utils/simulation/ArmSimulator.java b/src/main/java/frc/robot/utils/simulation/ArmSimulator.java index 84b1032a..0356c88b 100644 --- a/src/main/java/frc/robot/utils/simulation/ArmSimulator.java +++ b/src/main/java/frc/robot/utils/simulation/ArmSimulator.java @@ -93,7 +93,8 @@ public void stepSimulation() { if (Constants.ARM_DEBUG) { SmartDashboard.putNumber(name + "/Arm Motor out voltage", motorOut); - SmartDashboard.putNumber(name + "/Arm Velocity rads per s", velocityRadsPerSecond.getRadians()); + SmartDashboard.putNumber( + name + "/Arm Velocity rads per s", velocityRadsPerSecond.getRadians()); SmartDashboard.putNumber(name + "/Arm RPM", rpm); SmartDashboard.putNumber(name + "/Arm actual position", armSim.getAngleRads()); SmartDashboard.putNumber(name + "/Arm Mechanism angle", armSim.getAngleRads()); diff --git a/src/main/java/frc/robot/utils/simulation/RobotVisualizer.java b/src/main/java/frc/robot/utils/simulation/RobotVisualizer.java index 41b668d9..f0f40d55 100644 --- a/src/main/java/frc/robot/utils/simulation/RobotVisualizer.java +++ b/src/main/java/frc/robot/utils/simulation/RobotVisualizer.java @@ -14,8 +14,8 @@ public class RobotVisualizer { private final LoggedMechanismLigament2d elevatorLigament; private final LoggedMechanismLigament2d algaeByeByeTiltLigament; private final LoggedMechanismLigament2d algaeByeByeRollerLigament; -// private final LoggedMechanismLigament2d algaeHiHiTiltLigament; -// private final LoggedMechanismLigament2d algaeHiHiRollerLigament; + // private final LoggedMechanismLigament2d algaeHiHiTiltLigament; + // private final LoggedMechanismLigament2d algaeHiHiRollerLigament; private final LoggedMechanismLigament2d coralRollerLigament; private static final RobotVisualizer instance = new RobotVisualizer(); @@ -28,8 +28,8 @@ public RobotVisualizer() { LoggedMechanismRoot2d elevatorRoot = mech2d.getRoot( "Elevator Root", Constants.DRIVE_BASE_WIDTH / 2, Constants.INITIAL_ELEVATOR_HEIGHT); -// LoggedMechanismRoot2d algaeHiHiRoot = -// mech2d.getRoot("Algae HiHi Root", Constants.DRIVE_BASE_WIDTH - 0.05, 0.1); + // LoggedMechanismRoot2d algaeHiHiRoot = + // mech2d.getRoot("Algae HiHi Root", Constants.DRIVE_BASE_WIDTH - 0.05, 0.1); this.elevatorLigament = elevatorRoot.append( new LoggedMechanismLigament2d( @@ -54,14 +54,14 @@ public RobotVisualizer() { this.algaeByeByeTiltLigament.append( new LoggedMechanismLigament2d( "AlgaeByeByeRoller", 0.05, 180, 5, new Color8Bit(Color.kGreen))); -// this.algaeHiHiTiltLigament = -// algaeHiHiRoot.append( -// new LoggedMechanismLigament2d( -// "AlgaeHiHiTilt", 0.2, -90, 4, new Color8Bit(Color.kBlue))); -// this.algaeHiHiRollerLigament = -// algaeHiHiTiltLigament.append( -// new LoggedMechanismLigament2d( -// "AlgaeHiHiRoller", 0.05, 0, 5, new Color8Bit(Color.kGreen))); + // this.algaeHiHiTiltLigament = + // algaeHiHiRoot.append( + // new LoggedMechanismLigament2d( + // "AlgaeHiHiTilt", 0.2, -90, 4, new Color8Bit(Color.kBlue))); + // this.algaeHiHiRollerLigament = + // algaeHiHiTiltLigament.append( + // new LoggedMechanismLigament2d( + // "AlgaeHiHiRoller", 0.05, 0, 5, new Color8Bit(Color.kGreen))); LoggedMechanismLigament2d coralRiser = byeByeRiser.append( new LoggedMechanismLigament2d( @@ -88,13 +88,13 @@ public LoggedMechanismLigament2d getAlgaeByeByeRollerLigament() { return algaeByeByeRollerLigament; } -// public LoggedMechanismLigament2d getAlgaeHiHiTiltLigament() { -// return algaeHiHiTiltLigament; -// } + // public LoggedMechanismLigament2d getAlgaeHiHiTiltLigament() { + // return algaeHiHiTiltLigament; + // } -// public LoggedMechanismLigament2d getAlgaeHiHiRollerLigament() { -// return algaeHiHiRollerLigament; -// } + // public LoggedMechanismLigament2d getAlgaeHiHiRollerLigament() { + // return algaeHiHiRollerLigament; + // } public LoggedMechanismLigament2d getCoralRollerLigament() { return coralRollerLigament; From 84cca6aee7b5743afa47e891d59c608e316181b7 Mon Sep 17 00:00:00 2001 From: codetoad Date: Wed, 19 Mar 2025 18:47:34 -0400 Subject: [PATCH 28/99] built code after merge --- src/main/java/frc/robot/RobotContainer.java | 1 + .../robot/constants/AlignmentPosition.java | 36 +++++++------------ .../subsystems/swervev3/SwerveDrivetrain.java | 8 ++--- 3 files changed, 15 insertions(+), 30 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 1279bd5a..8b1040b3 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -98,6 +98,7 @@ import frc.robot.utils.logging.LoggableIO; import frc.robot.utils.motor.Gain; import frc.robot.utils.motor.PID; +import frc.robot.utils.shuffleboard.SmartShuffleboard; import frc.robot.utils.simulation.RobotVisualizer; import frc.robot.utils.simulation.SwerveSimulationUtils; import java.util.function.Consumer; diff --git a/src/main/java/frc/robot/constants/AlignmentPosition.java b/src/main/java/frc/robot/constants/AlignmentPosition.java index 07ed7365..e691e005 100644 --- a/src/main/java/frc/robot/constants/AlignmentPosition.java +++ b/src/main/java/frc/robot/constants/AlignmentPosition.java @@ -12,63 +12,51 @@ public enum AlignmentPosition { BRANCH_A( new Pose2d(-0.3424, 4.068154, Rotation2d.fromRadians(3.141592654)), Apriltag.EIGHTEEN, - Apriltag.SEVEN - ), + Apriltag.SEVEN), BRANCH_B( new Pose2d(-0.3424, 3.739154, Rotation2d.fromRadians(3.141592654)), Apriltag.EIGHTEEN, - Apriltag.SEVEN - ), + Apriltag.SEVEN), BRANCH_C( new Pose2d(2.021430073, -0.09755978616, Rotation2d.fromRadians(4.188790205)), Apriltag.SEVENTEEN, - Apriltag.SIX - ), + Apriltag.SIX), BRANCH_D( new Pose2d(2.30635243, -0.2620597862, Rotation2d.fromRadians(4.188790205)), Apriltag.SEVENTEEN, - Apriltag.SIX - ), + Apriltag.SIX), BRANCH_E( new Pose2d(6.810959073, -0.1332797862, Rotation2d.fromRadians(5.235987756)), Apriltag.TWENTY_TWO, - Apriltag.ELEVEN - ), + Apriltag.ELEVEN), BRANCH_F( new Pose2d(7.09588143, 0.03122021384, Rotation2d.fromRadians(5.235987756)), Apriltag.TWENTY_TWO, - Apriltag.ELEVEN - ), + Apriltag.ELEVEN), BRANCH_G( new Pose2d(9.236658, 3.996714, Rotation2d.fromRadians(6.283185307)), Apriltag.TWENTY_ONE, - Apriltag.TEN - ), + Apriltag.TEN), BRANCH_H( new Pose2d(9.236658, 4.325714, Rotation2d.fromRadians(6.283185307)), Apriltag.TWENTY_ONE, - Apriltag.TEN - ), + Apriltag.TEN), BRANCH_I( new Pose2d(6.872827927, 8.162427786, Rotation2d.fromRadians(7.330382858)), Apriltag.TWENTY, - Apriltag.NINE - ), + Apriltag.NINE), BRANCH_J( new Pose2d(6.58790557, 8.326927786, Rotation2d.fromRadians(7.330382858)), Apriltag.TWENTY, - Apriltag.NINE - ), + Apriltag.NINE), BRANCH_K( new Pose2d(2.083298927, 8.198147786, Rotation2d.fromRadians(8.37758041)), Apriltag.NINETEEN, - Apriltag.EIGHT - ), + Apriltag.EIGHT), BRANCH_L( new Pose2d(1.79837657, 8.033647786, Rotation2d.fromRadians(8.37758041)), Apriltag.NINETEEN, - Apriltag.EIGHT - ); + Apriltag.EIGHT); private final Pose2d position; private final Apriltag blueTag; diff --git a/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java index 41d1bae4..066743e7 100644 --- a/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java @@ -2,10 +2,6 @@ import static edu.wpi.first.units.Units.*; -import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.config.PIDConstants; -import com.pathplanner.lib.config.RobotConfig; -import com.pathplanner.lib.controllers.PPHolonomicDriveController; import edu.wpi.first.math.Vector; import edu.wpi.first.math.controller.HolonomicDriveController; import edu.wpi.first.math.controller.PIDController; @@ -37,8 +33,8 @@ import frc.robot.utils.DriveMode; import frc.robot.utils.logging.LoggableIO; import frc.robot.utils.logging.subsystem.LoggableSystem; -import java.util.function.Consumer; import java.util.Collections; +import java.util.function.Consumer; import org.littletonrobotics.junction.Logger; public class SwerveDrivetrain extends SubsystemBase { @@ -89,7 +85,7 @@ public SwerveDrivetrain( this.poseEstimator = new PoseEstimator( frontLeft, frontRight, backLeft, backRight, apriltagIO, kinematics, getLastGyro()); - this.resetSimulationPoseCallBack = resetSimulationPoseCallBack; + this.resetSimulationPoseCallBack = resetSimulationPoseCallBack; finePathController.setTolerance(new Pose2d(0.02, 0.02, Rotation2d.fromDegrees(5))); trajectoryConfig = new TrajectoryConfig(0.5, 0.5); } From dce9593620f14c7521c01ad59640db61d6ca35fc Mon Sep 17 00:00:00 2001 From: codetoad Date: Wed, 19 Mar 2025 18:57:32 -0400 Subject: [PATCH 29/99] ignore other tags if in focus mode. --- .../subsystems/swervev3/SwerveDrivetrain.java | 6 ++- .../swervev3/estimation/PoseEstimator.java | 44 ++++++++++++++----- src/main/java/frc/robot/utils/Apriltag.java | 2 +- 3 files changed, 38 insertions(+), 14 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java index 066743e7..1db0c4d3 100644 --- a/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java @@ -105,7 +105,11 @@ public void periodic() { getLastGyro()); Logger.recordOutput("LastOdomModPoses", odom.modulePosition()); poseEstimator.updatePosition(odom); - poseEstimator.updateVision(); + if (focusTagMade) { + poseEstimator.updateVision(focusedApriltag); + } else { + poseEstimator.updateVision(); + } Logger.recordOutput( "realSwerveStates", frontLeft.getLatestState(), diff --git a/src/main/java/frc/robot/subsystems/swervev3/estimation/PoseEstimator.java b/src/main/java/frc/robot/subsystems/swervev3/estimation/PoseEstimator.java index b811a845..b7ee2989 100644 --- a/src/main/java/frc/robot/subsystems/swervev3/estimation/PoseEstimator.java +++ b/src/main/java/frc/robot/subsystems/swervev3/estimation/PoseEstimator.java @@ -20,10 +20,12 @@ import frc.robot.subsystems.swervev3.io.SwerveModule; import frc.robot.subsystems.swervev3.vision.BasicVisionFilter; import frc.robot.subsystems.swervev3.vision.SquareVisionTruster; +import frc.robot.utils.Apriltag; import frc.robot.utils.RobotMode; import frc.robot.utils.logging.LoggableIO; import frc.robot.utils.logging.subsystem.LoggableSystem; import frc.robot.utils.math.ArrayUtils; +import java.util.Arrays; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; @@ -114,11 +116,7 @@ private boolean validAprilTagPose(double[] measurement) { return !ArrayUtils.allMatch(measurement, -1.0) && measurement.length == 3; } - /** - * Collects Apriltag measurement(s) from the IO and checks their validity. If they are valid they - * are sent to the {@link PoseManager} for further processing - */ - public void updateVision() { + private void updateVision(int... invalidApriltagNumbers) { if (Constants.ENABLE_VISION && Robot.getMode() != RobotMode.DISABLED) { for (int i = 0; i < apriltagSystem.getInputs().timestamp.length; i++) { double[] pos = @@ -128,19 +126,15 @@ public void updateVision() { apriltagSystem.getInputs().poseYaw[i] }; if (validAprilTagPose(pos) + && !ArrayUtils.contains( + invalidApriltagNumbers, apriltagSystem.getInputs().apriltagNumber[i]) && apriltagSystem.getInputs().apriltagNumber[i] != 15 && apriltagSystem.getInputs().apriltagNumber[i] != 4 && apriltagSystem.getInputs().apriltagNumber[i] != 14 && apriltagSystem.getInputs().apriltagNumber[i] != 5 && apriltagSystem.getInputs().apriltagNumber[i] != 16 && apriltagSystem.getInputs().apriltagNumber[i] != 3) { - double serverTime = apriltagSystem.getInputs().serverTime[i]; - double timestamp = 0; // latency is not right we are assuming zero - double latencyInSec = (serverTime - timestamp) / 1000; - Pose2d visionPose = new Pose2d(pos[0], pos[1], getEstimatedPose().getRotation()); - double distanceFromTag = apriltagSystem.getInputs().distanceToTag[i]; - VisionMeasurement measurement = - new VisionMeasurement(visionPose, distanceFromTag, latencyInSec); + VisionMeasurement measurement = getVisionMeasurement(pos, i); poseManager.registerVisionMeasurement(measurement); } else { invalidCounter++; @@ -150,6 +144,32 @@ public void updateVision() { } } + private VisionMeasurement getVisionMeasurement(double[] pos, int index) { + double serverTime = apriltagSystem.getInputs().serverTime[index]; + double timestamp = 0; // latency is not right we are assuming zero + double latencyInSec = (serverTime - timestamp) / 1000; + Pose2d visionPose = new Pose2d(pos[0], pos[1], getEstimatedPose().getRotation()); + double distanceFromTag = apriltagSystem.getInputs().distanceToTag[index]; + return new VisionMeasurement(visionPose, distanceFromTag, latencyInSec); + } + + /** + * Collects Apriltag measurement(s) from the IO and checks their validity. If they are valid they + * are sent to the {@link PoseManager} for further processing + */ + public void updateVision() { + updateVision(15, 4, 14, 5, 16, 3); + } + + public void updateVision(Apriltag focusedTag) { + int[] invalidTags = + Arrays.stream(Apriltag.values()) + .filter(a -> a != focusedTag) + .mapToInt(Apriltag::number) + .toArray(); + updateVision(invalidTags); + } + /** * @param radians robot angle to reset odometry to * @param translation2d robot field position to reset odometry to diff --git a/src/main/java/frc/robot/utils/Apriltag.java b/src/main/java/frc/robot/utils/Apriltag.java index c010ffbb..73971529 100644 --- a/src/main/java/frc/robot/utils/Apriltag.java +++ b/src/main/java/frc/robot/utils/Apriltag.java @@ -59,7 +59,7 @@ public Translation3d getTranslation() { return new Translation3d(x, y, z); } - public double number() { + public int number() { return ordinal(); } } From aa9329bf8c8c64771585ec0552f6f0883519e413 Mon Sep 17 00:00:00 2001 From: codetoad Date: Wed, 19 Mar 2025 18:58:43 -0400 Subject: [PATCH 30/99] got rid of duplicate logic --- .../subsystems/swervev3/estimation/PoseEstimator.java | 8 +------- 1 file changed, 1 insertion(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/swervev3/estimation/PoseEstimator.java b/src/main/java/frc/robot/subsystems/swervev3/estimation/PoseEstimator.java index b7ee2989..7a0c2c60 100644 --- a/src/main/java/frc/robot/subsystems/swervev3/estimation/PoseEstimator.java +++ b/src/main/java/frc/robot/subsystems/swervev3/estimation/PoseEstimator.java @@ -127,13 +127,7 @@ private void updateVision(int... invalidApriltagNumbers) { }; if (validAprilTagPose(pos) && !ArrayUtils.contains( - invalidApriltagNumbers, apriltagSystem.getInputs().apriltagNumber[i]) - && apriltagSystem.getInputs().apriltagNumber[i] != 15 - && apriltagSystem.getInputs().apriltagNumber[i] != 4 - && apriltagSystem.getInputs().apriltagNumber[i] != 14 - && apriltagSystem.getInputs().apriltagNumber[i] != 5 - && apriltagSystem.getInputs().apriltagNumber[i] != 16 - && apriltagSystem.getInputs().apriltagNumber[i] != 3) { + invalidApriltagNumbers, apriltagSystem.getInputs().apriltagNumber[i])) { VisionMeasurement measurement = getVisionMeasurement(pos, i); poseManager.registerVisionMeasurement(measurement); } else { From 01d87c2cd86bec77d70e8982a47420c15daac751 Mon Sep 17 00:00:00 2001 From: Michael Kovalev <134699174+michaelk036@users.noreply.github.com> Date: Thu, 20 Mar 2025 19:56:30 -0400 Subject: [PATCH 31/99] Fixed from Lev's data, didn't touch Center x and y --- .../robot/constants/AlignmentPosition.java | 24 ++++++------- .../frc/robot/constants/BranchPositions.java | 36 +++++++++---------- 2 files changed, 30 insertions(+), 30 deletions(-) diff --git a/src/main/java/frc/robot/constants/AlignmentPosition.java b/src/main/java/frc/robot/constants/AlignmentPosition.java index e691e005..13f8eebc 100644 --- a/src/main/java/frc/robot/constants/AlignmentPosition.java +++ b/src/main/java/frc/robot/constants/AlignmentPosition.java @@ -10,51 +10,51 @@ public enum AlignmentPosition { BRANCH_A( - new Pose2d(-0.3424, 4.068154, Rotation2d.fromRadians(3.141592654)), + new Pose2d(3.2576000000000, 4.0614305050000, Rotation2d.fromRadians(3.1415926535898)), Apriltag.EIGHTEEN, Apriltag.SEVEN), BRANCH_B( - new Pose2d(-0.3424, 3.739154, Rotation2d.fromRadians(3.141592654)), + new Pose2d(3.2576000000000, 3.7328134950000, Rotation2d.fromRadians(3.1415926535898)), Apriltag.EIGHTEEN, Apriltag.SEVEN), BRANCH_C( - new Pose2d(2.021430073, -0.09755978616, Rotation2d.fromRadians(4.188790205)), + new Pose2d(3.8426981000607, 2.9769498582828, Rotation2d.fromRadians(4.1887902047864)), Apriltag.SEVENTEEN, Apriltag.SIX), BRANCH_D( - new Pose2d(2.30635243, -0.2620597862, Rotation2d.fromRadians(4.188790205)), + new Pose2d(4.1272887788364, 2.8126413532828, Rotation2d.fromRadians(4.1887902047864)), Apriltag.SEVENTEEN, Apriltag.SIX), BRANCH_E( - new Pose2d(6.810959073, -0.1332797862, Rotation2d.fromRadians(5.235987756)), + new Pose2d(5.0744349400607, 2.9414193532828, Rotation2d.fromRadians(5.2359877559830)), Apriltag.TWENTY_TWO, Apriltag.ELEVEN), BRANCH_F( - new Pose2d(7.09588143, 0.03122021384, Rotation2d.fromRadians(5.235987756)), + new Pose2d(5.3590256188364, 3.1057278582828, Rotation2d.fromRadians(5.2359877559830)), Apriltag.TWENTY_TWO, Apriltag.ELEVEN), BRANCH_G( - new Pose2d(9.236658, 3.996714, Rotation2d.fromRadians(6.283185307)), + new Pose2d(5.7210736800000, 3.9903694950000, Rotation2d.fromRadians(6.2831853071796)), Apriltag.TWENTY_ONE, Apriltag.TEN), BRANCH_H( - new Pose2d(9.236658, 4.325714, Rotation2d.fromRadians(6.283185307)), + new Pose2d(5.7210736800000, 4.3189865050000, Rotation2d.fromRadians(6.2831853071796)), Apriltag.TWENTY_ONE, Apriltag.TEN), BRANCH_I( - new Pose2d(6.872827927, 8.162427786, Rotation2d.fromRadians(7.330382858)), + new Pose2d(5.1359755799393, 5.0748501417172, Rotation2d.fromRadians(7.3303828583762)), Apriltag.TWENTY, Apriltag.NINE), BRANCH_J( - new Pose2d(6.58790557, 8.326927786, Rotation2d.fromRadians(7.330382858)), + new Pose2d(4.8513849011636, 5.2391586467172, Rotation2d.fromRadians(7.3303828583762)), Apriltag.TWENTY, Apriltag.NINE), BRANCH_K( - new Pose2d(2.083298927, 8.198147786, Rotation2d.fromRadians(8.37758041)), + new Pose2d(3.9042387399393, 5.1103806467172, Rotation2d.fromRadians(8.3775804095728)), Apriltag.NINETEEN, Apriltag.EIGHT), BRANCH_L( - new Pose2d(1.79837657, 8.033647786, Rotation2d.fromRadians(8.37758041)), + new Pose2d(3.6196480611636, 4.9460721417172, Rotation2d.fromRadians(8.3775804095728)), Apriltag.NINETEEN, Apriltag.EIGHT); diff --git a/src/main/java/frc/robot/constants/BranchPositions.java b/src/main/java/frc/robot/constants/BranchPositions.java index 53906dfd..c14a1fd4 100644 --- a/src/main/java/frc/robot/constants/BranchPositions.java +++ b/src/main/java/frc/robot/constants/BranchPositions.java @@ -6,25 +6,25 @@ public enum BranchPositions { CENTER(new Pose2d(0, 0, Rotation2d.fromRadians(0))), // TODO: add number for center of the reef - CENTER_A_B(new Pose2d(3.6576, 4.032434, Rotation2d.fromRadians(3.141592654))), - CENTER_C_D(new Pose2d(4.0523645, 3.348681829, Rotation2d.fromRadians(4.188790205))), - CENTER_E_F(new Pose2d(4.8418935, 3.348681829, Rotation2d.fromRadians(5.235987756))), - CENTER_G_H(new Pose2d(5.236658, 4.032434, Rotation2d.fromRadians(6.283185307))), - CENTER_I_J(new Pose2d(4.8418935, 4.716186171, Rotation2d.fromRadians(7.330382858))), - CENTER_K_L(new Pose2d(4.0523645, 4.716186171, Rotation2d.fromRadians(8.37758041))), + CENTER_A_B(new Pose2d(3.6576, 4.032434, Rotation2d.fromRadians(3.1415926535898))), + CENTER_C_D(new Pose2d(4.0523645, 3.348681829, Rotation2d.fromRadians(4.1887902047864))), + CENTER_E_F(new Pose2d(4.8418935, 3.348681829, Rotation2d.fromRadians(5.2359877559830))), + CENTER_G_H(new Pose2d(5.236658, 4.032434, Rotation2d.fromRadians(6.2831853071796))), + CENTER_I_J(new Pose2d(4.8418935, 4.716186171, Rotation2d.fromRadians(7.3303828583762))), + CENTER_K_L(new Pose2d(4.0523645, 4.716186171, Rotation2d.fromRadians(8.3775804095728))), - BRANCH_A(new Pose2d(3.6576, 4.196934, Rotation2d.fromRadians(3.141592654))), - BRANCH_B(new Pose2d(3.6576, 3.867934, Rotation2d.fromRadians(3.141592654))), - BRANCH_C(new Pose2d(3.909903321, 3.430931829, Rotation2d.fromRadians(4.188790205))), - BRANCH_D(new Pose2d(4.194825679, 3.266431829, Rotation2d.fromRadians(4.188790205))), - BRANCH_E(new Pose2d(4.699432321, 3.266431829, Rotation2d.fromRadians(5.235987756))), - BRANCH_F(new Pose2d(4.984354679, 3.430931829, Rotation2d.fromRadians(5.235987756))), - BRANCH_G(new Pose2d(5.236658, 3.867934, Rotation2d.fromRadians(6.283185307))), - BRANCH_H(new Pose2d(5.236658, 4.196934, Rotation2d.fromRadians(6.283185307))), - BRANCH_I(new Pose2d(4.984354679, 4.633936171, Rotation2d.fromRadians(7.330382858))), - BRANCH_J(new Pose2d(4.699432321, 4.798436171, Rotation2d.fromRadians(7.330382858))), - BRANCH_K(new Pose2d(4.194825679, 4.798436171, Rotation2d.fromRadians(8.37758041))), - BRANCH_L(new Pose2d(3.909903321, 4.633936171, Rotation2d.fromRadians(8.37758041))); + BRANCH_A(new Pose2d(3.6576000000000, 4.1902085050000, Rotation2d.fromRadians(3.1415926535898))), + BRANCH_B(new Pose2d(3.6576000000000, 3.8615914950000, Rotation2d.fromRadians(3.1415926535898))), + BRANCH_C(new Pose2d(3.9311730806122, 3.3877490197966, Rotation2d.fromRadians(4.1887902047864))), + BRANCH_D(new Pose2d(4.2157637593878, 3.2234405147966, Rotation2d.fromRadians(4.1887902047864))), + BRANCH_E(new Pose2d(4.7629099206122, 3.2234405147966, Rotation2d.fromRadians(5.2359877559830))), + BRANCH_F(new Pose2d(5.0475005993878, 3.3877490197966, Rotation2d.fromRadians(5.2359877559830))), + BRANCH_G(new Pose2d(5.3210736800000, 3.8615914950000, Rotation2d.fromRadians(6.2831853071796))), + BRANCH_H(new Pose2d(5.3210736800000, 4.1902085050000, Rotation2d.fromRadians(6.2831853071796))), + BRANCH_I(new Pose2d(5.0475005993878, 4.6640509802034, Rotation2d.fromRadians(7.3303828583762))), + BRANCH_J(new Pose2d(4.7629099206122, 4.8283594852034, Rotation2d.fromRadians(7.3303828583762))), + BRANCH_K(new Pose2d(4.2157637593878, 4.8283594852034, Rotation2d.fromRadians(8.3775804095728))), + BRANCH_L(new Pose2d(3.9311730806122, 4.6640509802034, Rotation2d.fromRadians(8.3775804095728))); private final Pose2d position; From 79e10571bedde0e241a017e4faa556815c377f5a Mon Sep 17 00:00:00 2001 From: codetoad Date: Fri, 21 Mar 2025 20:41:27 -0400 Subject: [PATCH 32/99] subtracked pi --- src/main/java/frc/robot/RobotContainer.java | 2 +- .../robot/constants/AlignmentPosition.java | 36 ++++++++++++------- 2 files changed, 25 insertions(+), 13 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 8b1040b3..babb4dfa 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -176,6 +176,7 @@ public RobotContainer() { } setupDriveTrain(); configureBindings(); + setupPathPlanning(); setupAutoChooser(); putShuffleboardCommands(); pathPlannerCommands(); @@ -222,7 +223,6 @@ private void pathPlannerCommands() { NamedCommands.registerCommand( "ElevatorToPositionL4", new SetElevatorStoredPosition(ElevatorPosition.LEVEL4, elevatorSubsystem, lightStrip)); - setupPathPlanning(); } private void configureBindings() { diff --git a/src/main/java/frc/robot/constants/AlignmentPosition.java b/src/main/java/frc/robot/constants/AlignmentPosition.java index 13f8eebc..8e389bed 100644 --- a/src/main/java/frc/robot/constants/AlignmentPosition.java +++ b/src/main/java/frc/robot/constants/AlignmentPosition.java @@ -10,51 +10,63 @@ public enum AlignmentPosition { BRANCH_A( - new Pose2d(3.2576000000000, 4.0614305050000, Rotation2d.fromRadians(3.1415926535898)), + new Pose2d( + 3.2576000000000, 4.0614305050000, Rotation2d.fromRadians(3.1415926535898 - Math.PI)), Apriltag.EIGHTEEN, Apriltag.SEVEN), BRANCH_B( - new Pose2d(3.2576000000000, 3.7328134950000, Rotation2d.fromRadians(3.1415926535898)), + new Pose2d( + 3.2576000000000, 3.7328134950000, Rotation2d.fromRadians(3.1415926535898 - Math.PI)), Apriltag.EIGHTEEN, Apriltag.SEVEN), BRANCH_C( - new Pose2d(3.8426981000607, 2.9769498582828, Rotation2d.fromRadians(4.1887902047864)), + new Pose2d( + 3.8426981000607, 2.9769498582828, Rotation2d.fromRadians(4.1887902047864 - Math.PI)), Apriltag.SEVENTEEN, Apriltag.SIX), BRANCH_D( - new Pose2d(4.1272887788364, 2.8126413532828, Rotation2d.fromRadians(4.1887902047864)), + new Pose2d( + 4.1272887788364, 2.8126413532828, Rotation2d.fromRadians(4.1887902047864 - Math.PI)), Apriltag.SEVENTEEN, Apriltag.SIX), BRANCH_E( - new Pose2d(5.0744349400607, 2.9414193532828, Rotation2d.fromRadians(5.2359877559830)), + new Pose2d( + 5.0744349400607, 2.9414193532828, Rotation2d.fromRadians(5.2359877559830 - Math.PI)), Apriltag.TWENTY_TWO, Apriltag.ELEVEN), BRANCH_F( - new Pose2d(5.3590256188364, 3.1057278582828, Rotation2d.fromRadians(5.2359877559830)), + new Pose2d( + 5.3590256188364, 3.1057278582828, Rotation2d.fromRadians(5.2359877559830 - Math.PI)), Apriltag.TWENTY_TWO, Apriltag.ELEVEN), BRANCH_G( - new Pose2d(5.7210736800000, 3.9903694950000, Rotation2d.fromRadians(6.2831853071796)), + new Pose2d( + 5.7210736800000, 3.9903694950000, Rotation2d.fromRadians(6.2831853071796 - Math.PI)), Apriltag.TWENTY_ONE, Apriltag.TEN), BRANCH_H( - new Pose2d(5.7210736800000, 4.3189865050000, Rotation2d.fromRadians(6.2831853071796)), + new Pose2d( + 5.7210736800000, 4.3189865050000, Rotation2d.fromRadians(6.2831853071796 - Math.PI)), Apriltag.TWENTY_ONE, Apriltag.TEN), BRANCH_I( - new Pose2d(5.1359755799393, 5.0748501417172, Rotation2d.fromRadians(7.3303828583762)), + new Pose2d( + 5.1359755799393, 5.0748501417172, Rotation2d.fromRadians(7.3303828583762 - Math.PI)), Apriltag.TWENTY, Apriltag.NINE), BRANCH_J( - new Pose2d(4.8513849011636, 5.2391586467172, Rotation2d.fromRadians(7.3303828583762)), + new Pose2d( + 4.8513849011636, 5.2391586467172, Rotation2d.fromRadians(7.3303828583762 - Math.PI)), Apriltag.TWENTY, Apriltag.NINE), BRANCH_K( - new Pose2d(3.9042387399393, 5.1103806467172, Rotation2d.fromRadians(8.3775804095728)), + new Pose2d( + 3.9042387399393, 5.1103806467172, Rotation2d.fromRadians(8.3775804095728 - Math.PI)), Apriltag.NINETEEN, Apriltag.EIGHT), BRANCH_L( - new Pose2d(3.6196480611636, 4.9460721417172, Rotation2d.fromRadians(8.3775804095728)), + new Pose2d( + 3.6196480611636, 4.9460721417172, Rotation2d.fromRadians(8.3775804095728 - Math.PI)), Apriltag.NINETEEN, Apriltag.EIGHT); From 0336b4cd515ed5ca9ac3bb9448ed1e6ea647392c Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Fri, 21 Mar 2025 23:53:27 -0400 Subject: [PATCH 33/99] some small stuff --- src/main/java/frc/robot/RobotContainer.java | 8 +- .../commands/FindCorrectBranchFromPos.java | 84 ++++++++++++ .../java/frc/robot/commands/UnitTester.java | 16 +++ .../frc/robot/constants/AlgaePositions.java | 52 ++++++++ .../frc/robot/constants/BranchPositions.java | 126 ++++++++++++++++++ .../frc/robot/constants/GameConstants.java | 10 ++ 6 files changed, 294 insertions(+), 2 deletions(-) create mode 100644 src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java create mode 100644 src/main/java/frc/robot/commands/UnitTester.java create mode 100644 src/main/java/frc/robot/constants/AlgaePositions.java create mode 100644 src/main/java/frc/robot/constants/BranchPositions.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 424ee467..51287d68 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -22,6 +22,7 @@ import frc.robot.autochooser.FieldLocation; import frc.robot.autochooser.chooser.AutoChooser2025; import frc.robot.autochooser.event.RealAutoEventProvider; +import frc.robot.commands.UnitTester; import frc.robot.commands.byebye.ByeByeToFwrLimit; import frc.robot.commands.byebye.ByeByeToRevLimit; import frc.robot.commands.climber.ClimbToLimit; @@ -78,6 +79,7 @@ import frc.robot.utils.logging.LoggableIO; import frc.robot.utils.motor.Gain; import frc.robot.utils.motor.PID; +import frc.robot.utils.shuffleboard.SmartShuffleboard; import frc.robot.utils.simulation.RobotVisualizer; import frc.robot.utils.simulation.SwerveSimulationUtils; import java.util.function.Consumer; @@ -250,7 +252,9 @@ private void configureBindings() { new SetElevatorTargetPosition(controller::getLeftY, elevatorSubsystem); elevatorSubsystem.setDefaultCommand(setElevatorTargetPosition); controller.b().onTrue(new ClimbToLimit(climber, Constants.CLIMBER_PHASE2_SPEED)); - controller.a().onTrue(new DeployHarpoon(climber, elevatorSubsystem, lightStrip, ElevatorPosition.CLIMB)); + controller + .a() + .onTrue(new DeployHarpoon(climber, elevatorSubsystem, lightStrip, ElevatorPosition.CLIMB)); // controller.a().onTrue(new DeployClimber(climber)); controller.x().onTrue(new ByeByeAllDone(byebyeTilt, byebyeRoller, elevatorSubsystem)); controller.y().onTrue(new RemoveAlgaeFromReef(byebyeTilt, byebyeRoller, elevatorSubsystem)); @@ -436,7 +440,7 @@ public RobotVisualizer getRobotVisualizer() { } public void putShuffleboardCommands() { - + SmartShuffleboard.putCommand("What Branch Unit Test", "Test 1", new UnitTester()); if (Constants.CORAL_DEBUG) { SmartDashboard.putData( "Shoot Coral", new ShootCoral(coralSubsystem, Constants.CORAL_SHOOTER_SPEED)); diff --git a/src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java b/src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java new file mode 100644 index 00000000..2ca339c7 --- /dev/null +++ b/src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java @@ -0,0 +1,84 @@ +package frc.robot.commands; + +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.Vector; +import edu.wpi.first.math.geometry.*; +import edu.wpi.first.math.numbers.N3; +import frc.robot.constants.AlgaePositions; +import frc.robot.constants.BranchPositions; +import frc.robot.constants.Constants; + +public class FindCorrectBranchFromPos { + public static final double POS_TO_FOV_CONVERSION_X = + Math.tan(Constants.LIMELIGHT_HALF_FOV.getX()) / Constants.LIMELIGHT_HALF_POS.getX(); + public static final double POS_TO_FOV_CONVERSION_Y = + Math.tan(Constants.LIMELIGHT_HALF_FOV.getY()) / Constants.LIMELIGHT_HALF_POS.getY(); + public static final double SPECIAL_X = + Constants.LIMELIGHT_HALF_POS.getX() / Math.tan(Constants.LIMELIGHT_HALF_FOV.getX()); + public static final double SPECIAL_Y = + Constants.LIMELIGHT_HALF_POS.getY() / Math.tan(Constants.LIMELIGHT_HALF_FOV.getY()); + public static final double SPECIAL_RATIO = SPECIAL_Y / SPECIAL_X; + private static final BranchPositions[] BRANCHES = BranchPositions.values(); + private static final AlgaePositions[] ALGAES = AlgaePositions.values(); + + public static BranchPositions UnitTest1() { + double x = -4.1902085050000 / 3.8337244850000 * SPECIAL_X + Constants.LIMELIGHT_HALF_POS.getX(); + double y = -0.706978325 / 3.8337244850000 * SPECIAL_Y + Constants.LIMELIGHT_HALF_POS.getY(); + return FindCoralBranch(new Pose2d(0, 0, new Rotation2d(0)), new Translation2d(x, y)); + } + + public static BranchPositions FindCoralBranch(Pose2d robotPos, Translation2d piecePos) { + final Pose3d cameraPos = new Pose3d(robotPos).transformBy(Constants.CAMERA_TO_ROBOT); + final Rotation3d invCameraRotation = cameraPos.getRotation().unaryMinus(); + final Translation2d pieceTranslation = Constants.LIMELIGHT_HALF_POS.minus(piecePos); + final Vector pieceVec = + VecBuilder.fill(SPECIAL_Y, pieceTranslation.getX() * SPECIAL_RATIO, pieceTranslation.getY()) + .unit(); + double maxDot = -1.0; + BranchPositions closestBranch = null; + for (BranchPositions branch : BRANCHES) { + Vector branchVec = + branch + .getPosition() + .minus(cameraPos) + .getTranslation() + .rotateBy(invCameraRotation) + .toVector() + .unit(); + double dot = pieceVec.dot(branchVec); + if (dot > maxDot) { + maxDot = dot; + closestBranch = branch; + } + } + return closestBranch; + } + + public static AlgaePositions FindAlgae(Pose2d robotPos, Translation2d piecePos) { + final Pose3d cameraPos = new Pose3d(robotPos).transformBy(Constants.CAMERA_TO_ROBOT); + final Rotation3d invCameraRotation = cameraPos.getRotation().unaryMinus(); + final Translation2d pieceTranslation = Constants.LIMELIGHT_HALF_POS.minus(piecePos); + + final Vector pieceVec = + VecBuilder.fill(SPECIAL_Y, pieceTranslation.getX() * SPECIAL_RATIO, pieceTranslation.getY()) + .unit(); + double maxDot = -1.0; + AlgaePositions closestAlgae = null; + for (AlgaePositions algae : ALGAES) { + Vector algaeVec = + algae + .getPosition() + .minus(cameraPos) + .getTranslation() + .rotateBy(invCameraRotation) + .toVector() + .unit(); + double dot = pieceVec.dot(algaeVec); + if (dot > maxDot) { + maxDot = dot; + closestAlgae = algae; + } + } + return closestAlgae; + } +} diff --git a/src/main/java/frc/robot/commands/UnitTester.java b/src/main/java/frc/robot/commands/UnitTester.java new file mode 100644 index 00000000..15a090ce --- /dev/null +++ b/src/main/java/frc/robot/commands/UnitTester.java @@ -0,0 +1,16 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.utils.shuffleboard.SmartShuffleboard; + +public class UnitTester extends Command { + @Override + public void execute() { + } + + @Override + public boolean isFinished() { + return true; + } +} diff --git a/src/main/java/frc/robot/constants/AlgaePositions.java b/src/main/java/frc/robot/constants/AlgaePositions.java new file mode 100644 index 00000000..12d63fdf --- /dev/null +++ b/src/main/java/frc/robot/constants/AlgaePositions.java @@ -0,0 +1,52 @@ +package frc.robot.constants; + +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation3d; + +public enum AlgaePositions { + Algae_AB_LOW( + new Pose3d( + 3.8337244850000, 4.0259000000000, 0.909311815, new Rotation3d(0, 0, 3.1415926535898))), + Algae_AB_HIGH( + new Pose3d( + 3.8337244850000, 4.0259000000000, 1.313978795, new Rotation3d(0, 0, 3.1415926535898))), + Algae_CD_LOW( + new Pose3d( + 4.1615306625000, 3.4581230455351, 0.909311815, new Rotation3d(0, 0, 4.1887902047864))), + Algae_CD_HIGH( + new Pose3d( + 4.1615306625000, 3.4581230455351, 1.313978795, new Rotation3d(0, 0, 4.1887902047864))), + Algae_EF_LOW( + new Pose3d( + 4.8171430175000, 3.4581230455351, 0.909311815, new Rotation3d(0, 0, 5.2359877559830))), + Algae_EF_HIGH( + new Pose3d( + 4.8171430175000, 3.4581230455351, 1.313978795, new Rotation3d(0, 0, 5.2359877559830))), + Algae_GH_LOW( + new Pose3d( + 5.1449491950000, 4.0259000000000, 0.909311815, new Rotation3d(0, 0, 6.2831853071796))), + Algae_GH_HIGH( + new Pose3d( + 5.1449491950000, 4.0259000000000, 1.313978795, new Rotation3d(0, 0, 6.2831853071796))), + Algae_IJ_LOW( + new Pose3d( + 4.8171430175000, 4.5936769544649, 0.909311815, new Rotation3d(0, 0, 7.3303828583762))), + Algae_IJ_HIGH( + new Pose3d( + 4.8171430175000, 4.5936769544649, 1.313978795, new Rotation3d(0, 0, 7.3303828583762))), + Algae_KL_LOW( + new Pose3d( + 4.1615306625000, 4.5936769544649, 0.909311815, new Rotation3d(0, 0, 8.3775804095728))), + Algae_KL_HIGH( + new Pose3d( + 4.1615306625000, 4.5936769544649, 1.313978795, new Rotation3d(0, 0, 8.3775804095728))); + private final Pose3d position; + + AlgaePositions(Pose3d position) { + this.position = position; + } + + public Pose3d getPosition() { + return position; + } +} diff --git a/src/main/java/frc/robot/constants/BranchPositions.java b/src/main/java/frc/robot/constants/BranchPositions.java new file mode 100644 index 00000000..ff961848 --- /dev/null +++ b/src/main/java/frc/robot/constants/BranchPositions.java @@ -0,0 +1,126 @@ +/** Technically not used now, mainly just for checking if the math is right */ +package frc.robot.constants; + +import edu.wpi.first.math.geometry.*; + +// TODO: change all of these later to the correct values +public enum BranchPositions { + BRANCH_A_2( + new Pose3d( + 3.8337244850000, 4.1902085050000, 0.706978325, new Rotation3d(0, 0, 3.1415926535898))), + BRANCH_A_3( + new Pose3d( + 3.8337244850000, 4.1902085050000, 1.111645305, new Rotation3d(0, 0, 3.1415926535898))), + BRANCH_A_4( + new Pose3d( + 3.6896079100000, 4.1902085050000, 1.736182305, new Rotation3d(0, 0, 3.1415926535898))), + BRANCH_B_2( + new Pose3d( + 3.8337244850000, 3.8615914950000, 0.706978325, new Rotation3d(0, 0, 3.1415926535898))), + BRANCH_B_3( + new Pose3d( + 3.8337244850000, 3.8615914950000, 1.111645305, new Rotation3d(0, 0, 3.1415926535898))), + BRANCH_B_4( + new Pose3d( + 3.6896079100000, 3.8615914950000, 1.736182305, new Rotation3d(0, 0, 3.1415926535898))), + BRANCH_C_2( + new Pose3d( + 4.0192353231122, 3.5402772980351, 0.706978325, new Rotation3d(0, 0, 4.1887902047864))), + BRANCH_C_3( + new Pose3d( + 4.0192353231122, 3.5402772980351, 1.111645305, new Rotation3d(0, 0, 4.1887902047864))), + BRANCH_C_4( + new Pose3d( + 3.9471770356122, 3.4154686829787, 1.736182305, new Rotation3d(0, 0, 4.1887902047864))), + BRANCH_D_2( + new Pose3d( + 4.3038260018878, 3.3759687930351, 0.706978325, new Rotation3d(0, 0, 4.1887902047864))), + BRANCH_D_3( + new Pose3d( + 4.3038260018878, 3.3759687930351, 1.111645305, new Rotation3d(0, 0, 4.1887902047864))), + BRANCH_D_4( + new Pose3d( + 4.2317677143878, 3.2511601779787, 1.736182305, new Rotation3d(0, 0, 4.1887902047864))), + BRANCH_E_2( + new Pose3d( + 4.6748476781122, 3.3759687930351, 0.706978325, new Rotation3d(0, 0, 5.2359877559830))), + BRANCH_E_3( + new Pose3d( + 4.6748476781122, 3.3759687930351, 1.111645305, new Rotation3d(0, 0, 5.2359877559830))), + BRANCH_E_4( + new Pose3d( + 4.7469059656122, 3.2511601779787, 1.736182305, new Rotation3d(0, 0, 5.2359877559830))), + BRANCH_F_2( + new Pose3d( + 4.9594383568878, 3.5402772980351, 0.706978325, new Rotation3d(0, 0, 5.2359877559830))), + BRANCH_F_3( + new Pose3d( + 4.9594383568878, 3.5402772980351, 1.111645305, new Rotation3d(0, 0, 5.2359877559830))), + BRANCH_F_4( + new Pose3d( + 5.0314966443878, 3.4154686829787, 1.736182305, new Rotation3d(0, 0, 5.2359877559830))), + BRANCH_G_2( + new Pose3d( + 5.1449491950000, 3.8615914950000, 0.706978325, new Rotation3d(0, 0, 6.2831853071796))), + BRANCH_G_3( + new Pose3d( + 5.1449491950000, 3.8615914950000, 1.111645305, new Rotation3d(0, 0, 6.2831853071796))), + BRANCH_G_4( + new Pose3d( + 5.2890657700000, 3.8615914950000, 1.736182305, new Rotation3d(0, 0, 6.2831853071796))), + BRANCH_H_2( + new Pose3d( + 5.1449491950000, 4.1902085050000, 0.706978325, new Rotation3d(0, 0, 6.2831853071796))), + BRANCH_H_3( + new Pose3d( + 5.1449491950000, 4.1902085050000, 1.111645305, new Rotation3d(0, 0, 6.2831853071796))), + BRANCH_H_4( + new Pose3d( + 5.2890657700000, 4.1902085050000, 1.736182305, new Rotation3d(0, 0, 6.2831853071796))), + BRANCH_I_2( + new Pose3d( + 4.9594383568878, 4.5115227019649, 0.706978325, new Rotation3d(0, 0, 7.3303828583762))), + BRANCH_I_3( + new Pose3d( + 4.9594383568878, 4.5115227019649, 1.111645305, new Rotation3d(0, 0, 7.3303828583762))), + BRANCH_I_4( + new Pose3d( + 5.0314966443878, 4.6363313170214, 1.736182305, new Rotation3d(0, 0, 7.3303828583762))), + BRANCH_J_2( + new Pose3d( + 4.6748476781122, 4.6758312069649, 0.706978325, new Rotation3d(0, 0, 7.3303828583762))), + BRANCH_J_3( + new Pose3d( + 4.6748476781122, 4.6758312069649, 1.111645305, new Rotation3d(0, 0, 7.3303828583762))), + BRANCH_J_4( + new Pose3d( + 4.7469059656122, 4.8006398220214, 1.736182305, new Rotation3d(0, 0, 7.3303828583762))), + BRANCH_K_2( + new Pose3d( + 4.3038260018878, 4.6758312069649, 0.706978325, new Rotation3d(0, 0, 8.3775804095728))), + BRANCH_K_3( + new Pose3d( + 4.3038260018878, 4.6758312069649, 1.111645305, new Rotation3d(0, 0, 8.3775804095728))), + BRANCH_K_4( + new Pose3d( + 4.2317677143878, 4.8006398220214, 1.736182305, new Rotation3d(0, 0, 8.3775804095728))), + BRANCH_L_2( + new Pose3d( + 4.0192353231122, 4.5115227019649, 0.706978325, new Rotation3d(0, 0, 8.3775804095728))), + BRANCH_L_3( + new Pose3d( + 4.0192353231122, 4.5115227019649, 1.111645305, new Rotation3d(0, 0, 8.3775804095728))), + BRANCH_L_4( + new Pose3d( + 3.9471770356122, 4.6363313170214, 1.736182305, new Rotation3d(0, 0, 8.3775804095728))); + + private final Pose3d position; + + BranchPositions(Pose3d position) { + this.position = position; + } + + public Pose3d getPosition() { + return position; + } +} diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java index 09cfc094..c56ebce8 100644 --- a/src/main/java/frc/robot/constants/GameConstants.java +++ b/src/main/java/frc/robot/constants/GameConstants.java @@ -1,6 +1,9 @@ package frc.robot.constants; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.RobotBase; import frc.robot.utils.SwerveModuleProfileV2; @@ -218,4 +221,11 @@ public enum Mode { public static final double ELEVATOR_MANUAL_DEADBAND = 0.2; public static final double ELEVATOR_MANUAL_MAX_SPEED_UP = -.3; public static final double ELEVATOR_MANUAL_MAX_SPEED_DOWN = .15; + + // Limelight + public static final Translation2d LIMELIGHT_HALF_POS = new Translation2d(240, 320).div(2); + public static final Transform3d CAMERA_TO_ROBOT = + new Transform3d(0.0, 0.0, 0.0, new Rotation3d(0.0, 0, 0.0)); // TODO Change Later + public static final Translation2d LIMELIGHT_HALF_FOV = + new Translation2d(0.86742863824118179973, 1.04021623418862042785).div(2); } From 916fad44c83582a6e2ec1c9ead2011bde1a9f355 Mon Sep 17 00:00:00 2001 From: jamthepersonj Date: Sat, 22 Mar 2025 11:31:02 -0400 Subject: [PATCH 34/99] added a command to switch to a robot centric drive mode --- src/main/java/frc/robot/RobotContainer.java | 4 +- .../drivetrain/RobotCentricDrive.java | 50 +++++++++++++++++++ 2 files changed, 53 insertions(+), 1 deletion(-) create mode 100644 src/main/java/frc/robot/commands/drivetrain/RobotCentricDrive.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 424ee467..52e4fc79 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -250,7 +250,9 @@ private void configureBindings() { new SetElevatorTargetPosition(controller::getLeftY, elevatorSubsystem); elevatorSubsystem.setDefaultCommand(setElevatorTargetPosition); controller.b().onTrue(new ClimbToLimit(climber, Constants.CLIMBER_PHASE2_SPEED)); - controller.a().onTrue(new DeployHarpoon(climber, elevatorSubsystem, lightStrip, ElevatorPosition.CLIMB)); + controller + .a() + .onTrue(new DeployHarpoon(climber, elevatorSubsystem, lightStrip, ElevatorPosition.CLIMB)); // controller.a().onTrue(new DeployClimber(climber)); controller.x().onTrue(new ByeByeAllDone(byebyeTilt, byebyeRoller, elevatorSubsystem)); controller.y().onTrue(new RemoveAlgaeFromReef(byebyeTilt, byebyeRoller, elevatorSubsystem)); diff --git a/src/main/java/frc/robot/commands/drivetrain/RobotCentricDrive.java b/src/main/java/frc/robot/commands/drivetrain/RobotCentricDrive.java new file mode 100644 index 00000000..364f2e8b --- /dev/null +++ b/src/main/java/frc/robot/commands/drivetrain/RobotCentricDrive.java @@ -0,0 +1,50 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands.drivetrain; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import frc.robot.constants.Constants; +import frc.robot.subsystems.swervev3.SwerveDrivetrain; +import frc.robot.utils.DriveMode; +import frc.robot.utils.logging.commands.LoggableCommand; +import java.util.function.DoubleSupplier; + +public class RobotCentricDrive extends LoggableCommand { + private final SwerveDrivetrain drivetrain; + private final DoubleSupplier joystickHorizontal; + private final DoubleSupplier joystickVerticle; + + public RobotCentricDrive( + SwerveDrivetrain drivetrain, + DoubleSupplier joystickHorizontal, + DoubleSupplier joystickVerticle) { + this.drivetrain = drivetrain; + this.joystickHorizontal = joystickHorizontal; + this.joystickVerticle = joystickVerticle; + addRequirements(drivetrain); + } + + @Override + public void initialize() {} + + @Override + public void execute() { + double y = + MathUtil.applyDeadband(joystickHorizontal.getAsDouble(), 0.05) * Constants.MAX_VELOCITY; + double x = + MathUtil.applyDeadband(joystickVerticle.getAsDouble(), 0.05) * Constants.MAX_VELOCITY; + ChassisSpeeds speeds = drivetrain.createChassisSpeeds(x, y, 0, DriveMode.ROBOT_CENTRIC); + drivetrain.drive(speeds); + } + + @Override + public void end(boolean interrupted) {} + + @Override + public boolean isFinished() { + return false; + } +} From 416b0409782038073df54a5f529c901d7edb4037 Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Sat, 22 Mar 2025 12:29:22 -0400 Subject: [PATCH 35/99] MORE linear algebra --- .../commands/FindCorrectBranchFromPos.java | 74 +++++++++---------- .../java/frc/robot/commands/UnitTester.java | 5 +- .../frc/robot/constants/GameConstants.java | 11 ++- 3 files changed, 43 insertions(+), 47 deletions(-) diff --git a/src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java b/src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java index 2ca339c7..4ad6b4d8 100644 --- a/src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java +++ b/src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java @@ -3,82 +3,78 @@ import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.Vector; import edu.wpi.first.math.geometry.*; +import edu.wpi.first.math.numbers.N2; import edu.wpi.first.math.numbers.N3; import frc.robot.constants.AlgaePositions; import frc.robot.constants.BranchPositions; import frc.robot.constants.Constants; +import java.util.Arrays; public class FindCorrectBranchFromPos { - public static final double POS_TO_FOV_CONVERSION_X = - Math.tan(Constants.LIMELIGHT_HALF_FOV.getX()) / Constants.LIMELIGHT_HALF_POS.getX(); - public static final double POS_TO_FOV_CONVERSION_Y = - Math.tan(Constants.LIMELIGHT_HALF_FOV.getY()) / Constants.LIMELIGHT_HALF_POS.getY(); public static final double SPECIAL_X = - Constants.LIMELIGHT_HALF_POS.getX() / Math.tan(Constants.LIMELIGHT_HALF_FOV.getX()); + Constants.LIMELIGHT_HALF_POS.get(0) / Math.tan(Constants.LIMELIGHT_HALF_FOV.get(0)); public static final double SPECIAL_Y = - Constants.LIMELIGHT_HALF_POS.getY() / Math.tan(Constants.LIMELIGHT_HALF_FOV.getY()); + Constants.LIMELIGHT_HALF_POS.get(0) / Math.tan(Constants.LIMELIGHT_HALF_FOV.get(0)); public static final double SPECIAL_RATIO = SPECIAL_Y / SPECIAL_X; private static final BranchPositions[] BRANCHES = BranchPositions.values(); private static final AlgaePositions[] ALGAES = AlgaePositions.values(); + private static final Vector[] PRECOMPUTED_BRANCH_VECS = + Arrays.stream(BranchPositions.values()) + .map(branch -> branch.getPosition().getTranslation().toVector()) + .toArray(Vector[]::new); + private static final Vector[] PRECOMPUTED_ALGAE_VECS = + Arrays.stream(AlgaePositions.values()) + .map(algae -> algae.getPosition().getTranslation().toVector()) + .toArray(Vector[]::new); - public static BranchPositions UnitTest1() { - double x = -4.1902085050000 / 3.8337244850000 * SPECIAL_X + Constants.LIMELIGHT_HALF_POS.getX(); - double y = -0.706978325 / 3.8337244850000 * SPECIAL_Y + Constants.LIMELIGHT_HALF_POS.getY(); - return FindCoralBranch(new Pose2d(0, 0, new Rotation2d(0)), new Translation2d(x, y)); - } - - public static BranchPositions FindCoralBranch(Pose2d robotPos, Translation2d piecePos) { + public static BranchPositions FindCoralBranch(Pose2d robotPos, Vector piecePos) { final Pose3d cameraPos = new Pose3d(robotPos).transformBy(Constants.CAMERA_TO_ROBOT); - final Rotation3d invCameraRotation = cameraPos.getRotation().unaryMinus(); - final Translation2d pieceTranslation = Constants.LIMELIGHT_HALF_POS.minus(piecePos); + final Vector cameraPosVec = cameraPos.getTranslation().toVector(); + final Vector invCameraRotation = cameraPos.getRotation().unaryMinus().toVector(); + final Vector pieceTranslation = Constants.LIMELIGHT_HALF_POS.minus(piecePos); final Vector pieceVec = - VecBuilder.fill(SPECIAL_Y, pieceTranslation.getX() * SPECIAL_RATIO, pieceTranslation.getY()) + VecBuilder.fill(SPECIAL_Y, pieceTranslation.get(0) * SPECIAL_RATIO, pieceTranslation.get(1)) .unit(); double maxDot = -1.0; BranchPositions closestBranch = null; - for (BranchPositions branch : BRANCHES) { + for (int i = 0; i < BRANCHES.length; i++) { Vector branchVec = - branch - .getPosition() - .minus(cameraPos) - .getTranslation() - .rotateBy(invCameraRotation) - .toVector() - .unit(); + Vector.cross(PRECOMPUTED_BRANCH_VECS[i].minus(cameraPosVec), invCameraRotation).unit(); double dot = pieceVec.dot(branchVec); if (dot > maxDot) { maxDot = dot; - closestBranch = branch; + closestBranch = BRANCHES[i]; } } return closestBranch; } - public static AlgaePositions FindAlgae(Pose2d robotPos, Translation2d piecePos) { + public static AlgaePositions FindAlgae(Pose2d robotPos, Vector piecePos) { final Pose3d cameraPos = new Pose3d(robotPos).transformBy(Constants.CAMERA_TO_ROBOT); - final Rotation3d invCameraRotation = cameraPos.getRotation().unaryMinus(); - final Translation2d pieceTranslation = Constants.LIMELIGHT_HALF_POS.minus(piecePos); - + final Vector cameraPosVec = cameraPos.getTranslation().toVector(); + final Vector invCameraRotation = cameraPos.getRotation().unaryMinus().toVector(); + final Vector pieceTranslation = Constants.LIMELIGHT_HALF_POS.minus(piecePos); final Vector pieceVec = - VecBuilder.fill(SPECIAL_Y, pieceTranslation.getX() * SPECIAL_RATIO, pieceTranslation.getY()) + VecBuilder.fill(SPECIAL_Y, pieceTranslation.get(0) * SPECIAL_RATIO, pieceTranslation.get(1)) .unit(); double maxDot = -1.0; AlgaePositions closestAlgae = null; - for (AlgaePositions algae : ALGAES) { + for (int i = 0; i < ALGAES.length; i++) { Vector algaeVec = - algae - .getPosition() - .minus(cameraPos) - .getTranslation() - .rotateBy(invCameraRotation) - .toVector() - .unit(); + Vector.cross(PRECOMPUTED_ALGAE_VECS[i].minus(cameraPosVec), invCameraRotation).unit(); double dot = pieceVec.dot(algaeVec); if (dot > maxDot) { maxDot = dot; - closestAlgae = algae; + closestAlgae = ALGAES[i]; } } return closestAlgae; } + + public static BranchPositions UnitTest1() { + double x = -4.1902085050000 / 3.8337244850000 * SPECIAL_X + Constants.LIMELIGHT_HALF_POS.get(0); + double y = -0.706978325 / 3.8337244850000 * SPECIAL_Y + Constants.LIMELIGHT_HALF_POS.get(1); + return FindCoralBranch( + new Pose2d(0, 0, new Rotation2d(0)), new Vector(VecBuilder.fill(x, y))); + } } diff --git a/src/main/java/frc/robot/commands/UnitTester.java b/src/main/java/frc/robot/commands/UnitTester.java index 15a090ce..9e70edff 100644 --- a/src/main/java/frc/robot/commands/UnitTester.java +++ b/src/main/java/frc/robot/commands/UnitTester.java @@ -1,13 +1,10 @@ package frc.robot.commands; -import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.utils.shuffleboard.SmartShuffleboard; public class UnitTester extends Command { @Override - public void execute() { - } + public void execute() {} @Override public boolean isFinished() { diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java index c56ebce8..15a2aa65 100644 --- a/src/main/java/frc/robot/constants/GameConstants.java +++ b/src/main/java/frc/robot/constants/GameConstants.java @@ -1,9 +1,11 @@ package frc.robot.constants; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.Vector; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.numbers.N2; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.RobotBase; import frc.robot.utils.SwerveModuleProfileV2; @@ -223,9 +225,10 @@ public enum Mode { public static final double ELEVATOR_MANUAL_MAX_SPEED_DOWN = .15; // Limelight - public static final Translation2d LIMELIGHT_HALF_POS = new Translation2d(240, 320).div(2); + public static final Vector LIMELIGHT_HALF_POS = + new Vector(VecBuilder.fill(240, 320).div(2)); public static final Transform3d CAMERA_TO_ROBOT = new Transform3d(0.0, 0.0, 0.0, new Rotation3d(0.0, 0, 0.0)); // TODO Change Later - public static final Translation2d LIMELIGHT_HALF_FOV = - new Translation2d(0.86742863824118179973, 1.04021623418862042785).div(2); + public static final Vector LIMELIGHT_HALF_FOV = + new Vector(VecBuilder.fill(0.86742863824118179973, 1.04021623418862042785).div(2)); } From c27a848691d89094be699d375690311746ae3711 Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Sat, 22 Mar 2025 12:54:40 -0400 Subject: [PATCH 36/99] find quaternions are better --- .../commands/FindCorrectBranchFromPos.java | 42 +++++++++++-------- 1 file changed, 25 insertions(+), 17 deletions(-) diff --git a/src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java b/src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java index 4ad6b4d8..ebdfbb1f 100644 --- a/src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java +++ b/src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java @@ -14,32 +14,36 @@ public class FindCorrectBranchFromPos { public static final double SPECIAL_X = Constants.LIMELIGHT_HALF_POS.get(0) / Math.tan(Constants.LIMELIGHT_HALF_FOV.get(0)); public static final double SPECIAL_Y = - Constants.LIMELIGHT_HALF_POS.get(0) / Math.tan(Constants.LIMELIGHT_HALF_FOV.get(0)); + Constants.LIMELIGHT_HALF_POS.get(1) / Math.tan(Constants.LIMELIGHT_HALF_FOV.get(1)); public static final double SPECIAL_RATIO = SPECIAL_Y / SPECIAL_X; private static final BranchPositions[] BRANCHES = BranchPositions.values(); private static final AlgaePositions[] ALGAES = AlgaePositions.values(); - private static final Vector[] PRECOMPUTED_BRANCH_VECS = + private static final Translation3d[] PRECOMPUTED_BRANCH_VECS = Arrays.stream(BranchPositions.values()) - .map(branch -> branch.getPosition().getTranslation().toVector()) - .toArray(Vector[]::new); - private static final Vector[] PRECOMPUTED_ALGAE_VECS = + .map(branch -> branch.getPosition().getTranslation()) + .toArray(Translation3d[]::new); + private static final Translation3d[] PRECOMPUTED_ALGAE_VECS = Arrays.stream(AlgaePositions.values()) - .map(algae -> algae.getPosition().getTranslation().toVector()) - .toArray(Vector[]::new); + .map(algae -> algae.getPosition().getTranslation()) + .toArray(Translation3d[]::new); public static BranchPositions FindCoralBranch(Pose2d robotPos, Vector piecePos) { final Pose3d cameraPos = new Pose3d(robotPos).transformBy(Constants.CAMERA_TO_ROBOT); - final Vector cameraPosVec = cameraPos.getTranslation().toVector(); - final Vector invCameraRotation = cameraPos.getRotation().unaryMinus().toVector(); + final Translation3d cameraPosVec = cameraPos.getTranslation(); + final Rotation3d invCameraRotation = cameraPos.getRotation().unaryMinus(); final Vector pieceTranslation = Constants.LIMELIGHT_HALF_POS.minus(piecePos); final Vector pieceVec = - VecBuilder.fill(SPECIAL_Y, pieceTranslation.get(0) * SPECIAL_RATIO, pieceTranslation.get(1)) - .unit(); + VecBuilder.fill( + SPECIAL_Y, pieceTranslation.get(0) * SPECIAL_RATIO, pieceTranslation.get(1)); double maxDot = -1.0; BranchPositions closestBranch = null; for (int i = 0; i < BRANCHES.length; i++) { Vector branchVec = - Vector.cross(PRECOMPUTED_BRANCH_VECS[i].minus(cameraPosVec), invCameraRotation).unit(); + PRECOMPUTED_BRANCH_VECS[i] + .minus(cameraPosVec) + .rotateBy(invCameraRotation) + .toVector() + .unit(); double dot = pieceVec.dot(branchVec); if (dot > maxDot) { maxDot = dot; @@ -51,17 +55,21 @@ public static BranchPositions FindCoralBranch(Pose2d robotPos, Vector pieceP public static AlgaePositions FindAlgae(Pose2d robotPos, Vector piecePos) { final Pose3d cameraPos = new Pose3d(robotPos).transformBy(Constants.CAMERA_TO_ROBOT); - final Vector cameraPosVec = cameraPos.getTranslation().toVector(); - final Vector invCameraRotation = cameraPos.getRotation().unaryMinus().toVector(); + final Translation3d cameraPosVec = cameraPos.getTranslation(); + final Rotation3d invCameraRotation = cameraPos.getRotation().unaryMinus(); final Vector pieceTranslation = Constants.LIMELIGHT_HALF_POS.minus(piecePos); final Vector pieceVec = - VecBuilder.fill(SPECIAL_Y, pieceTranslation.get(0) * SPECIAL_RATIO, pieceTranslation.get(1)) - .unit(); + VecBuilder.fill( + SPECIAL_Y, pieceTranslation.get(0) * SPECIAL_RATIO, pieceTranslation.get(1)); double maxDot = -1.0; AlgaePositions closestAlgae = null; for (int i = 0; i < ALGAES.length; i++) { Vector algaeVec = - Vector.cross(PRECOMPUTED_ALGAE_VECS[i].minus(cameraPosVec), invCameraRotation).unit(); + PRECOMPUTED_ALGAE_VECS[i] + .minus(cameraPosVec) + .rotateBy(invCameraRotation) + .toVector() + .unit(); double dot = pieceVec.dot(algaeVec); if (dot > maxDot) { maxDot = dot; From 848990ea502ca0c2e37a77f20a1ef3ac632dbb93 Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Sat, 22 Mar 2025 12:56:04 -0400 Subject: [PATCH 37/99] for loops are better --- .../commands/FindCorrectBranchFromPos.java | 23 ++++++++++++------- 1 file changed, 15 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java b/src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java index ebdfbb1f..8d2c860a 100644 --- a/src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java +++ b/src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java @@ -18,14 +18,21 @@ public class FindCorrectBranchFromPos { public static final double SPECIAL_RATIO = SPECIAL_Y / SPECIAL_X; private static final BranchPositions[] BRANCHES = BranchPositions.values(); private static final AlgaePositions[] ALGAES = AlgaePositions.values(); - private static final Translation3d[] PRECOMPUTED_BRANCH_VECS = - Arrays.stream(BranchPositions.values()) - .map(branch -> branch.getPosition().getTranslation()) - .toArray(Translation3d[]::new); - private static final Translation3d[] PRECOMPUTED_ALGAE_VECS = - Arrays.stream(AlgaePositions.values()) - .map(algae -> algae.getPosition().getTranslation()) - .toArray(Translation3d[]::new); + private static final Vector[] PRECOMPUTED_BRANCH_VECS; + private static final Vector[] PRECOMPUTED_ALGAE_VECS; + + static { + PRECOMPUTED_BRANCH_VECS = new Vector[BRANCHES.length]; + for (int i = 0; i < BRANCHES.length; i++) { + PRECOMPUTED_BRANCH_VECS[i] = BRANCHES[i].getPosition().getTranslation().toVector(); + } + + PRECOMPUTED_ALGAE_VECS = new Vector[ALGAES.length]; + for (int i = 0; i < ALGAES.length; i++) { + PRECOMPUTED_ALGAE_VECS[i] = ALGAES[i].getPosition().getTranslation().toVector(); + } + } + public static BranchPositions FindCoralBranch(Pose2d robotPos, Vector piecePos) { final Pose3d cameraPos = new Pose3d(robotPos).transformBy(Constants.CAMERA_TO_ROBOT); From cc499ae5a4beddaca34e84aed634e36c98a2f61a Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Sat, 22 Mar 2025 12:59:03 -0400 Subject: [PATCH 38/99] fixed --- .../frc/robot/commands/FindCorrectBranchFromPos.java | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java b/src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java index 8d2c860a..c3cd12fe 100644 --- a/src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java +++ b/src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java @@ -18,18 +18,18 @@ public class FindCorrectBranchFromPos { public static final double SPECIAL_RATIO = SPECIAL_Y / SPECIAL_X; private static final BranchPositions[] BRANCHES = BranchPositions.values(); private static final AlgaePositions[] ALGAES = AlgaePositions.values(); - private static final Vector[] PRECOMPUTED_BRANCH_VECS; - private static final Vector[] PRECOMPUTED_ALGAE_VECS; + private static final Translation3d[] PRECOMPUTED_BRANCH_VECS; + private static final Translation3d[] PRECOMPUTED_ALGAE_VECS; static { - PRECOMPUTED_BRANCH_VECS = new Vector[BRANCHES.length]; + PRECOMPUTED_BRANCH_VECS = new Translation3d[BRANCHES.length]; for (int i = 0; i < BRANCHES.length; i++) { - PRECOMPUTED_BRANCH_VECS[i] = BRANCHES[i].getPosition().getTranslation().toVector(); + PRECOMPUTED_BRANCH_VECS[i] = BRANCHES[i].getPosition().getTranslation(); } - PRECOMPUTED_ALGAE_VECS = new Vector[ALGAES.length]; + PRECOMPUTED_ALGAE_VECS = new Translation3d[ALGAES.length]; for (int i = 0; i < ALGAES.length; i++) { - PRECOMPUTED_ALGAE_VECS[i] = ALGAES[i].getPosition().getTranslation().toVector(); + PRECOMPUTED_ALGAE_VECS[i] = ALGAES[i].getPosition().getTranslation(); } } From 99f5a8c1d3e487644693fc43fbf2d097f21bfb44 Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Sat, 22 Mar 2025 13:03:14 -0400 Subject: [PATCH 39/99] linter --- src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java b/src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java index c3cd12fe..44368e8d 100644 --- a/src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java +++ b/src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java @@ -8,7 +8,6 @@ import frc.robot.constants.AlgaePositions; import frc.robot.constants.BranchPositions; import frc.robot.constants.Constants; -import java.util.Arrays; public class FindCorrectBranchFromPos { public static final double SPECIAL_X = @@ -33,7 +32,6 @@ public class FindCorrectBranchFromPos { } } - public static BranchPositions FindCoralBranch(Pose2d robotPos, Vector piecePos) { final Pose3d cameraPos = new Pose3d(robotPos).transformBy(Constants.CAMERA_TO_ROBOT); final Translation3d cameraPosVec = cameraPos.getTranslation(); From d7cfb4ec584f65810fde6a2d83a764ae0e29c17d Mon Sep 17 00:00:00 2001 From: codetoad Date: Sat, 22 Mar 2025 14:36:32 -0400 Subject: [PATCH 40/99] new pos --- src/main/java/frc/robot/RobotContainer.java | 5 +++-- src/main/java/frc/robot/constants/AlignmentPosition.java | 4 ++-- 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index d2ac6cf9..069aecdd 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -26,7 +26,6 @@ import frc.robot.autochooser.FieldLocation; import frc.robot.autochooser.chooser.AutoChooser2025; import frc.robot.autochooser.event.RealAutoEventProvider; -import frc.robot.commands.RollAlgae; import frc.robot.commands.alignment.AlignClosestBranch; import frc.robot.commands.byebye.ByeByeToFwrLimit; import frc.robot.commands.byebye.ByeByeToRevLimit; @@ -260,7 +259,9 @@ private void configureBindings() { new SetElevatorTargetPosition(controller::getLeftY, elevatorSubsystem); elevatorSubsystem.setDefaultCommand(setElevatorTargetPosition); controller.b().onTrue(new ClimbToLimit(climber, Constants.CLIMBER_PHASE2_SPEED)); - controller.a().onTrue(new DeployHarpoon(climber, elevatorSubsystem, lightStrip, ElevatorPosition.CLIMB)); + controller + .a() + .onTrue(new DeployHarpoon(climber, elevatorSubsystem, lightStrip, ElevatorPosition.CLIMB)); // controller.a().onTrue(new DeployClimber(climber)); controller.x().onTrue(new ByeByeAllDone(byebyeTilt, byebyeRoller, elevatorSubsystem)); controller.y().onTrue(new RemoveAlgaeFromReef(byebyeTilt, byebyeRoller, elevatorSubsystem)); diff --git a/src/main/java/frc/robot/constants/AlignmentPosition.java b/src/main/java/frc/robot/constants/AlignmentPosition.java index 8e389bed..9fc3f2fe 100644 --- a/src/main/java/frc/robot/constants/AlignmentPosition.java +++ b/src/main/java/frc/robot/constants/AlignmentPosition.java @@ -11,12 +11,12 @@ public enum AlignmentPosition { BRANCH_A( new Pose2d( - 3.2576000000000, 4.0614305050000, Rotation2d.fromRadians(3.1415926535898 - Math.PI)), + 3.2067500000000, 4.0614305050000, Rotation2d.fromRadians(3.1415926535898 - Math.PI)), Apriltag.EIGHTEEN, Apriltag.SEVEN), BRANCH_B( new Pose2d( - 3.2576000000000, 3.7328134950000, Rotation2d.fromRadians(3.1415926535898 - Math.PI)), + 3.2067500000000, 3.7328134950000, Rotation2d.fromRadians(3.1415926535898 - Math.PI)), Apriltag.EIGHTEEN, Apriltag.SEVEN), BRANCH_C( From f38f5b8c9c0df6441529a85a463878af6d28b02b Mon Sep 17 00:00:00 2001 From: jamthepersonj Date: Sat, 22 Mar 2025 16:20:09 -0400 Subject: [PATCH 41/99] update --- .../commands/drivetrain/RobotCentricDrive.java | 18 +++++++----------- 1 file changed, 7 insertions(+), 11 deletions(-) diff --git a/src/main/java/frc/robot/commands/drivetrain/RobotCentricDrive.java b/src/main/java/frc/robot/commands/drivetrain/RobotCentricDrive.java index 364f2e8b..e84253c8 100644 --- a/src/main/java/frc/robot/commands/drivetrain/RobotCentricDrive.java +++ b/src/main/java/frc/robot/commands/drivetrain/RobotCentricDrive.java @@ -14,16 +14,16 @@ public class RobotCentricDrive extends LoggableCommand { private final SwerveDrivetrain drivetrain; - private final DoubleSupplier joystickHorizontal; - private final DoubleSupplier joystickVerticle; + private final double xMove; + private final double yMove; public RobotCentricDrive( SwerveDrivetrain drivetrain, - DoubleSupplier joystickHorizontal, - DoubleSupplier joystickVerticle) { + double xMove, + double yMove) { this.drivetrain = drivetrain; - this.joystickHorizontal = joystickHorizontal; - this.joystickVerticle = joystickVerticle; + this.xMove = xMove; + this.yMove = yMove; addRequirements(drivetrain); } @@ -32,11 +32,7 @@ public void initialize() {} @Override public void execute() { - double y = - MathUtil.applyDeadband(joystickHorizontal.getAsDouble(), 0.05) * Constants.MAX_VELOCITY; - double x = - MathUtil.applyDeadband(joystickVerticle.getAsDouble(), 0.05) * Constants.MAX_VELOCITY; - ChassisSpeeds speeds = drivetrain.createChassisSpeeds(x, y, 0, DriveMode.ROBOT_CENTRIC); + ChassisSpeeds speeds = drivetrain.createChassisSpeeds(xMove, yMove, 0, DriveMode.ROBOT_CENTRIC); drivetrain.drive(speeds); } From 49d196773b2faddb534ac98d03049938f25efb35 Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Sat, 22 Mar 2025 16:58:50 -0400 Subject: [PATCH 42/99] corrected for api --- .../commands/FindCorrectBranchFromPos.java | 22 +++++++------------ .../java/frc/robot/commands/UnitTester.java | 5 ++++- .../frc/robot/constants/GameConstants.java | 2 +- 3 files changed, 13 insertions(+), 16 deletions(-) diff --git a/src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java b/src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java index 44368e8d..2ab37a8d 100644 --- a/src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java +++ b/src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java @@ -10,11 +10,6 @@ import frc.robot.constants.Constants; public class FindCorrectBranchFromPos { - public static final double SPECIAL_X = - Constants.LIMELIGHT_HALF_POS.get(0) / Math.tan(Constants.LIMELIGHT_HALF_FOV.get(0)); - public static final double SPECIAL_Y = - Constants.LIMELIGHT_HALF_POS.get(1) / Math.tan(Constants.LIMELIGHT_HALF_FOV.get(1)); - public static final double SPECIAL_RATIO = SPECIAL_Y / SPECIAL_X; private static final BranchPositions[] BRANCHES = BranchPositions.values(); private static final AlgaePositions[] ALGAES = AlgaePositions.values(); private static final Translation3d[] PRECOMPUTED_BRANCH_VECS; @@ -32,14 +27,13 @@ public class FindCorrectBranchFromPos { } } + // PeicePos is in degrees tx,ty public static BranchPositions FindCoralBranch(Pose2d robotPos, Vector piecePos) { final Pose3d cameraPos = new Pose3d(robotPos).transformBy(Constants.CAMERA_TO_ROBOT); final Translation3d cameraPosVec = cameraPos.getTranslation(); final Rotation3d invCameraRotation = cameraPos.getRotation().unaryMinus(); - final Vector pieceTranslation = Constants.LIMELIGHT_HALF_POS.minus(piecePos); final Vector pieceVec = - VecBuilder.fill( - SPECIAL_Y, pieceTranslation.get(0) * SPECIAL_RATIO, pieceTranslation.get(1)); + VecBuilder.fill(1, Math.tan(-piecePos.get(0)), Math.tan(piecePos.get(1))); double maxDot = -1.0; BranchPositions closestBranch = null; for (int i = 0; i < BRANCHES.length; i++) { @@ -58,14 +52,13 @@ public static BranchPositions FindCoralBranch(Pose2d robotPos, Vector pieceP return closestBranch; } + // peicepos is in digrees tx,ty public static AlgaePositions FindAlgae(Pose2d robotPos, Vector piecePos) { final Pose3d cameraPos = new Pose3d(robotPos).transformBy(Constants.CAMERA_TO_ROBOT); final Translation3d cameraPosVec = cameraPos.getTranslation(); final Rotation3d invCameraRotation = cameraPos.getRotation().unaryMinus(); - final Vector pieceTranslation = Constants.LIMELIGHT_HALF_POS.minus(piecePos); final Vector pieceVec = - VecBuilder.fill( - SPECIAL_Y, pieceTranslation.get(0) * SPECIAL_RATIO, pieceTranslation.get(1)); + VecBuilder.fill(1, Math.tan(-piecePos.get(0)), Math.tan(piecePos.get(1))); double maxDot = -1.0; AlgaePositions closestAlgae = null; for (int i = 0; i < ALGAES.length; i++) { @@ -85,9 +78,10 @@ public static AlgaePositions FindAlgae(Pose2d robotPos, Vector piecePos) { } public static BranchPositions UnitTest1() { - double x = -4.1902085050000 / 3.8337244850000 * SPECIAL_X + Constants.LIMELIGHT_HALF_POS.get(0); - double y = -0.706978325 / 3.8337244850000 * SPECIAL_Y + Constants.LIMELIGHT_HALF_POS.get(1); + double x = 120; + double y = 116.49122807017543859649; return FindCoralBranch( - new Pose2d(0, 0, new Rotation2d(0)), new Vector(VecBuilder.fill(x, y))); + new Pose2d(2.763724485, 4.1902085050000, new Rotation2d(0)), + new Vector(VecBuilder.fill(x, y))); } } diff --git a/src/main/java/frc/robot/commands/UnitTester.java b/src/main/java/frc/robot/commands/UnitTester.java index 9e70edff..99d00477 100644 --- a/src/main/java/frc/robot/commands/UnitTester.java +++ b/src/main/java/frc/robot/commands/UnitTester.java @@ -1,10 +1,13 @@ package frc.robot.commands; import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.utils.shuffleboard.SmartShuffleboard; public class UnitTester extends Command { @Override - public void execute() {} + public void execute() { + SmartShuffleboard.put("Branch", "Branch", FindCorrectBranchFromPos.UnitTest1().toString()); + } @Override public boolean isFinished() { diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java index 15a2aa65..ce85b575 100644 --- a/src/main/java/frc/robot/constants/GameConstants.java +++ b/src/main/java/frc/robot/constants/GameConstants.java @@ -228,7 +228,7 @@ public enum Mode { public static final Vector LIMELIGHT_HALF_POS = new Vector(VecBuilder.fill(240, 320).div(2)); public static final Transform3d CAMERA_TO_ROBOT = - new Transform3d(0.0, 0.0, 0.0, new Rotation3d(0.0, 0, 0.0)); // TODO Change Later + new Transform3d(0.0, 0.0, 0.55, new Rotation3d(0.0, 0, 0.0)); // TODO Change Later public static final Vector LIMELIGHT_HALF_FOV = new Vector(VecBuilder.fill(0.86742863824118179973, 1.04021623418862042785).div(2)); } From 49da7b6c9218b1deeda7573dacf9e393e06d8c81 Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Sat, 22 Mar 2025 18:27:43 -0400 Subject: [PATCH 43/99] fixed some sutff --- .../frc/robot/commands/FindCorrectBranchFromPos.java | 10 +++++----- src/main/java/frc/robot/constants/GameConstants.java | 7 ------- 2 files changed, 5 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java b/src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java index 2ab37a8d..6d3d22f7 100644 --- a/src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java +++ b/src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java @@ -27,13 +27,13 @@ public class FindCorrectBranchFromPos { } } - // PeicePos is in degrees tx,ty + // PeicePos is in Radians public static BranchPositions FindCoralBranch(Pose2d robotPos, Vector piecePos) { final Pose3d cameraPos = new Pose3d(robotPos).transformBy(Constants.CAMERA_TO_ROBOT); final Translation3d cameraPosVec = cameraPos.getTranslation(); final Rotation3d invCameraRotation = cameraPos.getRotation().unaryMinus(); final Vector pieceVec = - VecBuilder.fill(1, Math.tan(-piecePos.get(0)), Math.tan(piecePos.get(1))); + VecBuilder.fill(1, Math.tan(piecePos.get(1)), Math.tan(piecePos.get(0))); double maxDot = -1.0; BranchPositions closestBranch = null; for (int i = 0; i < BRANCHES.length; i++) { @@ -52,7 +52,7 @@ public static BranchPositions FindCoralBranch(Pose2d robotPos, Vector pieceP return closestBranch; } - // peicepos is in digrees tx,ty + // peicepos is in Radians public static AlgaePositions FindAlgae(Pose2d robotPos, Vector piecePos) { final Pose3d cameraPos = new Pose3d(robotPos).transformBy(Constants.CAMERA_TO_ROBOT); final Translation3d cameraPosVec = cameraPos.getTranslation(); @@ -78,8 +78,8 @@ public static AlgaePositions FindAlgae(Pose2d robotPos, Vector piecePos) { } public static BranchPositions UnitTest1() { - double x = 120; - double y = 116.49122807017543859649; + double x = 0.84151552345920325992; + double y = 0; return FindCoralBranch( new Pose2d(2.763724485, 4.1902085050000, new Rotation2d(0)), new Vector(VecBuilder.fill(x, y))); diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java index ce85b575..839ce57d 100644 --- a/src/main/java/frc/robot/constants/GameConstants.java +++ b/src/main/java/frc/robot/constants/GameConstants.java @@ -1,11 +1,8 @@ package frc.robot.constants; -import edu.wpi.first.math.VecBuilder; -import edu.wpi.first.math.Vector; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.math.numbers.N2; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.RobotBase; import frc.robot.utils.SwerveModuleProfileV2; @@ -225,10 +222,6 @@ public enum Mode { public static final double ELEVATOR_MANUAL_MAX_SPEED_DOWN = .15; // Limelight - public static final Vector LIMELIGHT_HALF_POS = - new Vector(VecBuilder.fill(240, 320).div(2)); public static final Transform3d CAMERA_TO_ROBOT = new Transform3d(0.0, 0.0, 0.55, new Rotation3d(0.0, 0, 0.0)); // TODO Change Later - public static final Vector LIMELIGHT_HALF_FOV = - new Vector(VecBuilder.fill(0.86742863824118179973, 1.04021623418862042785).div(2)); } From da456e916b1b9c92911d25f0cb99635692db2413 Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Sat, 22 Mar 2025 18:39:23 -0400 Subject: [PATCH 44/99] made code more maintanable and stuff --- .../commands/FindCorrectBranchFromPos.java | 51 +++++++------------ 1 file changed, 17 insertions(+), 34 deletions(-) diff --git a/src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java b/src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java index 6d3d22f7..85a908e5 100644 --- a/src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java +++ b/src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java @@ -29,52 +29,35 @@ public class FindCorrectBranchFromPos { // PeicePos is in Radians public static BranchPositions FindCoralBranch(Pose2d robotPos, Vector piecePos) { + return findClosestPosition(robotPos, piecePos, BRANCHES, PRECOMPUTED_BRANCH_VECS); + } + public static > T findClosestPosition (Pose2d robotPos, Vector piecePos, T[] values, Translation3d[] preComputedVecs) { final Pose3d cameraPos = new Pose3d(robotPos).transformBy(Constants.CAMERA_TO_ROBOT); final Translation3d cameraPosVec = cameraPos.getTranslation(); final Rotation3d invCameraRotation = cameraPos.getRotation().unaryMinus(); final Vector pieceVec = - VecBuilder.fill(1, Math.tan(piecePos.get(1)), Math.tan(piecePos.get(0))); + VecBuilder.fill(1, Math.tan(-piecePos.get(0)), Math.tan(piecePos.get(1))); double maxDot = -1.0; - BranchPositions closestBranch = null; - for (int i = 0; i < BRANCHES.length; i++) { - Vector branchVec = - PRECOMPUTED_BRANCH_VECS[i] - .minus(cameraPosVec) - .rotateBy(invCameraRotation) - .toVector() - .unit(); - double dot = pieceVec.dot(branchVec); + T closest = null; + for (int i = 0; i < values.length; i++) { + Vector locationVec = + preComputedVecs[i] + .minus(cameraPosVec) + .rotateBy(invCameraRotation) + .toVector() + .unit(); + double dot = pieceVec.dot(locationVec); if (dot > maxDot) { maxDot = dot; - closestBranch = BRANCHES[i]; + closest = values[i]; } } - return closestBranch; + return closest; } // peicepos is in Radians public static AlgaePositions FindAlgae(Pose2d robotPos, Vector piecePos) { - final Pose3d cameraPos = new Pose3d(robotPos).transformBy(Constants.CAMERA_TO_ROBOT); - final Translation3d cameraPosVec = cameraPos.getTranslation(); - final Rotation3d invCameraRotation = cameraPos.getRotation().unaryMinus(); - final Vector pieceVec = - VecBuilder.fill(1, Math.tan(-piecePos.get(0)), Math.tan(piecePos.get(1))); - double maxDot = -1.0; - AlgaePositions closestAlgae = null; - for (int i = 0; i < ALGAES.length; i++) { - Vector algaeVec = - PRECOMPUTED_ALGAE_VECS[i] - .minus(cameraPosVec) - .rotateBy(invCameraRotation) - .toVector() - .unit(); - double dot = pieceVec.dot(algaeVec); - if (dot > maxDot) { - maxDot = dot; - closestAlgae = ALGAES[i]; - } - } - return closestAlgae; + return findClosestPosition(robotPos, piecePos, ALGAES, PRECOMPUTED_ALGAE_VECS); } public static BranchPositions UnitTest1() { @@ -82,6 +65,6 @@ public static BranchPositions UnitTest1() { double y = 0; return FindCoralBranch( new Pose2d(2.763724485, 4.1902085050000, new Rotation2d(0)), - new Vector(VecBuilder.fill(x, y))); + VecBuilder.fill(x, y)); } } From 2e4df64398962fb6a7737ed362d000297cc3cb47 Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Sat, 22 Mar 2025 19:17:02 -0400 Subject: [PATCH 45/99] all linear algebra now --- .../commands/FindCorrectBranchFromPos.java | 29 +++++++++---------- 1 file changed, 14 insertions(+), 15 deletions(-) diff --git a/src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java b/src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java index 85a908e5..5fbfcc3b 100644 --- a/src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java +++ b/src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java @@ -1,8 +1,10 @@ package frc.robot.commands; +import edu.wpi.first.math.Matrix; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.Vector; import edu.wpi.first.math.geometry.*; +import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N2; import edu.wpi.first.math.numbers.N3; import frc.robot.constants.AlgaePositions; @@ -12,18 +14,18 @@ public class FindCorrectBranchFromPos { private static final BranchPositions[] BRANCHES = BranchPositions.values(); private static final AlgaePositions[] ALGAES = AlgaePositions.values(); - private static final Translation3d[] PRECOMPUTED_BRANCH_VECS; - private static final Translation3d[] PRECOMPUTED_ALGAE_VECS; + private static final Vector[] PRECOMPUTED_BRANCH_VECS; + private static final Vector[] PRECOMPUTED_ALGAE_VECS; static { - PRECOMPUTED_BRANCH_VECS = new Translation3d[BRANCHES.length]; + PRECOMPUTED_BRANCH_VECS = new Vector[BRANCHES.length]; for (int i = 0; i < BRANCHES.length; i++) { - PRECOMPUTED_BRANCH_VECS[i] = BRANCHES[i].getPosition().getTranslation(); + PRECOMPUTED_BRANCH_VECS[i] = BRANCHES[i].getPosition().getTranslation().toVector(); } - PRECOMPUTED_ALGAE_VECS = new Translation3d[ALGAES.length]; + PRECOMPUTED_ALGAE_VECS = new Vector[ALGAES.length]; for (int i = 0; i < ALGAES.length; i++) { - PRECOMPUTED_ALGAE_VECS[i] = ALGAES[i].getPosition().getTranslation(); + PRECOMPUTED_ALGAE_VECS[i] = ALGAES[i].getPosition().getTranslation().toVector(); } } @@ -31,22 +33,19 @@ public class FindCorrectBranchFromPos { public static BranchPositions FindCoralBranch(Pose2d robotPos, Vector piecePos) { return findClosestPosition(robotPos, piecePos, BRANCHES, PRECOMPUTED_BRANCH_VECS); } - public static > T findClosestPosition (Pose2d robotPos, Vector piecePos, T[] values, Translation3d[] preComputedVecs) { + public static > T findClosestPosition (Pose2d robotPos, Vector piecePos, T[] values, Vector[] preComputedVecs) { final Pose3d cameraPos = new Pose3d(robotPos).transformBy(Constants.CAMERA_TO_ROBOT); - final Translation3d cameraPosVec = cameraPos.getTranslation(); - final Rotation3d invCameraRotation = cameraPos.getRotation().unaryMinus(); + final Vector cameraPosVec = cameraPos.getTranslation().toVector(); + final Matrix invCameraRotation = cameraPos.getRotation().unaryMinus().toMatrix(); final Vector pieceVec = VecBuilder.fill(1, Math.tan(-piecePos.get(0)), Math.tan(piecePos.get(1))); double maxDot = -1.0; T closest = null; for (int i = 0; i < values.length; i++) { - Vector locationVec = + Matrix locationVec =(invCameraRotation.times( preComputedVecs[i] - .minus(cameraPosVec) - .rotateBy(invCameraRotation) - .toVector() - .unit(); - double dot = pieceVec.dot(locationVec); + .minus(cameraPosVec))); + double dot = pieceVec.dot((Vector) locationVec); if (dot > maxDot) { maxDot = dot; closest = values[i]; From f64921482b2d891d6a9e0c725de4d644b22212e2 Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Sat, 22 Mar 2025 19:17:49 -0400 Subject: [PATCH 46/99] linter --- .../robot/commands/FindCorrectBranchFromPos.java | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java b/src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java index 5fbfcc3b..9654b4c4 100644 --- a/src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java +++ b/src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java @@ -33,18 +33,19 @@ public class FindCorrectBranchFromPos { public static BranchPositions FindCoralBranch(Pose2d robotPos, Vector piecePos) { return findClosestPosition(robotPos, piecePos, BRANCHES, PRECOMPUTED_BRANCH_VECS); } - public static > T findClosestPosition (Pose2d robotPos, Vector piecePos, T[] values, Vector[] preComputedVecs) { + + public static > T findClosestPosition( + Pose2d robotPos, Vector piecePos, T[] values, Vector[] preComputedVecs) { final Pose3d cameraPos = new Pose3d(robotPos).transformBy(Constants.CAMERA_TO_ROBOT); final Vector cameraPosVec = cameraPos.getTranslation().toVector(); final Matrix invCameraRotation = cameraPos.getRotation().unaryMinus().toMatrix(); final Vector pieceVec = - VecBuilder.fill(1, Math.tan(-piecePos.get(0)), Math.tan(piecePos.get(1))); + VecBuilder.fill(1, Math.tan(-piecePos.get(0)), Math.tan(piecePos.get(1))); double maxDot = -1.0; T closest = null; for (int i = 0; i < values.length; i++) { - Matrix locationVec =(invCameraRotation.times( - preComputedVecs[i] - .minus(cameraPosVec))); + Matrix locationVec = + (invCameraRotation.times(preComputedVecs[i].minus(cameraPosVec))); double dot = pieceVec.dot((Vector) locationVec); if (dot > maxDot) { maxDot = dot; @@ -63,7 +64,6 @@ public static BranchPositions UnitTest1() { double x = 0.84151552345920325992; double y = 0; return FindCoralBranch( - new Pose2d(2.763724485, 4.1902085050000, new Rotation2d(0)), - VecBuilder.fill(x, y)); + new Pose2d(2.763724485, 4.1902085050000, new Rotation2d(0)), VecBuilder.fill(x, y)); } } From fd5830a42c775bd21e42411752558b1581a02424 Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Sat, 22 Mar 2025 19:26:08 -0400 Subject: [PATCH 47/99] more generilization --- .../commands/FindCorrectBranchFromPos.java | 27 +++++++++++-------- 1 file changed, 16 insertions(+), 11 deletions(-) diff --git a/src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java b/src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java index 9654b4c4..d7b91b6a 100644 --- a/src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java +++ b/src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java @@ -14,19 +14,24 @@ public class FindCorrectBranchFromPos { private static final BranchPositions[] BRANCHES = BranchPositions.values(); private static final AlgaePositions[] ALGAES = AlgaePositions.values(); - private static final Vector[] PRECOMPUTED_BRANCH_VECS; - private static final Vector[] PRECOMPUTED_ALGAE_VECS; + // Precomputed vectors for positions + private static final Vector[] PRECOMPUTED_BRANCH_VECS = precomputePositionVectors(BRANCHES); + private static final Vector[] PRECOMPUTED_ALGAE_VECS = precomputePositionVectors(ALGAES); - static { - PRECOMPUTED_BRANCH_VECS = new Vector[BRANCHES.length]; - for (int i = 0; i < BRANCHES.length; i++) { - PRECOMPUTED_BRANCH_VECS[i] = BRANCHES[i].getPosition().getTranslation().toVector(); - } - - PRECOMPUTED_ALGAE_VECS = new Vector[ALGAES.length]; - for (int i = 0; i < ALGAES.length; i++) { - PRECOMPUTED_ALGAE_VECS[i] = ALGAES[i].getPosition().getTranslation().toVector(); + // Precompute position vectors for any enum type with a getPosition method + @SuppressWarnings("unchecked") + private static > Vector[] precomputePositionVectors(T[] values) { + Vector[] vectors = new Vector[values.length]; + for (int i = 0; i < values.length; i++) { + // Using reflection to access getPosition method - assumes the method exists on the enum + try { + Pose3d pose = (Pose3d) values[i].getClass().getMethod("getPosition").invoke(values[i]); + vectors[i] = pose.getTranslation().toVector(); + } catch (Exception e) { + throw new RuntimeException("Failed to precompute position vectors", e); + } } + return vectors; } // PeicePos is in Radians From 129217270a33cbe21aaa42766e4e096d21222866 Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Sat, 22 Mar 2025 19:44:44 -0400 Subject: [PATCH 48/99] EA SPORTS --- .../java/frc/robot/commands/FindCorrectBranchFromPos.java | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java b/src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java index d7b91b6a..3b1bcb7b 100644 --- a/src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java +++ b/src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java @@ -45,13 +45,16 @@ public static > T findClosestPosition( final Vector cameraPosVec = cameraPos.getTranslation().toVector(); final Matrix invCameraRotation = cameraPos.getRotation().unaryMinus().toMatrix(); final Vector pieceVec = - VecBuilder.fill(1, Math.tan(-piecePos.get(0)), Math.tan(piecePos.get(1))); + VecBuilder.fill(1, Math.tan(-piecePos.get(1)), Math.tan(piecePos.get(0))); double maxDot = -1.0; T closest = null; for (int i = 0; i < values.length; i++) { Matrix locationVec = (invCameraRotation.times(preComputedVecs[i].minus(cameraPosVec))); - double dot = pieceVec.dot((Vector) locationVec); + double dot = + pieceVec.dot( + VecBuilder.fill(locationVec.get(0, 0), locationVec.get(1, 0), locationVec.get(2, 0)) + .unit()); if (dot > maxDot) { maxDot = dot; closest = values[i]; From 47fe8e05c29971a77d6e621851b4f138f00735df Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Sun, 23 Mar 2025 11:27:34 -0400 Subject: [PATCH 49/99] fixed --- .../frc/robot/commands/FindCorrectBranchFromPos.java | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java b/src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java index 3b1bcb7b..cdd2a03e 100644 --- a/src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java +++ b/src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java @@ -45,7 +45,7 @@ public static > T findClosestPosition( final Vector cameraPosVec = cameraPos.getTranslation().toVector(); final Matrix invCameraRotation = cameraPos.getRotation().unaryMinus().toMatrix(); final Vector pieceVec = - VecBuilder.fill(1, Math.tan(-piecePos.get(1)), Math.tan(piecePos.get(0))); + VecBuilder.fill(1, Math.tan(piecePos.get(1)), Math.tan(piecePos.get(0))); double maxDot = -1.0; T closest = null; for (int i = 0; i < values.length; i++) { @@ -68,10 +68,10 @@ public static AlgaePositions FindAlgae(Pose2d robotPos, Vector piecePos) { return findClosestPosition(robotPos, piecePos, ALGAES, PRECOMPUTED_ALGAE_VECS); } - public static BranchPositions UnitTest1() { - double x = 0.84151552345920325992; - double y = 0; - return FindCoralBranch( - new Pose2d(2.763724485, 4.1902085050000, new Rotation2d(0)), VecBuilder.fill(x, y)); + public static AlgaePositions UnitTest1() { + double x = 0.31119220563058896357; + double y = 0.06719517620178168871; + return FindAlgae( + new Pose2d(1.453724485, 3.8615914950000, new Rotation2d(0)), VecBuilder.fill(x, y)); } } From a3024518a625aa7fadfbc7639ae318262303a23a Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Sun, 23 Mar 2025 12:22:02 -0400 Subject: [PATCH 50/99] added limelight --- .../frc/robot/constants/GameConstants.java | 1 + .../subsystems/limelight/MockVisionIO.java | 6 ++ .../subsystems/limelight/RealVisionIO.java | 56 +++++++++++++++++++ .../robot/subsystems/limelight/Vision.java | 53 ++++++++++++++++++ .../robot/subsystems/limelight/VisionIO.java | 5 ++ .../subsystems/limelight/VisionInputs.java | 40 +++++++++++++ .../frc/robot/utils/diag/DiagLimelight.java | 47 ++++++++++++++++ 7 files changed, 208 insertions(+) create mode 100644 src/main/java/frc/robot/subsystems/limelight/MockVisionIO.java create mode 100644 src/main/java/frc/robot/subsystems/limelight/RealVisionIO.java create mode 100644 src/main/java/frc/robot/subsystems/limelight/Vision.java create mode 100644 src/main/java/frc/robot/subsystems/limelight/VisionIO.java create mode 100644 src/main/java/frc/robot/subsystems/limelight/VisionInputs.java create mode 100644 src/main/java/frc/robot/utils/diag/DiagLimelight.java diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java index 839ce57d..0c648888 100644 --- a/src/main/java/frc/robot/constants/GameConstants.java +++ b/src/main/java/frc/robot/constants/GameConstants.java @@ -224,4 +224,5 @@ public enum Mode { // Limelight public static final Transform3d CAMERA_TO_ROBOT = new Transform3d(0.0, 0.0, 0.55, new Rotation3d(0.0, 0, 0.0)); // TODO Change Later + public static final String LIMELIGHT_IP_ADDRESS = "10.40.48.36"; // TODO Change Later } diff --git a/src/main/java/frc/robot/subsystems/limelight/MockVisionIO.java b/src/main/java/frc/robot/subsystems/limelight/MockVisionIO.java new file mode 100644 index 00000000..634f7193 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/limelight/MockVisionIO.java @@ -0,0 +1,6 @@ +package frc.robot.subsystems.limelight; + +public class MockVisionIO implements VisionIO { + @Override + public void updateInputs(VisionInputs inputs) {} +} diff --git a/src/main/java/frc/robot/subsystems/limelight/RealVisionIO.java b/src/main/java/frc/robot/subsystems/limelight/RealVisionIO.java new file mode 100644 index 00000000..a7c00a70 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/limelight/RealVisionIO.java @@ -0,0 +1,56 @@ +package frc.robot.subsystems.limelight; + +import edu.wpi.first.cscore.HttpCamera; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableEntry; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; +import frc.robot.Robot; +import frc.robot.commands.FindCorrectBranchFromPos; +import frc.robot.constants.Constants; +import frc.robot.subsystems.swervev3.SwerveDrivetrain; +import frc.robot.utils.diag.DiagLimelight; +import java.util.Map; + +public class RealVisionIO implements VisionIO { + private final NetworkTableEntry tv; + private final NetworkTableEntry tx; + private final NetworkTableEntry ty; + private final SwerveDrivetrain drivetrain; + + public RealVisionIO(SwerveDrivetrain drivetrain) { + this.drivetrain = drivetrain; + NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight"); + tv = table.getEntry("tv"); + tx = table.getEntry("tx"); + ty = table.getEntry("ty"); + Robot.getDiagnostics().addDiagnosable(new DiagLimelight("Vision", "Piece Seen")); + + HttpCamera limelightFeed = + new HttpCamera( + "limelight", "http://" + Constants.LIMELIGHT_IP_ADDRESS + ":5800/stream.mjpg"); + ShuffleboardTab dashboardTab = Shuffleboard.getTab("Driver"); + dashboardTab + .add("Limelight feed", limelightFeed) + .withSize(6, 4) + .withPosition(2, 0) + .withProperties(Map.of("Show Crosshair", false, "Show Controls", false)); + } + + @Override + public void updateInputs(VisionInputs inputs) { + inputs.tv = tv.getDouble(0); + inputs.tx = tx.getDouble(0); + inputs.ty = ty.getDouble(0); + if (inputs.tv != 0) { + inputs.coralSeen.add( + FindCorrectBranchFromPos.FindCoralBranch( + drivetrain.getPose(), VecBuilder.fill(tx.getDouble(0), ty.getDouble(0)))); + inputs.algaeSeen.add( + FindCorrectBranchFromPos.FindAlgae( + drivetrain.getPose(), VecBuilder.fill(tx.getDouble(0), ty.getDouble(0)))); + } + } +} diff --git a/src/main/java/frc/robot/subsystems/limelight/Vision.java b/src/main/java/frc/robot/subsystems/limelight/Vision.java new file mode 100644 index 00000000..14a8432b --- /dev/null +++ b/src/main/java/frc/robot/subsystems/limelight/Vision.java @@ -0,0 +1,53 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems.limelight; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.constants.AlgaePositions; +import frc.robot.constants.BranchPositions; +import frc.robot.subsystems.swervev3.SwerveDrivetrain; +import frc.robot.utils.logging.subsystem.LoggableSystem; +import java.util.ArrayList; + +public class Vision extends SubsystemBase { + LoggableSystem system; + SwerveDrivetrain drivetrain; + + public Vision(VisionIO io, SwerveDrivetrain drivetrain) { + system = new LoggableSystem<>(io, new VisionInputs("limelight")); + this.drivetrain = drivetrain; + } + + /** + * @return The piece's x offset angle in degrees and 0.0 if the piece isn't seen + */ + public double getPieceOffsetAngleX() { + return system.getInputs().tx; + } + + /** + * @return The piece's y offset angle in degrees and 0.0 if the piece isn't seen + */ + public double getPieceOffsetAngleY() { + return system.getInputs().ty; + } + + public boolean isPieceSeen() { + return system.getInputs().tv != 0; + } + + public ArrayList getAllBranchPosition() { + return system.getInputs().coralSeen; + } + + public ArrayList getAllAlgaePosition() { + return system.getInputs().algaeSeen; + } + + @Override + public void periodic() { + system.updateInputs(); + } +} diff --git a/src/main/java/frc/robot/subsystems/limelight/VisionIO.java b/src/main/java/frc/robot/subsystems/limelight/VisionIO.java new file mode 100644 index 00000000..990d52a4 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/limelight/VisionIO.java @@ -0,0 +1,5 @@ +package frc.robot.subsystems.limelight; + +import frc.robot.utils.logging.LoggableIO; + +public interface VisionIO extends LoggableIO {} diff --git a/src/main/java/frc/robot/subsystems/limelight/VisionInputs.java b/src/main/java/frc/robot/subsystems/limelight/VisionInputs.java new file mode 100644 index 00000000..c06e640f --- /dev/null +++ b/src/main/java/frc/robot/subsystems/limelight/VisionInputs.java @@ -0,0 +1,40 @@ +package frc.robot.subsystems.limelight; + +import frc.robot.constants.AlgaePositions; +import frc.robot.constants.BranchPositions; +import frc.robot.utils.logging.subsystem.FolderLoggableInputs; +import java.util.ArrayList; +import java.util.List; +import org.littletonrobotics.junction.LogTable; + +public class VisionInputs extends FolderLoggableInputs { + public double tv = 0; + public double tx = 0; + public double ty = 0; + public ArrayList coralSeen; + public ArrayList algaeSeen; + + public VisionInputs(String folder) { + super(folder); + } + + @Override + public void toLog(LogTable table) { + table.put("tv", tv); + table.put("tx", tx); + table.put("ty", ty); + table.put("coralSeen", coralSeen.toArray(BranchPositions[]::new)); + table.put("algaeSeen", algaeSeen.toArray(AlgaePositions[]::new)); + } + + @Override + public void fromLog(LogTable table) { + tv = table.get("tv", tv); + tx = table.get("tx", tx); + ty = table.get("ty", ty); + coralSeen = + new ArrayList<>(List.of(table.get("coralSeen", coralSeen.toArray(BranchPositions[]::new)))); + algaeSeen = + new ArrayList<>(List.of(table.get("algaeSeen", algaeSeen.toArray(AlgaePositions[]::new)))); + } +} diff --git a/src/main/java/frc/robot/utils/diag/DiagLimelight.java b/src/main/java/frc/robot/utils/diag/DiagLimelight.java new file mode 100644 index 00000000..ad4fbdd6 --- /dev/null +++ b/src/main/java/frc/robot/utils/diag/DiagLimelight.java @@ -0,0 +1,47 @@ +package frc.robot.utils.diag; + +import edu.wpi.first.networktables.DoubleSubscriber; +import edu.wpi.first.networktables.GenericEntry; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; + +public class DiagLimelight implements Diagnosable { + private DoubleSubscriber subscriber; + private String title; + private String name; + private GenericEntry networkTableEntry; + + public DiagLimelight(String title, String name) { + this.title = title; + this.name = name; + reset(); + } + + protected boolean getDiagResult() { + return subscriber.get() == 1; + } + + @Override + public void setShuffleBoardTab(ShuffleboardTab shuffleBoardTab, int width, int height) { + networkTableEntry = + shuffleBoardTab + .getLayout(title, BuiltInLayouts.kList) + .withSize(width, height) + .add(name, false) + .getEntry(); + } + + @Override + public void refresh() { + if (networkTableEntry != null) { + networkTableEntry.setBoolean(getDiagResult()); + } + } + + @Override + public void reset() { + subscriber = + NetworkTableInstance.getDefault().getTable("limelight").getDoubleTopic("tv").subscribe(-1); + } +} From f269cfd672f4410f2727c0107d0d1e71b390930c Mon Sep 17 00:00:00 2001 From: jamthepersonj Date: Sun, 23 Mar 2025 14:09:13 -0400 Subject: [PATCH 51/99] update --- .../drivetrain/RobotCentricDrive.java | 22 ++++++++++--------- 1 file changed, 12 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/commands/drivetrain/RobotCentricDrive.java b/src/main/java/frc/robot/commands/drivetrain/RobotCentricDrive.java index e84253c8..007952bd 100644 --- a/src/main/java/frc/robot/commands/drivetrain/RobotCentricDrive.java +++ b/src/main/java/frc/robot/commands/drivetrain/RobotCentricDrive.java @@ -4,26 +4,25 @@ package frc.robot.commands.drivetrain; -import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.kinematics.ChassisSpeeds; -import frc.robot.constants.Constants; +import edu.wpi.first.wpilibj.Timer; import frc.robot.subsystems.swervev3.SwerveDrivetrain; import frc.robot.utils.DriveMode; import frc.robot.utils.logging.commands.LoggableCommand; -import java.util.function.DoubleSupplier; public class RobotCentricDrive extends LoggableCommand { private final SwerveDrivetrain drivetrain; private final double xMove; - private final double yMove; + private final double time; + private final Timer timer; public RobotCentricDrive( SwerveDrivetrain drivetrain, - double xMove, - double yMove) { + double xMove, double time) { this.drivetrain = drivetrain; this.xMove = xMove; - this.yMove = yMove; + this.time = time; + timer = new Timer(); addRequirements(drivetrain); } @@ -32,15 +31,18 @@ public void initialize() {} @Override public void execute() { - ChassisSpeeds speeds = drivetrain.createChassisSpeeds(xMove, yMove, 0, DriveMode.ROBOT_CENTRIC); + ChassisSpeeds speeds = drivetrain.createChassisSpeeds(xMove, 0, 0, DriveMode.ROBOT_CENTRIC); drivetrain.drive(speeds); } @Override - public void end(boolean interrupted) {} + public void end(boolean interrupted) { + ChassisSpeeds speeds = drivetrain.createChassisSpeeds(0, 0, 0, DriveMode.FIELD_CENTRIC); + drivetrain.drive(speeds); + } @Override public boolean isFinished() { - return false; + return timer.hasElapsed(time); } } From 098687a21219f890ef831737b75f273ff54d7ef9 Mon Sep 17 00:00:00 2001 From: codetoad Date: Sun, 23 Mar 2025 14:11:27 -0400 Subject: [PATCH 52/99] added constant vision trustor --- .../swervev3/estimation/PoseEstimator.java | 6 +++--- .../swervev3/vision/ConstantVisionTruster.java | 18 ++++++++++++++++++ 2 files changed, 21 insertions(+), 3 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/swervev3/vision/ConstantVisionTruster.java diff --git a/src/main/java/frc/robot/subsystems/swervev3/estimation/PoseEstimator.java b/src/main/java/frc/robot/subsystems/swervev3/estimation/PoseEstimator.java index 7a0c2c60..fe734335 100644 --- a/src/main/java/frc/robot/subsystems/swervev3/estimation/PoseEstimator.java +++ b/src/main/java/frc/robot/subsystems/swervev3/estimation/PoseEstimator.java @@ -19,7 +19,7 @@ import frc.robot.subsystems.swervev3.bags.VisionMeasurement; import frc.robot.subsystems.swervev3.io.SwerveModule; import frc.robot.subsystems.swervev3.vision.BasicVisionFilter; -import frc.robot.subsystems.swervev3.vision.SquareVisionTruster; +import frc.robot.subsystems.swervev3.vision.ConstantVisionTruster; import frc.robot.utils.Apriltag; import frc.robot.utils.RobotMode; import frc.robot.utils.logging.LoggableIO; @@ -52,7 +52,7 @@ public class PoseEstimator { private static final double visionStdRateOfChange = 1; /* standard deviation of vision readings, the lower the numbers arm, the more we trust vision */ - private static final Vector visionMeasurementStdDevs2 = VecBuilder.fill(0.45, 0.45, 0.01); + private static final Vector visionMeasurementStdDevs2 = VecBuilder.fill(0.1, 0.1, 0.01); private final FilterablePoseManager poseManager; public PoseEstimator( @@ -92,7 +92,7 @@ public Pose2d getVisionPose(VisionMeasurement measurement) { return measurement.measurement(); } }, - new SquareVisionTruster(visionMeasurementStdDevs2, visionStdRateOfChange)); + new ConstantVisionTruster(visionMeasurementStdDevs2)); SmartDashboard.putData(field); } diff --git a/src/main/java/frc/robot/subsystems/swervev3/vision/ConstantVisionTruster.java b/src/main/java/frc/robot/subsystems/swervev3/vision/ConstantVisionTruster.java new file mode 100644 index 00000000..eab3ee90 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/swervev3/vision/ConstantVisionTruster.java @@ -0,0 +1,18 @@ +package frc.robot.subsystems.swervev3.vision; + +import edu.wpi.first.math.Vector; +import edu.wpi.first.math.numbers.N3; +import frc.robot.subsystems.swervev3.bags.VisionMeasurement; + +public class ConstantVisionTruster extends DistanceVisionTruster { + + public ConstantVisionTruster(Vector initialSTD) { + super(initialSTD); + this.initialSTD = initialSTD; + } + + @Override + public Vector calculateTrust(VisionMeasurement measurement) { + return initialSTD; + } +} From ee891f5df0a1bf8ef1446ebce2a5774eab00b607 Mon Sep 17 00:00:00 2001 From: codetoad Date: Sun, 23 Mar 2025 15:54:13 -0400 Subject: [PATCH 53/99] fixes --- .../frc/robot/commands/drivetrain/RobotCentricDrive.java | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/commands/drivetrain/RobotCentricDrive.java b/src/main/java/frc/robot/commands/drivetrain/RobotCentricDrive.java index 007952bd..126de34e 100644 --- a/src/main/java/frc/robot/commands/drivetrain/RobotCentricDrive.java +++ b/src/main/java/frc/robot/commands/drivetrain/RobotCentricDrive.java @@ -16,9 +16,7 @@ public class RobotCentricDrive extends LoggableCommand { private final double time; private final Timer timer; - public RobotCentricDrive( - SwerveDrivetrain drivetrain, - double xMove, double time) { + public RobotCentricDrive(SwerveDrivetrain drivetrain, double xMove, double time) { this.drivetrain = drivetrain; this.xMove = xMove; this.time = time; @@ -27,7 +25,9 @@ public RobotCentricDrive( } @Override - public void initialize() {} + public void initialize() { + timer.restart(); + } @Override public void execute() { From baca8c98f45c979014c62b213a8d26c72ecb0b74 Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Sun, 23 Mar 2025 16:39:46 -0400 Subject: [PATCH 54/99] small changes --- src/main/java/frc/robot/RobotContainer.java | 4 ++++ .../frc/robot/commands/FindCorrectBranchFromPos.java | 5 ++--- src/main/java/frc/robot/constants/GameConstants.java | 7 ++++--- .../java/frc/robot/subsystems/limelight/Vision.java | 12 ++++++++---- .../frc/robot/subsystems/limelight/VisionInputs.java | 8 ++++++-- 5 files changed, 24 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 51287d68..363b91d4 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -63,6 +63,8 @@ import frc.robot.subsystems.lightStrip.LightStrip; import frc.robot.subsystems.lightStrip.MockLightStripIO; import frc.robot.subsystems.lightStrip.RealLightStripIO; +import frc.robot.subsystems.limelight.RealVisionIO; +import frc.robot.subsystems.limelight.Vision; import frc.robot.subsystems.swervev3.KinematicsConversionConfig; import frc.robot.subsystems.swervev3.SwerveDrivetrain; import frc.robot.subsystems.swervev3.SwerveIdConfig; @@ -105,6 +107,7 @@ public class RobotContainer { private final Joystick joyright = new Joystick(Constants.RIGHT_JOYSTICK_ID); private RobotVisualizer robotVisualizer = null; private SwerveDriveSimulation driveSimulation; + private final Vision vision; public RobotContainer() { switch (Constants.currentMode) { @@ -163,6 +166,7 @@ public RobotContainer() { } } setupDriveTrain(); + vision = new Vision(new RealVisionIO(drivetrain)); configureBindings(); setupAutoChooser(); putShuffleboardCommands(); diff --git a/src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java b/src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java index cdd2a03e..e76b8e7a 100644 --- a/src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java +++ b/src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java @@ -45,7 +45,7 @@ public static > T findClosestPosition( final Vector cameraPosVec = cameraPos.getTranslation().toVector(); final Matrix invCameraRotation = cameraPos.getRotation().unaryMinus().toMatrix(); final Vector pieceVec = - VecBuilder.fill(1, Math.tan(piecePos.get(1)), Math.tan(piecePos.get(0))); + VecBuilder.fill(1, -Math.tan(piecePos.get(1)), -Math.tan(piecePos.get(0))); double maxDot = -1.0; T closest = null; for (int i = 0; i < values.length; i++) { @@ -71,7 +71,6 @@ public static AlgaePositions FindAlgae(Pose2d robotPos, Vector piecePos) { public static AlgaePositions UnitTest1() { double x = 0.31119220563058896357; double y = 0.06719517620178168871; - return FindAlgae( - new Pose2d(1.453724485, 3.8615914950000, new Rotation2d(0)), VecBuilder.fill(x, y)); + return FindAlgae(new Pose2d(2.614524485, 4.0259, new Rotation2d(0)), VecBuilder.fill(x, y)); } } diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java index 0c648888..1d05fd37 100644 --- a/src/main/java/frc/robot/constants/GameConstants.java +++ b/src/main/java/frc/robot/constants/GameConstants.java @@ -90,7 +90,7 @@ public class GameConstants { public static final int HIHI_EXTENDER_TICK_LIMIT = 10; // Mode - public static final Mode simMode = Mode.SIM; + public static final Mode simMode = Mode.REAL; public static final Mode currentMode = RobotBase.isReal() ? Mode.REAL : simMode; public enum Mode { @@ -223,6 +223,7 @@ public enum Mode { // Limelight public static final Transform3d CAMERA_TO_ROBOT = - new Transform3d(0.0, 0.0, 0.55, new Rotation3d(0.0, 0, 0.0)); // TODO Change Later - public static final String LIMELIGHT_IP_ADDRESS = "10.40.48.36"; // TODO Change Later + new Transform3d( + 0.0, 0.0, 0.720725, new Rotation3d(0.0, Math.PI / 6, 0.0)); // TODO Change Later + public static final String LIMELIGHT_IP_ADDRESS = "10.40.48.104"; // TODO Change Later } diff --git a/src/main/java/frc/robot/subsystems/limelight/Vision.java b/src/main/java/frc/robot/subsystems/limelight/Vision.java index 14a8432b..786636db 100644 --- a/src/main/java/frc/robot/subsystems/limelight/Vision.java +++ b/src/main/java/frc/robot/subsystems/limelight/Vision.java @@ -7,17 +7,15 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.constants.AlgaePositions; import frc.robot.constants.BranchPositions; -import frc.robot.subsystems.swervev3.SwerveDrivetrain; import frc.robot.utils.logging.subsystem.LoggableSystem; +import frc.robot.utils.shuffleboard.SmartShuffleboard; import java.util.ArrayList; public class Vision extends SubsystemBase { LoggableSystem system; - SwerveDrivetrain drivetrain; - public Vision(VisionIO io, SwerveDrivetrain drivetrain) { + public Vision(VisionIO io) { system = new LoggableSystem<>(io, new VisionInputs("limelight")); - this.drivetrain = drivetrain; } /** @@ -49,5 +47,11 @@ public ArrayList getAllAlgaePosition() { @Override public void periodic() { system.updateInputs(); + if (getAllAlgaePosition() != null) { + SmartShuffleboard.put("peice pos", "Algae", getAllAlgaePosition()); + } + if (getAllBranchPosition() != null) { + SmartShuffleboard.put("peice pos", "Coral", getAllBranchPosition()); + } } } diff --git a/src/main/java/frc/robot/subsystems/limelight/VisionInputs.java b/src/main/java/frc/robot/subsystems/limelight/VisionInputs.java index c06e640f..908d6eec 100644 --- a/src/main/java/frc/robot/subsystems/limelight/VisionInputs.java +++ b/src/main/java/frc/robot/subsystems/limelight/VisionInputs.java @@ -23,8 +23,12 @@ public void toLog(LogTable table) { table.put("tv", tv); table.put("tx", tx); table.put("ty", ty); - table.put("coralSeen", coralSeen.toArray(BranchPositions[]::new)); - table.put("algaeSeen", algaeSeen.toArray(AlgaePositions[]::new)); + if (coralSeen != null) { + table.put("coralSeen", coralSeen.toArray(BranchPositions[]::new)); + } + if (algaeSeen != null) { + table.put("algaeSeen", algaeSeen.toArray(AlgaePositions[]::new)); + } } @Override From a2cdbdcec8c901211b3b2ef909976956c287c944 Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Sun, 23 Mar 2025 18:59:40 -0400 Subject: [PATCH 55/99] fixed reflecton issue --- ...ranchFromPos.java => GamePieceLocate.java} | 36 ++++++++----------- .../java/frc/robot/commands/UnitTester.java | 2 +- .../subsystems/limelight/RealVisionIO.java | 6 ++-- 3 files changed, 19 insertions(+), 25 deletions(-) rename src/main/java/frc/robot/commands/{FindCorrectBranchFromPos.java => GamePieceLocate.java} (62%) diff --git a/src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java b/src/main/java/frc/robot/commands/GamePieceLocate.java similarity index 62% rename from src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java rename to src/main/java/frc/robot/commands/GamePieceLocate.java index e76b8e7a..3fc056c8 100644 --- a/src/main/java/frc/robot/commands/FindCorrectBranchFromPos.java +++ b/src/main/java/frc/robot/commands/GamePieceLocate.java @@ -11,31 +11,25 @@ import frc.robot.constants.BranchPositions; import frc.robot.constants.Constants; -public class FindCorrectBranchFromPos { +import java.util.Arrays; + +public class GamePieceLocate { private static final BranchPositions[] BRANCHES = BranchPositions.values(); private static final AlgaePositions[] ALGAES = AlgaePositions.values(); // Precomputed vectors for positions - private static final Vector[] PRECOMPUTED_BRANCH_VECS = precomputePositionVectors(BRANCHES); - private static final Vector[] PRECOMPUTED_ALGAE_VECS = precomputePositionVectors(ALGAES); + private static final Vector[] PRECOMPUTED_BRANCH_VECS = Arrays.stream(BranchPositions.values()) + .map(branch -> branch.getPosition().getTranslation().toVector()) + .toArray(Vector[]::new);; + + private static final Vector[] PRECOMPUTED_ALGAE_VECS = Arrays.stream(AlgaePositions.values()) + .map(algae -> algae.getPosition().getTranslation().toVector()) + .toArray(Vector[]::new); + + - // Precompute position vectors for any enum type with a getPosition method - @SuppressWarnings("unchecked") - private static > Vector[] precomputePositionVectors(T[] values) { - Vector[] vectors = new Vector[values.length]; - for (int i = 0; i < values.length; i++) { - // Using reflection to access getPosition method - assumes the method exists on the enum - try { - Pose3d pose = (Pose3d) values[i].getClass().getMethod("getPosition").invoke(values[i]); - vectors[i] = pose.getTranslation().toVector(); - } catch (Exception e) { - throw new RuntimeException("Failed to precompute position vectors", e); - } - } - return vectors; - } // PeicePos is in Radians - public static BranchPositions FindCoralBranch(Pose2d robotPos, Vector piecePos) { + public static BranchPositions findCoralBranch(Pose2d robotPos, Vector piecePos) { return findClosestPosition(robotPos, piecePos, BRANCHES, PRECOMPUTED_BRANCH_VECS); } @@ -64,13 +58,13 @@ public static > T findClosestPosition( } // peicepos is in Radians - public static AlgaePositions FindAlgae(Pose2d robotPos, Vector piecePos) { + public static AlgaePositions findAlgaePos(Pose2d robotPos, Vector piecePos) { return findClosestPosition(robotPos, piecePos, ALGAES, PRECOMPUTED_ALGAE_VECS); } public static AlgaePositions UnitTest1() { double x = 0.31119220563058896357; double y = 0.06719517620178168871; - return FindAlgae(new Pose2d(2.614524485, 4.0259, new Rotation2d(0)), VecBuilder.fill(x, y)); + return findAlgaePos(new Pose2d(2.614524485, 4.0259, new Rotation2d(0)), VecBuilder.fill(x, y)); } } diff --git a/src/main/java/frc/robot/commands/UnitTester.java b/src/main/java/frc/robot/commands/UnitTester.java index 99d00477..1ba6c7cc 100644 --- a/src/main/java/frc/robot/commands/UnitTester.java +++ b/src/main/java/frc/robot/commands/UnitTester.java @@ -6,7 +6,7 @@ public class UnitTester extends Command { @Override public void execute() { - SmartShuffleboard.put("Branch", "Branch", FindCorrectBranchFromPos.UnitTest1().toString()); + SmartShuffleboard.put("Branch", "Branch", GamePieceLocate.UnitTest1().toString()); } @Override diff --git a/src/main/java/frc/robot/subsystems/limelight/RealVisionIO.java b/src/main/java/frc/robot/subsystems/limelight/RealVisionIO.java index a7c00a70..04692537 100644 --- a/src/main/java/frc/robot/subsystems/limelight/RealVisionIO.java +++ b/src/main/java/frc/robot/subsystems/limelight/RealVisionIO.java @@ -8,7 +8,7 @@ import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import frc.robot.Robot; -import frc.robot.commands.FindCorrectBranchFromPos; +import frc.robot.commands.GamePieceLocate; import frc.robot.constants.Constants; import frc.robot.subsystems.swervev3.SwerveDrivetrain; import frc.robot.utils.diag.DiagLimelight; @@ -46,10 +46,10 @@ public void updateInputs(VisionInputs inputs) { inputs.ty = ty.getDouble(0); if (inputs.tv != 0) { inputs.coralSeen.add( - FindCorrectBranchFromPos.FindCoralBranch( + GamePieceLocate.findCoralBranch( drivetrain.getPose(), VecBuilder.fill(tx.getDouble(0), ty.getDouble(0)))); inputs.algaeSeen.add( - FindCorrectBranchFromPos.FindAlgae( + GamePieceLocate.findAlgaePos( drivetrain.getPose(), VecBuilder.fill(tx.getDouble(0), ty.getDouble(0)))); } } From 6cf0016798871d401610b93e3509c16cc00af8d1 Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Sun, 23 Mar 2025 19:00:38 -0400 Subject: [PATCH 56/99] fixed reflecton issue --- .../frc/robot/commands/GamePieceLocate.java | 13 ++++----- .../robot/commands/climber/StopClimber.java | 29 ++++++++++--------- .../robot/commands/sequences/CancelAll.java | 9 +++--- 3 files changed, 26 insertions(+), 25 deletions(-) diff --git a/src/main/java/frc/robot/commands/GamePieceLocate.java b/src/main/java/frc/robot/commands/GamePieceLocate.java index 3fc056c8..bfbbefb1 100644 --- a/src/main/java/frc/robot/commands/GamePieceLocate.java +++ b/src/main/java/frc/robot/commands/GamePieceLocate.java @@ -10,24 +10,23 @@ import frc.robot.constants.AlgaePositions; import frc.robot.constants.BranchPositions; import frc.robot.constants.Constants; - import java.util.Arrays; public class GamePieceLocate { private static final BranchPositions[] BRANCHES = BranchPositions.values(); private static final AlgaePositions[] ALGAES = AlgaePositions.values(); // Precomputed vectors for positions - private static final Vector[] PRECOMPUTED_BRANCH_VECS = Arrays.stream(BranchPositions.values()) + private static final Vector[] PRECOMPUTED_BRANCH_VECS = + Arrays.stream(BranchPositions.values()) .map(branch -> branch.getPosition().getTranslation().toVector()) - .toArray(Vector[]::new);; + .toArray(Vector[]::new); + ; - private static final Vector[] PRECOMPUTED_ALGAE_VECS = Arrays.stream(AlgaePositions.values()) + private static final Vector[] PRECOMPUTED_ALGAE_VECS = + Arrays.stream(AlgaePositions.values()) .map(algae -> algae.getPosition().getTranslation().toVector()) .toArray(Vector[]::new); - - - // PeicePos is in Radians public static BranchPositions findCoralBranch(Pose2d robotPos, Vector piecePos) { return findClosestPosition(robotPos, piecePos, BRANCHES, PRECOMPUTED_BRANCH_VECS); diff --git a/src/main/java/frc/robot/commands/climber/StopClimber.java b/src/main/java/frc/robot/commands/climber/StopClimber.java index eae2a033..2811e498 100644 --- a/src/main/java/frc/robot/commands/climber/StopClimber.java +++ b/src/main/java/frc/robot/commands/climber/StopClimber.java @@ -4,17 +4,20 @@ import frc.robot.utils.logging.commands.LoggableCommand; public class StopClimber extends LoggableCommand { - private ClimberSubsystem climber; - public StopClimber(ClimberSubsystem climber) { - this.climber = climber; - addRequirements(climber); - } - @Override - public void initialize() { - climber.stopClimber(); - } - @Override - public boolean isFinished() { - return true; - } + private ClimberSubsystem climber; + + public StopClimber(ClimberSubsystem climber) { + this.climber = climber; + addRequirements(climber); + } + + @Override + public void initialize() { + climber.stopClimber(); + } + + @Override + public boolean isFinished() { + return true; + } } diff --git a/src/main/java/frc/robot/commands/sequences/CancelAll.java b/src/main/java/frc/robot/commands/sequences/CancelAll.java index 476efc4d..745b0628 100644 --- a/src/main/java/frc/robot/commands/sequences/CancelAll.java +++ b/src/main/java/frc/robot/commands/sequences/CancelAll.java @@ -10,17 +10,16 @@ import frc.robot.subsystems.algaebyebyetilt.AlgaeByeByeTiltSubsystem; import frc.robot.subsystems.climber.ClimberSubsystem; import frc.robot.subsystems.elevator.ElevatorSubsystem; -import frc.robot.subsystems.swervev3.SwerveDrivetrain; import frc.robot.utils.logging.commands.LoggableSequentialCommandGroup; /** Add your docs here. */ public class CancelAll extends LoggableSequentialCommandGroup { public CancelAll( - AlgaeByeByeTiltSubsystem algaeByeByeTiltSubsystem, - AlgaeByeByeRollerSubsystem algaebyebyeroller, - ElevatorSubsystem elevatorSubsystem, - ClimberSubsystem climberSubsystem) { + AlgaeByeByeTiltSubsystem algaeByeByeTiltSubsystem, + AlgaeByeByeRollerSubsystem algaebyebyeroller, + ElevatorSubsystem elevatorSubsystem, + ClimberSubsystem climberSubsystem) { super( new StopClimber(climberSubsystem), new ByeByeAllDone(algaeByeByeTiltSubsystem, algaebyebyeroller, elevatorSubsystem), From 69cdd68d3d4965609baad53283a0554268fe6d67 Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Sun, 23 Mar 2025 19:02:59 -0400 Subject: [PATCH 57/99] fixed reflecton issue --- src/main/java/frc/robot/commands/GamePieceLocate.java | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/commands/GamePieceLocate.java b/src/main/java/frc/robot/commands/GamePieceLocate.java index bfbbefb1..b235ecc9 100644 --- a/src/main/java/frc/robot/commands/GamePieceLocate.java +++ b/src/main/java/frc/robot/commands/GamePieceLocate.java @@ -17,17 +17,17 @@ public class GamePieceLocate { private static final AlgaePositions[] ALGAES = AlgaePositions.values(); // Precomputed vectors for positions private static final Vector[] PRECOMPUTED_BRANCH_VECS = - Arrays.stream(BranchPositions.values()) + Arrays.stream(BRANCHES) .map(branch -> branch.getPosition().getTranslation().toVector()) .toArray(Vector[]::new); ; private static final Vector[] PRECOMPUTED_ALGAE_VECS = - Arrays.stream(AlgaePositions.values()) + Arrays.stream(ALGAES) .map(algae -> algae.getPosition().getTranslation().toVector()) .toArray(Vector[]::new); - // PeicePos is in Radians + // Piece Pos is in Radians public static BranchPositions findCoralBranch(Pose2d robotPos, Vector piecePos) { return findClosestPosition(robotPos, piecePos, BRANCHES, PRECOMPUTED_BRANCH_VECS); } @@ -56,7 +56,7 @@ public static > T findClosestPosition( return closest; } - // peicepos is in Radians + // piece pos is in Radians public static AlgaePositions findAlgaePos(Pose2d robotPos, Vector piecePos) { return findClosestPosition(robotPos, piecePos, ALGAES, PRECOMPUTED_ALGAE_VECS); } From f11649a3443c37b803135ae3af45969c70643291 Mon Sep 17 00:00:00 2001 From: codetoad Date: Sun, 23 Mar 2025 21:04:30 -0400 Subject: [PATCH 58/99] reset std after autonomous --- src/main/java/frc/robot/Robot.java | 4 +++- .../robot/subsystems/swervev3/estimation/PoseEstimator.java | 4 ++-- 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 043faf7f..e544a8e7 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -16,6 +16,7 @@ import frc.robot.commands.drivetrain.WheelAlign; import frc.robot.constants.Constants; import frc.robot.constants.GameConstants; +import frc.robot.subsystems.swervev3.estimation.PoseEstimator; import frc.robot.utils.RobotMode; import frc.robot.utils.diag.Diagnostics; import frc.robot.utils.logging.commands.CommandLogger; @@ -141,7 +142,7 @@ public void disabledExit() {} public void autonomousInit() { mode.set(RobotMode.AUTONOMOUS); new SetInitOdom(robotContainer.getDrivetrain(), robotContainer.getAutoChooser()).schedule(); - new SetBaseVisionStd(robotContainer.getDrivetrain(), VecBuilder.fill(0.45, 0.45, 0.1)); + new SetBaseVisionStd(robotContainer.getDrivetrain(), VecBuilder.fill(0.45, 0.45, 0.1)).schedule(); autoCommand = robotContainer.getAutonomousCommand(); if (autoCommand != null) { autoCommand.schedule(); @@ -156,6 +157,7 @@ public void autonomousExit() {} @Override public void teleopInit() { + new SetBaseVisionStd(robotContainer.getDrivetrain(), PoseEstimator.visionMeasurementStdDevs2).schedule(); mode.set(RobotMode.TELEOP); diagnostics.reset(); if (autoCommand != null) { diff --git a/src/main/java/frc/robot/subsystems/swervev3/estimation/PoseEstimator.java b/src/main/java/frc/robot/subsystems/swervev3/estimation/PoseEstimator.java index fe734335..48d32a72 100644 --- a/src/main/java/frc/robot/subsystems/swervev3/estimation/PoseEstimator.java +++ b/src/main/java/frc/robot/subsystems/swervev3/estimation/PoseEstimator.java @@ -43,7 +43,7 @@ public class PoseEstimator { private int invalidCounter = 0; /* standard deviation of robot states, the lower the numbers arm, the more we trust odometry */ - private static final Vector stateStdDevs1 = VecBuilder.fill(0.075, 0.075, 0.001); + public static final Vector stateStdDevs1 = VecBuilder.fill(0.075, 0.075, 0.001); /* standard deviation of vision readings, the lower the numbers arm, the more we trust vision */ // private static final Vector visionMeasurementStdDevs1 = VecBuilder.fill(0.5, 0.5, 0.5); @@ -52,7 +52,7 @@ public class PoseEstimator { private static final double visionStdRateOfChange = 1; /* standard deviation of vision readings, the lower the numbers arm, the more we trust vision */ - private static final Vector visionMeasurementStdDevs2 = VecBuilder.fill(0.1, 0.1, 0.01); + public static final Vector visionMeasurementStdDevs2 = VecBuilder.fill(0.1, 0.1, 0.01); private final FilterablePoseManager poseManager; public PoseEstimator( From 617859bb2881b3425d08fd035eea042ae63e9c48 Mon Sep 17 00:00:00 2001 From: codetoad Date: Sun, 23 Mar 2025 21:07:09 -0400 Subject: [PATCH 59/99] removed duplicate method --- .../frc/robot/subsystems/swervev3/SwerveDrivetrain.java | 8 +------- 1 file changed, 1 insertion(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java index 1db0c4d3..aa464a10 100644 --- a/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java @@ -143,13 +143,7 @@ public void drive(ChassisSpeeds speeds) { setModuleStates(swerveModuleStates); } - public ChassisSpeeds speedsFromStates() { - return kinematics.toChassisSpeeds( - frontLeft.getLatestState(), - frontRight.getLatestState(), - backLeft.getLatestState(), - backRight.getLatestState()); - } + private void setModuleStates(SwerveModuleState[] desiredStates) { Logger.recordOutput("desiredStates", desiredStates); From f8ecbe22bed166749784c668ae119d03114219fe Mon Sep 17 00:00:00 2001 From: codetoad Date: Sun, 23 Mar 2025 21:07:35 -0400 Subject: [PATCH 60/99] build code --- src/main/java/frc/robot/Robot.java | 6 ++-- src/main/java/frc/robot/RobotContainer.java | 2 +- .../robot/commands/climber/StopClimber.java | 29 ++++++++++--------- .../robot/commands/sequences/CancelAll.java | 9 +++--- .../subsystems/swervev3/SwerveDrivetrain.java | 2 -- 5 files changed, 25 insertions(+), 23 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index e544a8e7..68960c63 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -142,7 +142,8 @@ public void disabledExit() {} public void autonomousInit() { mode.set(RobotMode.AUTONOMOUS); new SetInitOdom(robotContainer.getDrivetrain(), robotContainer.getAutoChooser()).schedule(); - new SetBaseVisionStd(robotContainer.getDrivetrain(), VecBuilder.fill(0.45, 0.45, 0.1)).schedule(); + new SetBaseVisionStd(robotContainer.getDrivetrain(), VecBuilder.fill(0.45, 0.45, 0.1)) + .schedule(); autoCommand = robotContainer.getAutonomousCommand(); if (autoCommand != null) { autoCommand.schedule(); @@ -157,7 +158,8 @@ public void autonomousExit() {} @Override public void teleopInit() { - new SetBaseVisionStd(robotContainer.getDrivetrain(), PoseEstimator.visionMeasurementStdDevs2).schedule(); + new SetBaseVisionStd(robotContainer.getDrivetrain(), PoseEstimator.visionMeasurementStdDevs2) + .schedule(); mode.set(RobotMode.TELEOP); diagnostics.reset(); if (autoCommand != null) { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 7b4a174e..72195fe3 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -447,7 +447,7 @@ private void setupPathPlanning() { AutoBuilder.configure( drivetrain::getPose, drivetrain::resetOdometry, - drivetrain::speedsFromStates, + drivetrain::getChassisSpeeds, drivetrain::drive, new PPHolonomicDriveController( new PIDConstants( diff --git a/src/main/java/frc/robot/commands/climber/StopClimber.java b/src/main/java/frc/robot/commands/climber/StopClimber.java index eae2a033..2811e498 100644 --- a/src/main/java/frc/robot/commands/climber/StopClimber.java +++ b/src/main/java/frc/robot/commands/climber/StopClimber.java @@ -4,17 +4,20 @@ import frc.robot.utils.logging.commands.LoggableCommand; public class StopClimber extends LoggableCommand { - private ClimberSubsystem climber; - public StopClimber(ClimberSubsystem climber) { - this.climber = climber; - addRequirements(climber); - } - @Override - public void initialize() { - climber.stopClimber(); - } - @Override - public boolean isFinished() { - return true; - } + private ClimberSubsystem climber; + + public StopClimber(ClimberSubsystem climber) { + this.climber = climber; + addRequirements(climber); + } + + @Override + public void initialize() { + climber.stopClimber(); + } + + @Override + public boolean isFinished() { + return true; + } } diff --git a/src/main/java/frc/robot/commands/sequences/CancelAll.java b/src/main/java/frc/robot/commands/sequences/CancelAll.java index 476efc4d..745b0628 100644 --- a/src/main/java/frc/robot/commands/sequences/CancelAll.java +++ b/src/main/java/frc/robot/commands/sequences/CancelAll.java @@ -10,17 +10,16 @@ import frc.robot.subsystems.algaebyebyetilt.AlgaeByeByeTiltSubsystem; import frc.robot.subsystems.climber.ClimberSubsystem; import frc.robot.subsystems.elevator.ElevatorSubsystem; -import frc.robot.subsystems.swervev3.SwerveDrivetrain; import frc.robot.utils.logging.commands.LoggableSequentialCommandGroup; /** Add your docs here. */ public class CancelAll extends LoggableSequentialCommandGroup { public CancelAll( - AlgaeByeByeTiltSubsystem algaeByeByeTiltSubsystem, - AlgaeByeByeRollerSubsystem algaebyebyeroller, - ElevatorSubsystem elevatorSubsystem, - ClimberSubsystem climberSubsystem) { + AlgaeByeByeTiltSubsystem algaeByeByeTiltSubsystem, + AlgaeByeByeRollerSubsystem algaebyebyeroller, + ElevatorSubsystem elevatorSubsystem, + ClimberSubsystem climberSubsystem) { super( new StopClimber(climberSubsystem), new ByeByeAllDone(algaeByeByeTiltSubsystem, algaebyebyeroller, elevatorSubsystem), diff --git a/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java index aa464a10..bb2c5e67 100644 --- a/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java @@ -143,8 +143,6 @@ public void drive(ChassisSpeeds speeds) { setModuleStates(swerveModuleStates); } - - private void setModuleStates(SwerveModuleState[] desiredStates) { Logger.recordOutput("desiredStates", desiredStates); frontLeft.setState(desiredStates[0]); From 8f331ef9a567e761dcf0f47befdcaf4ee3552e2c Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Sun, 23 Mar 2025 21:37:34 -0400 Subject: [PATCH 61/99] small pr fixed --- src/main/java/frc/robot/commands/GamePieceLocate.java | 2 +- src/main/java/frc/robot/constants/GameConstants.java | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/commands/GamePieceLocate.java b/src/main/java/frc/robot/commands/GamePieceLocate.java index b235ecc9..3c201ef2 100644 --- a/src/main/java/frc/robot/commands/GamePieceLocate.java +++ b/src/main/java/frc/robot/commands/GamePieceLocate.java @@ -34,7 +34,7 @@ public static BranchPositions findCoralBranch(Pose2d robotPos, Vector pieceP public static > T findClosestPosition( Pose2d robotPos, Vector piecePos, T[] values, Vector[] preComputedVecs) { - final Pose3d cameraPos = new Pose3d(robotPos).transformBy(Constants.CAMERA_TO_ROBOT); + final Pose3d cameraPos = new Pose3d(robotPos).transformBy(Constants.LIMELIGHT_TO_ROBOT); final Vector cameraPosVec = cameraPos.getTranslation().toVector(); final Matrix invCameraRotation = cameraPos.getRotation().unaryMinus().toMatrix(); final Vector pieceVec = diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java index 1d05fd37..3654cac6 100644 --- a/src/main/java/frc/robot/constants/GameConstants.java +++ b/src/main/java/frc/robot/constants/GameConstants.java @@ -222,7 +222,7 @@ public enum Mode { public static final double ELEVATOR_MANUAL_MAX_SPEED_DOWN = .15; // Limelight - public static final Transform3d CAMERA_TO_ROBOT = + public static final Transform3d LIMELIGHT_TO_ROBOT = new Transform3d( 0.0, 0.0, 0.720725, new Rotation3d(0.0, Math.PI / 6, 0.0)); // TODO Change Later public static final String LIMELIGHT_IP_ADDRESS = "10.40.48.104"; // TODO Change Later From 0a0dbe76127b43dd28dd3566da1d80b7a4f14c4a Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Mon, 24 Mar 2025 11:56:02 -0400 Subject: [PATCH 62/99] made only check closest and neighboring algae, coral. --- .../frc/robot/commands/GamePieceLocate.java | 155 ++++++++++++++++-- .../frc/robot/constants/CenterPositions.java | 36 ++++ 2 files changed, 174 insertions(+), 17 deletions(-) create mode 100644 src/main/java/frc/robot/constants/CenterPositions.java diff --git a/src/main/java/frc/robot/commands/GamePieceLocate.java b/src/main/java/frc/robot/commands/GamePieceLocate.java index 3c201ef2..230fd1ee 100644 --- a/src/main/java/frc/robot/commands/GamePieceLocate.java +++ b/src/main/java/frc/robot/commands/GamePieceLocate.java @@ -9,6 +9,7 @@ import edu.wpi.first.math.numbers.N3; import frc.robot.constants.AlgaePositions; import frc.robot.constants.BranchPositions; +import frc.robot.constants.CenterPositions; import frc.robot.constants.Constants; import java.util.Arrays; @@ -29,28 +30,76 @@ public class GamePieceLocate { // Piece Pos is in Radians public static BranchPositions findCoralBranch(Pose2d robotPos, Vector piecePos) { - return findClosestPosition(robotPos, piecePos, BRANCHES, PRECOMPUTED_BRANCH_VECS); - } - - public static > T findClosestPosition( - Pose2d robotPos, Vector piecePos, T[] values, Vector[] preComputedVecs) { final Pose3d cameraPos = new Pose3d(robotPos).transformBy(Constants.LIMELIGHT_TO_ROBOT); final Vector cameraPosVec = cameraPos.getTranslation().toVector(); final Matrix invCameraRotation = cameraPos.getRotation().unaryMinus().toMatrix(); final Vector pieceVec = VecBuilder.fill(1, -Math.tan(piecePos.get(1)), -Math.tan(piecePos.get(0))); double maxDot = -1.0; - T closest = null; - for (int i = 0; i < values.length; i++) { - Matrix locationVec = - (invCameraRotation.times(preComputedVecs[i].minus(cameraPosVec))); - double dot = - pieceVec.dot( - VecBuilder.fill(locationVec.get(0, 0), locationVec.get(1, 0), locationVec.get(2, 0)) - .unit()); - if (dot > maxDot) { - maxDot = dot; - closest = values[i]; + BranchPositions closest = null; + int n = CenterPositions.getClosest(robotPos); + if (n == 0) { + for (int i = 0; i < 9; i++) { + Matrix locationVec = + (invCameraRotation.times(PRECOMPUTED_BRANCH_VECS[i].minus(cameraPosVec))); + double dot = + pieceVec.dot( + VecBuilder.fill(locationVec.get(0, 0), locationVec.get(1, 0), locationVec.get(2, 0)) + .unit()); + if (dot > maxDot) { + maxDot = dot; + closest = BRANCHES[i]; + } + } + for (int i = 33; i < 36; i++) { + Matrix locationVec = + (invCameraRotation.times(PRECOMPUTED_BRANCH_VECS[i].minus(cameraPosVec))); + double dot = + pieceVec.dot( + VecBuilder.fill(locationVec.get(0, 0), locationVec.get(1, 0), locationVec.get(2, 0)) + .unit()); + if (dot > maxDot) { + maxDot = dot; + closest = BRANCHES[i]; + } + } + } else if (n == 5) { + for (int i = 0; i < 3; i++) { + Matrix locationVec = + (invCameraRotation.times(PRECOMPUTED_BRANCH_VECS[i].minus(cameraPosVec))); + double dot = + pieceVec.dot( + VecBuilder.fill(locationVec.get(0, 0), locationVec.get(1, 0), locationVec.get(2, 0)) + .unit()); + if (dot > maxDot) { + maxDot = dot; + closest = BRANCHES[i]; + } + } + for (int i = 6 * n - 3; i < 6 * n + 6; i++) { + Matrix locationVec = + (invCameraRotation.times(PRECOMPUTED_BRANCH_VECS[i].minus(cameraPosVec))); + double dot = + pieceVec.dot( + VecBuilder.fill(locationVec.get(0, 0), locationVec.get(1, 0), locationVec.get(2, 0)) + .unit()); + if (dot > maxDot) { + maxDot = dot; + closest = BRANCHES[i]; + } + } + } else { + for (int i = 6 * n - 3; i < 6 * n + 9; i++) { + Matrix locationVec = + (invCameraRotation.times(PRECOMPUTED_BRANCH_VECS[i].minus(cameraPosVec))); + double dot = + pieceVec.dot( + VecBuilder.fill(locationVec.get(0, 0), locationVec.get(1, 0), locationVec.get(2, 0)) + .unit()); + if (dot > maxDot) { + maxDot = dot; + closest = BRANCHES[i]; + } } } return closest; @@ -58,7 +107,79 @@ public static > T findClosestPosition( // piece pos is in Radians public static AlgaePositions findAlgaePos(Pose2d robotPos, Vector piecePos) { - return findClosestPosition(robotPos, piecePos, ALGAES, PRECOMPUTED_ALGAE_VECS); + final Pose3d cameraPos = new Pose3d(robotPos).transformBy(Constants.LIMELIGHT_TO_ROBOT); + final Vector cameraPosVec = cameraPos.getTranslation().toVector(); + final Matrix invCameraRotation = cameraPos.getRotation().unaryMinus().toMatrix(); + final Vector pieceVec = + VecBuilder.fill(1, -Math.tan(piecePos.get(1)), -Math.tan(piecePos.get(0))); + double maxDot = -1.0; + AlgaePositions closest = null; + int n = CenterPositions.getClosest(robotPos); + if (n == 0) { + for (int i = 0; i < 6; i++) { + Matrix locationVec = + (invCameraRotation.times(PRECOMPUTED_ALGAE_VECS[i].minus(cameraPosVec))); + double dot = + pieceVec.dot( + VecBuilder.fill(locationVec.get(0, 0), locationVec.get(1, 0), locationVec.get(2, 0)) + .unit()); + if (dot > maxDot) { + maxDot = dot; + closest = ALGAES[i]; + } + } + for (int i = 10; i < 12; i++) { + Matrix locationVec = + (invCameraRotation.times(PRECOMPUTED_ALGAE_VECS[i].minus(cameraPosVec))); + double dot = + pieceVec.dot( + VecBuilder.fill(locationVec.get(0, 0), locationVec.get(1, 0), locationVec.get(2, 0)) + .unit()); + if (dot > maxDot) { + maxDot = dot; + closest = ALGAES[i]; + } + } + } else if (n == 5) { + for (int i = 0; i < 3; i++) { + Matrix locationVec = + (invCameraRotation.times(PRECOMPUTED_ALGAE_VECS[i].minus(cameraPosVec))); + double dot = + pieceVec.dot( + VecBuilder.fill(locationVec.get(0, 0), locationVec.get(1, 0), locationVec.get(2, 0)) + .unit()); + if (dot > maxDot) { + maxDot = dot; + closest = ALGAES[i]; + } + } + for (int i = 8; i < 12; i++) { + Matrix locationVec = + (invCameraRotation.times(PRECOMPUTED_ALGAE_VECS[i].minus(cameraPosVec))); + double dot = + pieceVec.dot( + VecBuilder.fill(locationVec.get(0, 0), locationVec.get(1, 0), locationVec.get(2, 0)) + .unit()); + if (dot > maxDot) { + maxDot = dot; + closest = ALGAES[i]; + } + } + } else { + for (int i = 2 * n - 2; i < 2 * n + 4; i++) { + Matrix locationVec = + (invCameraRotation.times(PRECOMPUTED_ALGAE_VECS[i].minus(cameraPosVec))); + double dot = + pieceVec.dot( + VecBuilder.fill(locationVec.get(0, 0), locationVec.get(1, 0), locationVec.get(2, 0)) + .unit()); + if (dot > maxDot) { + maxDot = dot; + closest = ALGAES[i]; + } + } + } + return closest; } public static AlgaePositions UnitTest1() { diff --git a/src/main/java/frc/robot/constants/CenterPositions.java b/src/main/java/frc/robot/constants/CenterPositions.java new file mode 100644 index 00000000..fd6746f4 --- /dev/null +++ b/src/main/java/frc/robot/constants/CenterPositions.java @@ -0,0 +1,36 @@ +package frc.robot.constants; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Translation2d; + +public enum CenterPositions { + CENTER_AB(new Translation2d(3.8337244850000, 4.0259000000000)), + CENTER_CD(new Translation2d(4.1615306625000, 3.4581230455351)), + CENTER_EF(new Translation2d(4.8171430175000, 3.4581230455351)), + CENTER_GH(new Translation2d(5.1449491950000, 4.0259000000000)), + CENTER_IJ(new Translation2d(4.8171430175000, 4.5936769544649)), + CENTER_KL(new Translation2d(4.1615306625000, 4.5936769544649)); + + private final Translation2d position; + + CenterPositions(Translation2d position) { + this.position = position; + } + + public Translation2d getPosition() { + return position; + } + + public static int getClosest(Pose2d robotPose) { + double closeDistance = 100; + int n = 0; + for (int i = 0; i < values().length; i++) { + double distance = robotPose.getTranslation().getDistance(values()[i].getPosition()); + if (distance < closeDistance) { + n = i; + closeDistance = distance; + } + } + return n; + } +} From 61dc0a2c46cb1d986d9201f201a6f2d9084a334e Mon Sep 17 00:00:00 2001 From: Code Toad <55628278+goatfanboi23@users.noreply.github.com> Date: Mon, 24 Mar 2025 11:58:44 -0400 Subject: [PATCH 63/99] made unit tests (#295) Co-authored-by: codetoad Co-authored-by: Lev Strougov <62769580+Levercpu@users.noreply.github.com> --- src/main/java/frc/robot/RobotContainer.java | 3 --- .../java/frc/robot/commands/UnitTester.java | 16 ------------- .../subsystems/limelight/RealVisionIO.java | 2 +- .../{commands => utils}/GamePieceLocate.java | 8 +------ .../frc/robot/utils/GamePieceLocateTest.java | 23 +++++++++++++++++++ 5 files changed, 25 insertions(+), 27 deletions(-) delete mode 100644 src/main/java/frc/robot/commands/UnitTester.java rename src/main/java/frc/robot/{commands => utils}/GamePieceLocate.java (96%) create mode 100644 src/test/java/frc/robot/utils/GamePieceLocateTest.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index cffea4f6..de9e5ce5 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -22,7 +22,6 @@ import frc.robot.autochooser.FieldLocation; import frc.robot.autochooser.chooser.AutoChooser2025; import frc.robot.autochooser.event.RealAutoEventProvider; -import frc.robot.commands.UnitTester; import frc.robot.commands.byebye.ByeByeToFwrLimit; import frc.robot.commands.byebye.ByeByeToRevLimit; import frc.robot.commands.coral.IntakeCoral; @@ -80,7 +79,6 @@ import frc.robot.utils.logging.LoggableIO; import frc.robot.utils.motor.Gain; import frc.robot.utils.motor.PID; -import frc.robot.utils.shuffleboard.SmartShuffleboard; import frc.robot.utils.simulation.RobotVisualizer; import frc.robot.utils.simulation.SwerveSimulationUtils; import java.util.function.Consumer; @@ -445,7 +443,6 @@ public RobotVisualizer getRobotVisualizer() { } public void putShuffleboardCommands() { - SmartShuffleboard.putCommand("What Branch Unit Test", "Test 1", new UnitTester()); if (Constants.CORAL_DEBUG) { SmartDashboard.putData( "Shoot Coral", new ShootCoral(coralSubsystem, Constants.CORAL_SHOOTER_SPEED)); diff --git a/src/main/java/frc/robot/commands/UnitTester.java b/src/main/java/frc/robot/commands/UnitTester.java deleted file mode 100644 index 1ba6c7cc..00000000 --- a/src/main/java/frc/robot/commands/UnitTester.java +++ /dev/null @@ -1,16 +0,0 @@ -package frc.robot.commands; - -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.utils.shuffleboard.SmartShuffleboard; - -public class UnitTester extends Command { - @Override - public void execute() { - SmartShuffleboard.put("Branch", "Branch", GamePieceLocate.UnitTest1().toString()); - } - - @Override - public boolean isFinished() { - return true; - } -} diff --git a/src/main/java/frc/robot/subsystems/limelight/RealVisionIO.java b/src/main/java/frc/robot/subsystems/limelight/RealVisionIO.java index 04692537..da30f85c 100644 --- a/src/main/java/frc/robot/subsystems/limelight/RealVisionIO.java +++ b/src/main/java/frc/robot/subsystems/limelight/RealVisionIO.java @@ -8,9 +8,9 @@ import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import frc.robot.Robot; -import frc.robot.commands.GamePieceLocate; import frc.robot.constants.Constants; import frc.robot.subsystems.swervev3.SwerveDrivetrain; +import frc.robot.utils.GamePieceLocate; import frc.robot.utils.diag.DiagLimelight; import java.util.Map; diff --git a/src/main/java/frc/robot/commands/GamePieceLocate.java b/src/main/java/frc/robot/utils/GamePieceLocate.java similarity index 96% rename from src/main/java/frc/robot/commands/GamePieceLocate.java rename to src/main/java/frc/robot/utils/GamePieceLocate.java index 230fd1ee..2d3dc80e 100644 --- a/src/main/java/frc/robot/commands/GamePieceLocate.java +++ b/src/main/java/frc/robot/utils/GamePieceLocate.java @@ -1,4 +1,4 @@ -package frc.robot.commands; +package frc.robot.utils; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.VecBuilder; @@ -181,10 +181,4 @@ public static AlgaePositions findAlgaePos(Pose2d robotPos, Vector piecePos) } return closest; } - - public static AlgaePositions UnitTest1() { - double x = 0.31119220563058896357; - double y = 0.06719517620178168871; - return findAlgaePos(new Pose2d(2.614524485, 4.0259, new Rotation2d(0)), VecBuilder.fill(x, y)); - } } diff --git a/src/test/java/frc/robot/utils/GamePieceLocateTest.java b/src/test/java/frc/robot/utils/GamePieceLocateTest.java new file mode 100644 index 00000000..631037a4 --- /dev/null +++ b/src/test/java/frc/robot/utils/GamePieceLocateTest.java @@ -0,0 +1,23 @@ +package frc.robot.utils; + +import static org.junit.jupiter.api.Assertions.*; + +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import frc.robot.constants.AlgaePositions; +import org.junit.jupiter.api.Test; + +class GamePieceLocateTest { + + @Test + void findAlgaePos() { + double x = 0.31119220563058896357; + double y = 0.06719517620178168871; + AlgaePositions algaePos = + GamePieceLocate.findAlgaePos( + new Pose2d(2.614524485, 4.0259, new Rotation2d(0)), VecBuilder.fill(x, y)); + assertEquals(5.144949195, algaePos.getPosition().getX()); + assertEquals(4.0259, algaePos.getPosition().getY()); + } +} From 035c70bf4cd1224727e811806e60f7c2e2ad91fb Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Mon, 24 Mar 2025 12:16:50 -0400 Subject: [PATCH 64/99] fixed noah's pr comments --- .../frc/robot/constants/GameConstants.java | 1 + .../subsystems/limelight/RealVisionIO.java | 24 +++++++++++++++---- .../subsystems/limelight/VisionInputs.java | 18 +++++--------- .../java/frc/robot/utils/GamePieceLocate.java | 4 ++-- 4 files changed, 29 insertions(+), 18 deletions(-) diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java index c599f96f..e3834e36 100644 --- a/src/main/java/frc/robot/constants/GameConstants.java +++ b/src/main/java/frc/robot/constants/GameConstants.java @@ -230,4 +230,5 @@ public enum Mode { new Transform3d( 0.0, 0.0, 0.720725, new Rotation3d(0.0, Math.PI / 6, 0.0)); // TODO Change Later public static final String LIMELIGHT_IP_ADDRESS = "10.40.48.104"; // TODO Change Later + public static final double MINUMUM_PIECE_DETECTION_DOT = 0.8; } diff --git a/src/main/java/frc/robot/subsystems/limelight/RealVisionIO.java b/src/main/java/frc/robot/subsystems/limelight/RealVisionIO.java index da30f85c..9e4faa6e 100644 --- a/src/main/java/frc/robot/subsystems/limelight/RealVisionIO.java +++ b/src/main/java/frc/robot/subsystems/limelight/RealVisionIO.java @@ -8,6 +8,8 @@ import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import frc.robot.Robot; +import frc.robot.constants.AlgaePositions; +import frc.robot.constants.BranchPositions; import frc.robot.constants.Constants; import frc.robot.subsystems.swervev3.SwerveDrivetrain; import frc.robot.utils.GamePieceLocate; @@ -45,12 +47,26 @@ public void updateInputs(VisionInputs inputs) { inputs.tx = tx.getDouble(0); inputs.ty = ty.getDouble(0); if (inputs.tv != 0) { - inputs.coralSeen.add( + BranchPositions[] newCoralArray = new BranchPositions[inputs.coralSeen.length + 1]; + AlgaePositions[] newAlgaeArray = new AlgaePositions[inputs.algaeSeen.length + 1]; + int oldCoralArrayLength = inputs.coralSeen.length; + int oldAlgaeArrayLength = inputs.algaeSeen.length; + for (int i = 0; i < oldCoralArrayLength; i++) { + newCoralArray[i] = inputs.coralSeen[i]; + } + newCoralArray[oldCoralArrayLength+1] = GamePieceLocate.findCoralBranch( - drivetrain.getPose(), VecBuilder.fill(tx.getDouble(0), ty.getDouble(0)))); - inputs.algaeSeen.add( + drivetrain.getPose(), VecBuilder.fill(tx.getDouble(0), ty.getDouble(0))); + + for (int i = 0; i < oldAlgaeArrayLength; i++) { + newAlgaeArray[i] = inputs.algaeSeen[i]; + } + newAlgaeArray[oldAlgaeArrayLength+1] = GamePieceLocate.findAlgaePos( - drivetrain.getPose(), VecBuilder.fill(tx.getDouble(0), ty.getDouble(0)))); + drivetrain.getPose(), VecBuilder.fill(tx.getDouble(0), ty.getDouble(0))); + + inputs.coralSeen = newCoralArray; + inputs.algaeSeen = newAlgaeArray; } } } diff --git a/src/main/java/frc/robot/subsystems/limelight/VisionInputs.java b/src/main/java/frc/robot/subsystems/limelight/VisionInputs.java index 908d6eec..13170f37 100644 --- a/src/main/java/frc/robot/subsystems/limelight/VisionInputs.java +++ b/src/main/java/frc/robot/subsystems/limelight/VisionInputs.java @@ -11,8 +11,8 @@ public class VisionInputs extends FolderLoggableInputs { public double tv = 0; public double tx = 0; public double ty = 0; - public ArrayList coralSeen; - public ArrayList algaeSeen; + public BranchPositions[] coralSeen; + public AlgaePositions[] algaeSeen; public VisionInputs(String folder) { super(folder); @@ -23,12 +23,8 @@ public void toLog(LogTable table) { table.put("tv", tv); table.put("tx", tx); table.put("ty", ty); - if (coralSeen != null) { - table.put("coralSeen", coralSeen.toArray(BranchPositions[]::new)); - } - if (algaeSeen != null) { - table.put("algaeSeen", algaeSeen.toArray(AlgaePositions[]::new)); - } + table.put("coralSeen", coralSeen); + table.put("algaeSeen", algaeSeen); } @Override @@ -36,9 +32,7 @@ public void fromLog(LogTable table) { tv = table.get("tv", tv); tx = table.get("tx", tx); ty = table.get("ty", ty); - coralSeen = - new ArrayList<>(List.of(table.get("coralSeen", coralSeen.toArray(BranchPositions[]::new)))); - algaeSeen = - new ArrayList<>(List.of(table.get("algaeSeen", algaeSeen.toArray(AlgaePositions[]::new)))); + coralSeen = table.get("coralSeen", coralSeen); + algaeSeen = table.get("algaeSeen", algaeSeen); } } diff --git a/src/main/java/frc/robot/utils/GamePieceLocate.java b/src/main/java/frc/robot/utils/GamePieceLocate.java index 2d3dc80e..75019b89 100644 --- a/src/main/java/frc/robot/utils/GamePieceLocate.java +++ b/src/main/java/frc/robot/utils/GamePieceLocate.java @@ -34,8 +34,8 @@ public static BranchPositions findCoralBranch(Pose2d robotPos, Vector pieceP final Vector cameraPosVec = cameraPos.getTranslation().toVector(); final Matrix invCameraRotation = cameraPos.getRotation().unaryMinus().toMatrix(); final Vector pieceVec = - VecBuilder.fill(1, -Math.tan(piecePos.get(1)), -Math.tan(piecePos.get(0))); - double maxDot = -1.0; + VecBuilder.fill(1, -Math.tan(piecePos.get(1)), -Math.tan(piecePos.get(0))).unit(); + double maxDot = Constants.MINUMUM_PIECE_DETECTION_DOT; BranchPositions closest = null; int n = CenterPositions.getClosest(robotPos); if (n == 0) { From e50b0e71a2b4c5b6fef64a02ca12ac2a31820351 Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Mon, 24 Mar 2025 12:18:34 -0400 Subject: [PATCH 65/99] small fix and linter --- src/main/java/frc/robot/constants/GameConstants.java | 2 +- .../java/frc/robot/subsystems/limelight/RealVisionIO.java | 4 ++-- src/main/java/frc/robot/subsystems/limelight/Vision.java | 5 ++--- .../java/frc/robot/subsystems/limelight/VisionInputs.java | 2 -- src/main/java/frc/robot/utils/GamePieceLocate.java | 2 +- 5 files changed, 6 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java index e3834e36..7a71d507 100644 --- a/src/main/java/frc/robot/constants/GameConstants.java +++ b/src/main/java/frc/robot/constants/GameConstants.java @@ -230,5 +230,5 @@ public enum Mode { new Transform3d( 0.0, 0.0, 0.720725, new Rotation3d(0.0, Math.PI / 6, 0.0)); // TODO Change Later public static final String LIMELIGHT_IP_ADDRESS = "10.40.48.104"; // TODO Change Later - public static final double MINUMUM_PIECE_DETECTION_DOT = 0.8; + public static final double MINIMUM_PIECE_DETECTION_DOT = 0.8; } diff --git a/src/main/java/frc/robot/subsystems/limelight/RealVisionIO.java b/src/main/java/frc/robot/subsystems/limelight/RealVisionIO.java index 9e4faa6e..3dbe2504 100644 --- a/src/main/java/frc/robot/subsystems/limelight/RealVisionIO.java +++ b/src/main/java/frc/robot/subsystems/limelight/RealVisionIO.java @@ -54,14 +54,14 @@ public void updateInputs(VisionInputs inputs) { for (int i = 0; i < oldCoralArrayLength; i++) { newCoralArray[i] = inputs.coralSeen[i]; } - newCoralArray[oldCoralArrayLength+1] = + newCoralArray[oldCoralArrayLength + 1] = GamePieceLocate.findCoralBranch( drivetrain.getPose(), VecBuilder.fill(tx.getDouble(0), ty.getDouble(0))); for (int i = 0; i < oldAlgaeArrayLength; i++) { newAlgaeArray[i] = inputs.algaeSeen[i]; } - newAlgaeArray[oldAlgaeArrayLength+1] = + newAlgaeArray[oldAlgaeArrayLength + 1] = GamePieceLocate.findAlgaePos( drivetrain.getPose(), VecBuilder.fill(tx.getDouble(0), ty.getDouble(0))); diff --git a/src/main/java/frc/robot/subsystems/limelight/Vision.java b/src/main/java/frc/robot/subsystems/limelight/Vision.java index 786636db..494e4954 100644 --- a/src/main/java/frc/robot/subsystems/limelight/Vision.java +++ b/src/main/java/frc/robot/subsystems/limelight/Vision.java @@ -9,7 +9,6 @@ import frc.robot.constants.BranchPositions; import frc.robot.utils.logging.subsystem.LoggableSystem; import frc.robot.utils.shuffleboard.SmartShuffleboard; -import java.util.ArrayList; public class Vision extends SubsystemBase { LoggableSystem system; @@ -36,11 +35,11 @@ public boolean isPieceSeen() { return system.getInputs().tv != 0; } - public ArrayList getAllBranchPosition() { + public BranchPositions[] getAllBranchPosition() { return system.getInputs().coralSeen; } - public ArrayList getAllAlgaePosition() { + public AlgaePositions[] getAllAlgaePosition() { return system.getInputs().algaeSeen; } diff --git a/src/main/java/frc/robot/subsystems/limelight/VisionInputs.java b/src/main/java/frc/robot/subsystems/limelight/VisionInputs.java index 13170f37..7d141965 100644 --- a/src/main/java/frc/robot/subsystems/limelight/VisionInputs.java +++ b/src/main/java/frc/robot/subsystems/limelight/VisionInputs.java @@ -3,8 +3,6 @@ import frc.robot.constants.AlgaePositions; import frc.robot.constants.BranchPositions; import frc.robot.utils.logging.subsystem.FolderLoggableInputs; -import java.util.ArrayList; -import java.util.List; import org.littletonrobotics.junction.LogTable; public class VisionInputs extends FolderLoggableInputs { diff --git a/src/main/java/frc/robot/utils/GamePieceLocate.java b/src/main/java/frc/robot/utils/GamePieceLocate.java index 75019b89..68aee175 100644 --- a/src/main/java/frc/robot/utils/GamePieceLocate.java +++ b/src/main/java/frc/robot/utils/GamePieceLocate.java @@ -35,7 +35,7 @@ public static BranchPositions findCoralBranch(Pose2d robotPos, Vector pieceP final Matrix invCameraRotation = cameraPos.getRotation().unaryMinus().toMatrix(); final Vector pieceVec = VecBuilder.fill(1, -Math.tan(piecePos.get(1)), -Math.tan(piecePos.get(0))).unit(); - double maxDot = Constants.MINUMUM_PIECE_DETECTION_DOT; + double maxDot = Constants.MINIMUM_PIECE_DETECTION_DOT; BranchPositions closest = null; int n = CenterPositions.getClosest(robotPos); if (n == 0) { From 671981ad33b633ec29e9e1f324ff864b291f7066 Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Mon, 24 Mar 2025 12:19:17 -0400 Subject: [PATCH 66/99] small fix and linter --- src/main/java/frc/robot/utils/GamePieceLocate.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/utils/GamePieceLocate.java b/src/main/java/frc/robot/utils/GamePieceLocate.java index 68aee175..ac2cf463 100644 --- a/src/main/java/frc/robot/utils/GamePieceLocate.java +++ b/src/main/java/frc/robot/utils/GamePieceLocate.java @@ -112,7 +112,7 @@ public static AlgaePositions findAlgaePos(Pose2d robotPos, Vector piecePos) final Matrix invCameraRotation = cameraPos.getRotation().unaryMinus().toMatrix(); final Vector pieceVec = VecBuilder.fill(1, -Math.tan(piecePos.get(1)), -Math.tan(piecePos.get(0))); - double maxDot = -1.0; + double maxDot = Constants.MINIMUM_PIECE_DETECTION_DOT; AlgaePositions closest = null; int n = CenterPositions.getClosest(robotPos); if (n == 0) { From d575d7e6d0979e6677b10c5feb321c8f8adbb98d Mon Sep 17 00:00:00 2001 From: codetoad Date: Mon, 24 Mar 2025 23:29:18 -0400 Subject: [PATCH 67/99] added red auto-align --- .../frc/robot/autochooser/FieldLocation.java | 3 +- .../alignment/AlignClosestBranch.java | 11 ++-- .../robot/constants/AlignmentPosition.java | 65 ++++++++++--------- 3 files changed, 44 insertions(+), 35 deletions(-) diff --git a/src/main/java/frc/robot/autochooser/FieldLocation.java b/src/main/java/frc/robot/autochooser/FieldLocation.java index e25b211b..bb8ae600 100644 --- a/src/main/java/frc/robot/autochooser/FieldLocation.java +++ b/src/main/java/frc/robot/autochooser/FieldLocation.java @@ -19,7 +19,8 @@ public enum FieldLocation { RIGHT(7.150, 2.000, 180, "Processor Side"); private static final double RED_X_POS = 2.3876; // meters - private static final double HEIGHT_OF_FIELD = 8.05; + public static final double HEIGHT_OF_FIELD = 8.05; + public static final double LENGTH_OF_FIELD = 17.548225; private final double yPose; private final double xPose; private final double angle; diff --git a/src/main/java/frc/robot/commands/alignment/AlignClosestBranch.java b/src/main/java/frc/robot/commands/alignment/AlignClosestBranch.java index 2d21e13e..9d42b5c8 100644 --- a/src/main/java/frc/robot/commands/alignment/AlignClosestBranch.java +++ b/src/main/java/frc/robot/commands/alignment/AlignClosestBranch.java @@ -1,5 +1,6 @@ package frc.robot.commands.alignment; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.CommandScheduler; import frc.robot.constants.AlignmentPosition; @@ -10,8 +11,7 @@ import org.littletonrobotics.junction.Logger; public class AlignClosestBranch extends LoggableCommand { - private AlignmentPosition alignmentTarget; - private final SwerveDrivetrain drivetrain; + private final SwerveDrivetrain drivetrain; private LoggableCommand followTrajectory; private final Timer timer = new Timer(); @@ -23,13 +23,14 @@ public AlignClosestBranch(SwerveDrivetrain drivetrain) { @Override public void initialize() { timer.restart(); - alignmentTarget = AlignmentPosition.getClosest(drivetrain.getPose().getTranslation()); + AlignmentPosition alignmentTarget = AlignmentPosition.getClosest(drivetrain.getPose().getTranslation()); Logger.recordOutput("TargetReefAlignment", alignmentTarget); - Logger.recordOutput("TargetReefPose", alignmentTarget.getPosition()); + Pose2d position = alignmentTarget.getPosition(); + Logger.recordOutput("TargetReefPose", position); drivetrain.setFocusedApriltag(alignmentTarget.getTag()); // spotless:off (shhhh don't tell anyone about the linter bypass) followTrajectory = LoggableCommandWrapper.wrap( - drivetrain.generateTrajectoryCommand(alignmentTarget.getPosition()) + drivetrain.generateTrajectoryCommand(position) ).withBasicName("FollowAlignToBranchTrajectory"); // spotless:on followTrajectory.setParent(this); diff --git a/src/main/java/frc/robot/constants/AlignmentPosition.java b/src/main/java/frc/robot/constants/AlignmentPosition.java index 9fc3f2fe..7a7279a1 100644 --- a/src/main/java/frc/robot/constants/AlignmentPosition.java +++ b/src/main/java/frc/robot/constants/AlignmentPosition.java @@ -5,90 +5,97 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.DriverStation; import frc.robot.Robot; +import frc.robot.autochooser.FieldLocation; import frc.robot.utils.Apriltag; import java.util.Optional; public enum AlignmentPosition { BRANCH_A( - new Pose2d( - 3.2067500000000, 4.0614305050000, Rotation2d.fromRadians(3.1415926535898 - Math.PI)), + new Pose2d(3.2067500000000, 4.0614305050000, Rotation2d.fromDegrees(0)), Apriltag.EIGHTEEN, Apriltag.SEVEN), BRANCH_B( - new Pose2d( - 3.2067500000000, 3.7328134950000, Rotation2d.fromRadians(3.1415926535898 - Math.PI)), + new Pose2d(3.2067500000000, 3.7328134950000, Rotation2d.fromRadians(0)), Apriltag.EIGHTEEN, Apriltag.SEVEN), BRANCH_C( - new Pose2d( - 3.8426981000607, 2.9769498582828, Rotation2d.fromRadians(4.1887902047864 - Math.PI)), + new Pose2d(3.8426981000607, 2.9769498582828, Rotation2d.fromDegrees(60)), Apriltag.SEVENTEEN, Apriltag.SIX), BRANCH_D( - new Pose2d( - 4.1272887788364, 2.8126413532828, Rotation2d.fromRadians(4.1887902047864 - Math.PI)), + new Pose2d(4.1272887788364, 2.8126413532828, Rotation2d.fromDegrees(60)), Apriltag.SEVENTEEN, Apriltag.SIX), BRANCH_E( - new Pose2d( - 5.0744349400607, 2.9414193532828, Rotation2d.fromRadians(5.2359877559830 - Math.PI)), + new Pose2d(5.0994349400607, 2.8981180830936, Rotation2d.fromDegrees(120)), Apriltag.TWENTY_TWO, Apriltag.ELEVEN), BRANCH_F( - new Pose2d( - 5.3590256188364, 3.1057278582828, Rotation2d.fromRadians(5.2359877559830 - Math.PI)), + new Pose2d(5.3590256188364, 3.1057278582828, Rotation2d.fromDegrees(120)), Apriltag.TWENTY_TWO, Apriltag.ELEVEN), BRANCH_G( - new Pose2d( - 5.7210736800000, 3.9903694950000, Rotation2d.fromRadians(6.2831853071796 - Math.PI)), + new Pose2d(5.7210736800000, 3.9903694950000, Rotation2d.fromDegrees(180)), Apriltag.TWENTY_ONE, Apriltag.TEN), BRANCH_H( - new Pose2d( - 5.7210736800000, 4.3189865050000, Rotation2d.fromRadians(6.2831853071796 - Math.PI)), + new Pose2d(5.7210736800000, 4.3189865050000, Rotation2d.fromDegrees(180)), Apriltag.TWENTY_ONE, Apriltag.TEN), BRANCH_I( - new Pose2d( - 5.1359755799393, 5.0748501417172, Rotation2d.fromRadians(7.3303828583762 - Math.PI)), + new Pose2d(5.1359755799393, 5.0748501417172, Rotation2d.fromDegrees(240)), Apriltag.TWENTY, Apriltag.NINE), BRANCH_J( - new Pose2d( - 4.8513849011636, 5.2391586467172, Rotation2d.fromRadians(7.3303828583762 - Math.PI)), + new Pose2d(4.8513849011636, 5.2391586467172, Rotation2d.fromDegrees(240)), Apriltag.TWENTY, Apriltag.NINE), BRANCH_K( - new Pose2d( - 3.9042387399393, 5.1103806467172, Rotation2d.fromRadians(8.3775804095728 - Math.PI)), + new Pose2d(3.9042387399393, 5.1103806467172, Rotation2d.fromDegrees(300)), Apriltag.NINETEEN, Apriltag.EIGHT), BRANCH_L( - new Pose2d( - 3.6196480611636, 4.9460721417172, Rotation2d.fromRadians(8.3775804095728 - Math.PI)), + new Pose2d(3.6196480611636, 4.9460721417172, Rotation2d.fromDegrees(300)), Apriltag.NINETEEN, Apriltag.EIGHT); - private final Pose2d position; + private final Pose2d bluePosition; + private final Pose2d redPosition; private final Apriltag blueTag; private final Apriltag redTag; AlignmentPosition(Pose2d position, Apriltag blueTag, Apriltag redTag) { - this.position = position; + this.bluePosition = position; + Rotation2d redRotation = Rotation2d.fromRadians(Math.PI - position.getRotation().getRadians()); + double redX = FieldLocation.LENGTH_OF_FIELD - bluePosition.getX(); + double redY = FieldLocation.HEIGHT_OF_FIELD - bluePosition.getY(); + this.redPosition = new Pose2d(redX, redY, redRotation); this.blueTag = blueTag; this.redTag = redTag; } + public Pose2d getBluePosition() { + return bluePosition; + } + + public Pose2d getRedPosition() { + return redPosition; + } + public Pose2d getPosition() { - return position; + Optional allianceColor = Robot.getAllianceColor(); + return allianceColor + .map( + alliance -> + alliance == DriverStation.Alliance.Blue ? getBluePosition() : getRedPosition()) + .orElse(new Pose2d()); } public static AlignmentPosition getClosest(Translation2d currentPosition) { int closestIndex = 0; - double closestDistance = values()[0].position.getTranslation().getDistance(currentPosition); + double closestDistance = values()[0].getPosition().getTranslation().getDistance(currentPosition); for (int i = 1; i < values().length; i++) { - double distance = values()[i].position.getTranslation().getDistance(currentPosition); + double distance = values()[i].getPosition().getTranslation().getDistance(currentPosition); if (distance < closestDistance) { closestDistance = distance; closestIndex = i; From 1ad5c7592a191bfe5b89e72cbb8cecb0bca0c9e4 Mon Sep 17 00:00:00 2001 From: codetoad Date: Mon, 24 Mar 2025 23:38:33 -0400 Subject: [PATCH 68/99] made it easy to paste new values for AlignmentPosition --- .../alignment/AlignClosestBranch.java | 5 +- .../robot/constants/AlignmentPosition.java | 65 +++++-------------- 2 files changed, 19 insertions(+), 51 deletions(-) diff --git a/src/main/java/frc/robot/commands/alignment/AlignClosestBranch.java b/src/main/java/frc/robot/commands/alignment/AlignClosestBranch.java index 9d42b5c8..ab215618 100644 --- a/src/main/java/frc/robot/commands/alignment/AlignClosestBranch.java +++ b/src/main/java/frc/robot/commands/alignment/AlignClosestBranch.java @@ -11,7 +11,7 @@ import org.littletonrobotics.junction.Logger; public class AlignClosestBranch extends LoggableCommand { - private final SwerveDrivetrain drivetrain; + private final SwerveDrivetrain drivetrain; private LoggableCommand followTrajectory; private final Timer timer = new Timer(); @@ -23,7 +23,8 @@ public AlignClosestBranch(SwerveDrivetrain drivetrain) { @Override public void initialize() { timer.restart(); - AlignmentPosition alignmentTarget = AlignmentPosition.getClosest(drivetrain.getPose().getTranslation()); + AlignmentPosition alignmentTarget = + AlignmentPosition.getClosest(drivetrain.getPose().getTranslation()); Logger.recordOutput("TargetReefAlignment", alignmentTarget); Pose2d position = alignmentTarget.getPosition(); Logger.recordOutput("TargetReefPose", position); diff --git a/src/main/java/frc/robot/constants/AlignmentPosition.java b/src/main/java/frc/robot/constants/AlignmentPosition.java index 7a7279a1..bb508e36 100644 --- a/src/main/java/frc/robot/constants/AlignmentPosition.java +++ b/src/main/java/frc/robot/constants/AlignmentPosition.java @@ -10,54 +10,20 @@ import java.util.Optional; public enum AlignmentPosition { - BRANCH_A( - new Pose2d(3.2067500000000, 4.0614305050000, Rotation2d.fromDegrees(0)), - Apriltag.EIGHTEEN, - Apriltag.SEVEN), - BRANCH_B( - new Pose2d(3.2067500000000, 3.7328134950000, Rotation2d.fromRadians(0)), - Apriltag.EIGHTEEN, - Apriltag.SEVEN), - BRANCH_C( - new Pose2d(3.8426981000607, 2.9769498582828, Rotation2d.fromDegrees(60)), - Apriltag.SEVENTEEN, - Apriltag.SIX), - BRANCH_D( - new Pose2d(4.1272887788364, 2.8126413532828, Rotation2d.fromDegrees(60)), - Apriltag.SEVENTEEN, - Apriltag.SIX), - BRANCH_E( - new Pose2d(5.0994349400607, 2.8981180830936, Rotation2d.fromDegrees(120)), - Apriltag.TWENTY_TWO, - Apriltag.ELEVEN), - BRANCH_F( - new Pose2d(5.3590256188364, 3.1057278582828, Rotation2d.fromDegrees(120)), - Apriltag.TWENTY_TWO, - Apriltag.ELEVEN), - BRANCH_G( - new Pose2d(5.7210736800000, 3.9903694950000, Rotation2d.fromDegrees(180)), - Apriltag.TWENTY_ONE, - Apriltag.TEN), - BRANCH_H( - new Pose2d(5.7210736800000, 4.3189865050000, Rotation2d.fromDegrees(180)), - Apriltag.TWENTY_ONE, - Apriltag.TEN), - BRANCH_I( - new Pose2d(5.1359755799393, 5.0748501417172, Rotation2d.fromDegrees(240)), - Apriltag.TWENTY, - Apriltag.NINE), - BRANCH_J( - new Pose2d(4.8513849011636, 5.2391586467172, Rotation2d.fromDegrees(240)), - Apriltag.TWENTY, - Apriltag.NINE), - BRANCH_K( - new Pose2d(3.9042387399393, 5.1103806467172, Rotation2d.fromDegrees(300)), - Apriltag.NINETEEN, - Apriltag.EIGHT), - BRANCH_L( - new Pose2d(3.6196480611636, 4.9460721417172, Rotation2d.fromDegrees(300)), - Apriltag.NINETEEN, - Apriltag.EIGHT); + // spotless:off Although this makes it harder to read it makes it easy to copy and paste from sheets + BRANCH_A(new Pose2d(3.2076000000000, 4.0614305050000, Rotation2d.fromDegrees(0)), Apriltag.EIGHTEEN, Apriltag.SEVEN), + BRANCH_B(new Pose2d(3.2076000000000, 3.7328134950000, Rotation2d.fromRadians(0)), Apriltag.EIGHTEEN, Apriltag.SEVEN), + BRANCH_C(new Pose2d(3.8176981000607, 2.9336485880936, Rotation2d.fromDegrees(60)), Apriltag.SEVENTEEN, Apriltag.SIX), + BRANCH_D(new Pose2d(4.1022887788364, 2.7693400830936, Rotation2d.fromDegrees(60)), Apriltag.SEVENTEEN, Apriltag.SIX), + BRANCH_E(new Pose2d(5.0994349400607, 2.8981180830936, Rotation2d.fromDegrees(120)), Apriltag.TWENTY_TWO, Apriltag.ELEVEN), + BRANCH_F(new Pose2d(5.3840256188364, 3.0624265880936, Rotation2d.fromDegrees(120)), Apriltag.TWENTY_TWO, Apriltag.ELEVEN), + BRANCH_G(new Pose2d(5.7710736800000, 3.9903694950000, Rotation2d.fromDegrees(180)), Apriltag.TWENTY_ONE, Apriltag.TEN), + BRANCH_H(new Pose2d(5.7710736800000, 4.3189865050000, Rotation2d.fromDegrees(180)), Apriltag.TWENTY_ONE, Apriltag.TEN), + BRANCH_I(new Pose2d(5.1609755799393, 5.1181514119064, Rotation2d.fromDegrees(240)), Apriltag.TWENTY, Apriltag.NINE), + BRANCH_J(new Pose2d(4.8763849011636, 5.2824599169064, Rotation2d.fromDegrees(240)), Apriltag.TWENTY, Apriltag.NINE), + BRANCH_K(new Pose2d(3.8792387399393, 5.1536819169064, Rotation2d.fromDegrees(300)), Apriltag.NINETEEN, Apriltag.EIGHT), + BRANCH_L(new Pose2d(3.5946480611636, 4.9893734119064, Rotation2d.fromDegrees(300)), Apriltag.NINETEEN, Apriltag.EIGHT); + // spotless:on private final Pose2d bluePosition; private final Pose2d redPosition; @@ -93,7 +59,8 @@ public Pose2d getPosition() { public static AlignmentPosition getClosest(Translation2d currentPosition) { int closestIndex = 0; - double closestDistance = values()[0].getPosition().getTranslation().getDistance(currentPosition); + double closestDistance = + values()[0].getPosition().getTranslation().getDistance(currentPosition); for (int i = 1; i < values().length; i++) { double distance = values()[i].getPosition().getTranslation().getDistance(currentPosition); if (distance < closestDistance) { From 2a1e4d19f65defc7ea9938e6988419505cf06ae2 Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Tue, 25 Mar 2025 08:51:58 -0400 Subject: [PATCH 69/99] fixed some pr --- .../java/frc/robot/utils/GamePieceLocate.java | 74 +++---------------- 1 file changed, 12 insertions(+), 62 deletions(-) diff --git a/src/main/java/frc/robot/utils/GamePieceLocate.java b/src/main/java/frc/robot/utils/GamePieceLocate.java index ac2cf463..410ea600 100644 --- a/src/main/java/frc/robot/utils/GamePieceLocate.java +++ b/src/main/java/frc/robot/utils/GamePieceLocate.java @@ -38,70 +38,20 @@ public static BranchPositions findCoralBranch(Pose2d robotPos, Vector pieceP double maxDot = Constants.MINIMUM_PIECE_DETECTION_DOT; BranchPositions closest = null; int n = CenterPositions.getClosest(robotPos); - if (n == 0) { - for (int i = 0; i < 9; i++) { - Matrix locationVec = - (invCameraRotation.times(PRECOMPUTED_BRANCH_VECS[i].minus(cameraPosVec))); - double dot = - pieceVec.dot( - VecBuilder.fill(locationVec.get(0, 0), locationVec.get(1, 0), locationVec.get(2, 0)) - .unit()); - if (dot > maxDot) { - maxDot = dot; - closest = BRANCHES[i]; - } - } - for (int i = 33; i < 36; i++) { - Matrix locationVec = - (invCameraRotation.times(PRECOMPUTED_BRANCH_VECS[i].minus(cameraPosVec))); - double dot = - pieceVec.dot( - VecBuilder.fill(locationVec.get(0, 0), locationVec.get(1, 0), locationVec.get(2, 0)) - .unit()); - if (dot > maxDot) { - maxDot = dot; - closest = BRANCHES[i]; - } - } - } else if (n == 5) { - for (int i = 0; i < 3; i++) { - Matrix locationVec = - (invCameraRotation.times(PRECOMPUTED_BRANCH_VECS[i].minus(cameraPosVec))); - double dot = - pieceVec.dot( - VecBuilder.fill(locationVec.get(0, 0), locationVec.get(1, 0), locationVec.get(2, 0)) - .unit()); - if (dot > maxDot) { - maxDot = dot; - closest = BRANCHES[i]; - } - } - for (int i = 6 * n - 3; i < 6 * n + 6; i++) { - Matrix locationVec = - (invCameraRotation.times(PRECOMPUTED_BRANCH_VECS[i].minus(cameraPosVec))); - double dot = - pieceVec.dot( - VecBuilder.fill(locationVec.get(0, 0), locationVec.get(1, 0), locationVec.get(2, 0)) - .unit()); - if (dot > maxDot) { - maxDot = dot; - closest = BRANCHES[i]; - } - } - } else { - for (int i = 6 * n - 3; i < 6 * n + 9; i++) { - Matrix locationVec = - (invCameraRotation.times(PRECOMPUTED_BRANCH_VECS[i].minus(cameraPosVec))); - double dot = - pieceVec.dot( - VecBuilder.fill(locationVec.get(0, 0), locationVec.get(1, 0), locationVec.get(2, 0)) - .unit()); - if (dot > maxDot) { - maxDot = dot; - closest = BRANCHES[i]; - } + for (int i = 6 * n - 3; i < 6 * n + 9; i++) { + int f = i % BRANCHES.length; + Matrix locationVec = + (invCameraRotation.times(PRECOMPUTED_BRANCH_VECS[f].minus(cameraPosVec))); + double dot = + pieceVec.dot( + VecBuilder.fill(locationVec.get(0, 0), locationVec.get(1, 0), locationVec.get(2, 0)) + .unit()); + if (dot > maxDot) { + maxDot = dot; + closest = BRANCHES[f]; } } + return closest; } From edb20ed8dc33bde7e4e52f384ceb0b201972e6cb Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Tue, 25 Mar 2025 09:08:30 -0400 Subject: [PATCH 70/99] fixed some pr --- src/main/java/frc/robot/constants/CenterPositions.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/constants/CenterPositions.java b/src/main/java/frc/robot/constants/CenterPositions.java index fd6746f4..1b23e264 100644 --- a/src/main/java/frc/robot/constants/CenterPositions.java +++ b/src/main/java/frc/robot/constants/CenterPositions.java @@ -22,9 +22,9 @@ public Translation2d getPosition() { } public static int getClosest(Pose2d robotPose) { - double closeDistance = 100; + double closeDistance = robotPose.getTranslation().getDistance(values()[0].getPosition()); int n = 0; - for (int i = 0; i < values().length; i++) { + for (int i = 1; i < values().length; i++) { double distance = robotPose.getTranslation().getDistance(values()[i].getPosition()); if (distance < closeDistance) { n = i; From 6573d0731ae0f103fa2c65ddb37a1cf15f0191d1 Mon Sep 17 00:00:00 2001 From: Code Toad <55628278+goatfanboi23@users.noreply.github.com> Date: Tue, 25 Mar 2025 11:52:29 -0400 Subject: [PATCH 71/99] Nh limelight (#298) * fixed visionIO * use limelightHelper to get all vision measurements and not just the one with the most confidence * get multiple readings and calculate detection * fixed --------- Co-authored-by: codetoad --- src/main/java/frc/robot/RobotContainer.java | 2 +- .../subsystems/limelight/RealVisionIO.java | 76 +- .../robot/subsystems/limelight/Vision.java | 55 +- .../subsystems/limelight/VisionInputs.java | 60 +- .../frc/robot/utils/LimelightHelpers.java | 1745 +++++++++++++++++ 5 files changed, 1872 insertions(+), 66 deletions(-) create mode 100644 src/main/java/frc/robot/utils/LimelightHelpers.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index fac33ead..e786ea1b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -164,7 +164,7 @@ public RobotContainer() { } } setupDriveTrain(); - vision = new Vision(new RealVisionIO(drivetrain)); + vision = new Vision(new RealVisionIO(), drivetrain::getPose); configureBindings(); setupAutoChooser(); putShuffleboardCommands(); diff --git a/src/main/java/frc/robot/subsystems/limelight/RealVisionIO.java b/src/main/java/frc/robot/subsystems/limelight/RealVisionIO.java index 3dbe2504..6bacb1b6 100644 --- a/src/main/java/frc/robot/subsystems/limelight/RealVisionIO.java +++ b/src/main/java/frc/robot/subsystems/limelight/RealVisionIO.java @@ -1,35 +1,25 @@ package frc.robot.subsystems.limelight; import edu.wpi.first.cscore.HttpCamera; -import edu.wpi.first.math.VecBuilder; import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableEntry; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import frc.robot.Robot; -import frc.robot.constants.AlgaePositions; -import frc.robot.constants.BranchPositions; import frc.robot.constants.Constants; -import frc.robot.subsystems.swervev3.SwerveDrivetrain; -import frc.robot.utils.GamePieceLocate; +import frc.robot.utils.LimelightHelpers; import frc.robot.utils.diag.DiagLimelight; import java.util.Map; public class RealVisionIO implements VisionIO { - private final NetworkTableEntry tv; - private final NetworkTableEntry tx; - private final NetworkTableEntry ty; - private final SwerveDrivetrain drivetrain; + private LimelightHelpers.LimelightResults results; + private final NetworkTableEntry ledModeEntry; - public RealVisionIO(SwerveDrivetrain drivetrain) { - this.drivetrain = drivetrain; - NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight"); - tv = table.getEntry("tv"); - tx = table.getEntry("tx"); - ty = table.getEntry("ty"); + public RealVisionIO() { Robot.getDiagnostics().addDiagnosable(new DiagLimelight("Vision", "Piece Seen")); - + NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight"); + ledModeEntry = table.getEntry("ledMode"); HttpCamera limelightFeed = new HttpCamera( "limelight", "http://" + Constants.LIMELIGHT_IP_ADDRESS + ":5800/stream.mjpg"); @@ -43,30 +33,36 @@ public RealVisionIO(SwerveDrivetrain drivetrain) { @Override public void updateInputs(VisionInputs inputs) { - inputs.tv = tv.getDouble(0); - inputs.tx = tx.getDouble(0); - inputs.ty = ty.getDouble(0); - if (inputs.tv != 0) { - BranchPositions[] newCoralArray = new BranchPositions[inputs.coralSeen.length + 1]; - AlgaePositions[] newAlgaeArray = new AlgaePositions[inputs.algaeSeen.length + 1]; - int oldCoralArrayLength = inputs.coralSeen.length; - int oldAlgaeArrayLength = inputs.algaeSeen.length; - for (int i = 0; i < oldCoralArrayLength; i++) { - newCoralArray[i] = inputs.coralSeen[i]; - } - newCoralArray[oldCoralArrayLength + 1] = - GamePieceLocate.findCoralBranch( - drivetrain.getPose(), VecBuilder.fill(tx.getDouble(0), ty.getDouble(0))); - - for (int i = 0; i < oldAlgaeArrayLength; i++) { - newAlgaeArray[i] = inputs.algaeSeen[i]; - } - newAlgaeArray[oldAlgaeArrayLength + 1] = - GamePieceLocate.findAlgaePos( - drivetrain.getPose(), VecBuilder.fill(tx.getDouble(0), ty.getDouble(0))); - - inputs.coralSeen = newCoralArray; - inputs.algaeSeen = newAlgaeArray; + // this might take a while so we might now want to update this every tick and do it in another + // thread. + results = LimelightHelpers.getLatestResults(""); + LimelightHelpers.LimelightTarget_Detector[] targetsDetector = results.targets_Detector; + int detectionLength = targetsDetector.length; + inputs.className = new String[detectionLength]; + inputs.classID = new double[detectionLength]; + inputs.confidence = new double[detectionLength]; + inputs.ta = new double[detectionLength]; + inputs.tx = new double[detectionLength]; + inputs.ty = new double[detectionLength]; + inputs.tx_pixels = new double[detectionLength]; + inputs.ty_pixels = new double[detectionLength]; + for (int i = 0; i < detectionLength; i++) { + inputs.className[i] = targetsDetector[i].className; + inputs.classID[i] = targetsDetector[i].classID; + inputs.confidence[i] = targetsDetector[i].confidence; + inputs.ta[i] = targetsDetector[i].ta; + inputs.tx[i] = targetsDetector[i].tx; + inputs.ty[i] = targetsDetector[i].ty; + inputs.tx_pixels[i] = targetsDetector[i].tx_pixels; + inputs.ty_pixels[i] = targetsDetector[i].ty_pixels; } + inputs.pipelineID = results.pipelineID; + inputs.latency_pipeline = results.latency_pipeline; + inputs.latency_capture = results.latency_capture; + inputs.latency_jsonParse = results.latency_jsonParse; + inputs.timestamp_LIMELIGHT_publish = results.timestamp_LIMELIGHT_publish; + inputs.timestamp_RIOFPGA_capture = results.timestamp_RIOFPGA_capture; + inputs.valid = results.valid; + inputs.ledMode = (int) ledModeEntry.getInteger(-1); } } diff --git a/src/main/java/frc/robot/subsystems/limelight/Vision.java b/src/main/java/frc/robot/subsystems/limelight/Vision.java index 494e4954..5bba6afc 100644 --- a/src/main/java/frc/robot/subsystems/limelight/Vision.java +++ b/src/main/java/frc/robot/subsystems/limelight/Vision.java @@ -4,53 +4,84 @@ package frc.robot.subsystems.limelight; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.constants.AlgaePositions; import frc.robot.constants.BranchPositions; +import frc.robot.utils.GamePieceLocate; import frc.robot.utils.logging.subsystem.LoggableSystem; -import frc.robot.utils.shuffleboard.SmartShuffleboard; +import java.util.ArrayList; +import java.util.function.Supplier; +import org.littletonrobotics.junction.AutoLogOutput; public class Vision extends SubsystemBase { LoggableSystem system; + private final Supplier pose2dSupplier; + ArrayList currentAlgaePosition = new ArrayList<>(); + ArrayList currentCoralPositions = new ArrayList<>(); - public Vision(VisionIO io) { - system = new LoggableSystem<>(io, new VisionInputs("limelight")); + public Vision(VisionIO io, Supplier pose2dSupplier) { + this.system = new LoggableSystem<>(io, new VisionInputs("limelight")); + this.pose2dSupplier = pose2dSupplier; } /** * @return The piece's x offset angle in degrees and 0.0 if the piece isn't seen */ - public double getPieceOffsetAngleX() { + public double[] getPieceOffsetAngleX(int detectionIndex) { return system.getInputs().tx; } /** * @return The piece's y offset angle in degrees and 0.0 if the piece isn't seen */ - public double getPieceOffsetAngleY() { + public double[] getPieceOffsetAngleY() { return system.getInputs().ty; } public boolean isPieceSeen() { - return system.getInputs().tv != 0; + return system.getInputs().valid; } + @AutoLogOutput public BranchPositions[] getAllBranchPosition() { - return system.getInputs().coralSeen; + return currentCoralPositions.toArray(BranchPositions[]::new); } + @AutoLogOutput public AlgaePositions[] getAllAlgaePosition() { - return system.getInputs().algaeSeen; + return currentAlgaePosition.toArray(AlgaePositions[]::new); } @Override public void periodic() { system.updateInputs(); - if (getAllAlgaePosition() != null) { - SmartShuffleboard.put("peice pos", "Algae", getAllAlgaePosition()); + locateGamePieces(); + } + + private void locateGamePieces() { + currentAlgaePosition.clear(); + currentCoralPositions.clear(); + int detectionLength = system.getInputs().tx.length; + if (!system.getInputs().valid) { + return; } - if (getAllBranchPosition() != null) { - SmartShuffleboard.put("peice pos", "Coral", getAllBranchPosition()); + for (int i = 0; i < detectionLength; i++) { + String className = system.getInputs().className[i]; + if (className.equalsIgnoreCase("algae")) { + BranchPositions coralBranch = + GamePieceLocate.findCoralBranch( + pose2dSupplier.get(), + VecBuilder.fill(system.getInputs().tx[i], system.getInputs().ty[i])); + currentCoralPositions.add(coralBranch); + } else if (className.equalsIgnoreCase("coral")) { + AlgaePositions algaePos = + GamePieceLocate.findAlgaePos( + pose2dSupplier.get(), + VecBuilder.fill(system.getInputs().tx[i], system.getInputs().ty[i])); + currentAlgaePosition.add(algaePos); + } } } } diff --git a/src/main/java/frc/robot/subsystems/limelight/VisionInputs.java b/src/main/java/frc/robot/subsystems/limelight/VisionInputs.java index 7d141965..03ef64ff 100644 --- a/src/main/java/frc/robot/subsystems/limelight/VisionInputs.java +++ b/src/main/java/frc/robot/subsystems/limelight/VisionInputs.java @@ -1,16 +1,27 @@ package frc.robot.subsystems.limelight; -import frc.robot.constants.AlgaePositions; -import frc.robot.constants.BranchPositions; import frc.robot.utils.logging.subsystem.FolderLoggableInputs; import org.littletonrobotics.junction.LogTable; public class VisionInputs extends FolderLoggableInputs { - public double tv = 0; - public double tx = 0; - public double ty = 0; - public BranchPositions[] coralSeen; - public AlgaePositions[] algaeSeen; + public double pipelineID; + public double latency_pipeline; + public double latency_capture; + public double latency_jsonParse; + public double timestamp_LIMELIGHT_publish; + public double timestamp_RIOFPGA_capture; + public boolean valid; + public int ledMode = -1; + + // from LimelightHelpers.LimelightTarget_Detector[] + public String[] className = new String[0]; + public double[] classID = new double[0]; + public double[] confidence = new double[0]; + public double[] ta = new double[0]; + public double[] tx = new double[0]; + public double[] ty = new double[0]; + public double[] tx_pixels = new double[0]; + public double[] ty_pixels = new double[0]; public VisionInputs(String folder) { super(folder); @@ -18,19 +29,42 @@ public VisionInputs(String folder) { @Override public void toLog(LogTable table) { - table.put("tv", tv); + table.put("pipelineID", pipelineID); + table.put("latency_pipeline", latency_pipeline); + table.put("latency_capture", latency_capture); + table.put("latency_jsonParse", latency_jsonParse); + table.put("timestamp_LIMELIGHT_publish", timestamp_LIMELIGHT_publish); + table.put("timestamp_RIOFPGA_capture", timestamp_RIOFPGA_capture); + table.put("valid", valid); + table.put("classNames", className); + table.put("classID", classID); + table.put("confidence", confidence); + table.put("ta", ta); table.put("tx", tx); table.put("ty", ty); - table.put("coralSeen", coralSeen); - table.put("algaeSeen", algaeSeen); + table.put("tx_pixels", tx_pixels); + table.put("ty_pixels", ty_pixels); + table.put("ledMode", ledMode); } @Override public void fromLog(LogTable table) { - tv = table.get("tv", tv); + pipelineID = table.get("pipelineID", pipelineID); + latency_pipeline = table.get("latency_pipeline", latency_pipeline); + latency_capture = table.get("latency_capture", latency_capture); + latency_jsonParse = table.get("latency_jsonParse", latency_jsonParse); + timestamp_LIMELIGHT_publish = + table.get("timestamp_LIMELIGHT_publish", timestamp_LIMELIGHT_publish); + timestamp_RIOFPGA_capture = table.get("timestamp_RIOFPGA_capture", timestamp_RIOFPGA_capture); + valid = table.get("valid", valid); + className = table.get("classNames", className); + classID = table.get("classID", classID); + confidence = table.get("confidence", confidence); + ta = table.get("ta", ta); tx = table.get("tx", tx); ty = table.get("ty", ty); - coralSeen = table.get("coralSeen", coralSeen); - algaeSeen = table.get("algaeSeen", algaeSeen); + tx_pixels = table.get("tx_pixels", tx_pixels); + ty_pixels = table.get("ty_pixels", ty_pixels); + ledMode = table.get("ledMode", ledMode); } } diff --git a/src/main/java/frc/robot/utils/LimelightHelpers.java b/src/main/java/frc/robot/utils/LimelightHelpers.java new file mode 100644 index 00000000..b03f4d20 --- /dev/null +++ b/src/main/java/frc/robot/utils/LimelightHelpers.java @@ -0,0 +1,1745 @@ +// LimelightHelpers v1.12 (REQUIRES LLOS 2025.0 OR LATER) + +package frc.robot.utils; + +import com.fasterxml.jackson.annotation.JsonFormat; +import com.fasterxml.jackson.annotation.JsonFormat.Shape; +import com.fasterxml.jackson.annotation.JsonProperty; +import com.fasterxml.jackson.core.JsonProcessingException; +import com.fasterxml.jackson.databind.DeserializationFeature; +import com.fasterxml.jackson.databind.ObjectMapper; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.networktables.DoubleArrayEntry; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableEntry; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.TimestampedDoubleArray; +import java.io.IOException; +import java.net.HttpURLConnection; +import java.net.MalformedURLException; +import java.net.URL; +import java.util.Arrays; +import java.util.Map; +import java.util.concurrent.CompletableFuture; +import java.util.concurrent.ConcurrentHashMap; + +/** + * LimelightHelpers provides static methods and classes for interfacing with Limelight vision + * cameras in FRC. This library supports all Limelight features including AprilTag tracking, Neural + * Networks, and standard color/retroreflective tracking. + */ +public class LimelightHelpers { + + private static final Map doubleArrayEntries = new ConcurrentHashMap<>(); + + /** Represents a Color/Retroreflective Target Result extracted from JSON Output */ + public static class LimelightTarget_Retro { + + @JsonProperty("t6c_ts") + private double[] cameraPose_TargetSpace; + + @JsonProperty("t6r_fs") + private double[] robotPose_FieldSpace; + + @JsonProperty("t6r_ts") + private double[] robotPose_TargetSpace; + + @JsonProperty("t6t_cs") + private double[] targetPose_CameraSpace; + + @JsonProperty("t6t_rs") + private double[] targetPose_RobotSpace; + + public Pose3d getCameraPose_TargetSpace() { + return toPose3D(cameraPose_TargetSpace); + } + + public Pose3d getRobotPose_FieldSpace() { + return toPose3D(robotPose_FieldSpace); + } + + public Pose3d getRobotPose_TargetSpace() { + return toPose3D(robotPose_TargetSpace); + } + + public Pose3d getTargetPose_CameraSpace() { + return toPose3D(targetPose_CameraSpace); + } + + public Pose3d getTargetPose_RobotSpace() { + return toPose3D(targetPose_RobotSpace); + } + + public Pose2d getCameraPose_TargetSpace2D() { + return toPose2D(cameraPose_TargetSpace); + } + + public Pose2d getRobotPose_FieldSpace2D() { + return toPose2D(robotPose_FieldSpace); + } + + public Pose2d getRobotPose_TargetSpace2D() { + return toPose2D(robotPose_TargetSpace); + } + + public Pose2d getTargetPose_CameraSpace2D() { + return toPose2D(targetPose_CameraSpace); + } + + public Pose2d getTargetPose_RobotSpace2D() { + return toPose2D(targetPose_RobotSpace); + } + + @JsonProperty("ta") + public double ta; + + @JsonProperty("tx") + public double tx; + + @JsonProperty("ty") + public double ty; + + @JsonProperty("txp") + public double tx_pixels; + + @JsonProperty("typ") + public double ty_pixels; + + @JsonProperty("tx_nocross") + public double tx_nocrosshair; + + @JsonProperty("ty_nocross") + public double ty_nocrosshair; + + @JsonProperty("ts") + public double ts; + + public LimelightTarget_Retro() { + cameraPose_TargetSpace = new double[6]; + robotPose_FieldSpace = new double[6]; + robotPose_TargetSpace = new double[6]; + targetPose_CameraSpace = new double[6]; + targetPose_RobotSpace = new double[6]; + } + } + + /** Represents an AprilTag/Fiducial Target Result extracted from JSON Output */ + public static class LimelightTarget_Fiducial { + + @JsonProperty("fID") + public double fiducialID; + + @JsonProperty("fam") + public String fiducialFamily; + + @JsonProperty("t6c_ts") + private double[] cameraPose_TargetSpace; + + @JsonProperty("t6r_fs") + private double[] robotPose_FieldSpace; + + @JsonProperty("t6r_ts") + private double[] robotPose_TargetSpace; + + @JsonProperty("t6t_cs") + private double[] targetPose_CameraSpace; + + @JsonProperty("t6t_rs") + private double[] targetPose_RobotSpace; + + public Pose3d getCameraPose_TargetSpace() { + return toPose3D(cameraPose_TargetSpace); + } + + public Pose3d getRobotPose_FieldSpace() { + return toPose3D(robotPose_FieldSpace); + } + + public Pose3d getRobotPose_TargetSpace() { + return toPose3D(robotPose_TargetSpace); + } + + public Pose3d getTargetPose_CameraSpace() { + return toPose3D(targetPose_CameraSpace); + } + + public Pose3d getTargetPose_RobotSpace() { + return toPose3D(targetPose_RobotSpace); + } + + public Pose2d getCameraPose_TargetSpace2D() { + return toPose2D(cameraPose_TargetSpace); + } + + public Pose2d getRobotPose_FieldSpace2D() { + return toPose2D(robotPose_FieldSpace); + } + + public Pose2d getRobotPose_TargetSpace2D() { + return toPose2D(robotPose_TargetSpace); + } + + public Pose2d getTargetPose_CameraSpace2D() { + return toPose2D(targetPose_CameraSpace); + } + + public Pose2d getTargetPose_RobotSpace2D() { + return toPose2D(targetPose_RobotSpace); + } + + @JsonProperty("ta") + public double ta; + + @JsonProperty("tx") + public double tx; + + @JsonProperty("ty") + public double ty; + + @JsonProperty("txp") + public double tx_pixels; + + @JsonProperty("typ") + public double ty_pixels; + + @JsonProperty("tx_nocross") + public double tx_nocrosshair; + + @JsonProperty("ty_nocross") + public double ty_nocrosshair; + + @JsonProperty("ts") + public double ts; + + public LimelightTarget_Fiducial() { + cameraPose_TargetSpace = new double[6]; + robotPose_FieldSpace = new double[6]; + robotPose_TargetSpace = new double[6]; + targetPose_CameraSpace = new double[6]; + targetPose_RobotSpace = new double[6]; + } + } + + /** Represents a Barcode Target Result extracted from JSON Output */ + public static class LimelightTarget_Barcode { + + /** Barcode family type (e.g. "QR", "DataMatrix", etc.) */ + @JsonProperty("fam") + public String family; + + /** Gets the decoded data content of the barcode */ + @JsonProperty("data") + public String data; + + @JsonProperty("txp") + public double tx_pixels; + + @JsonProperty("typ") + public double ty_pixels; + + @JsonProperty("tx") + public double tx; + + @JsonProperty("ty") + public double ty; + + @JsonProperty("tx_nocross") + public double tx_nocrosshair; + + @JsonProperty("ty_nocross") + public double ty_nocrosshair; + + @JsonProperty("ta") + public double ta; + + @JsonProperty("pts") + public double[][] corners; + + public LimelightTarget_Barcode() {} + + public String getFamily() { + return family; + } + } + + /** Represents a Neural Classifier Pipeline Result extracted from JSON Output */ + public static class LimelightTarget_Classifier { + + @JsonProperty("class") + public String className; + + @JsonProperty("classID") + public double classID; + + @JsonProperty("conf") + public double confidence; + + @JsonProperty("zone") + public double zone; + + @JsonProperty("tx") + public double tx; + + @JsonProperty("txp") + public double tx_pixels; + + @JsonProperty("ty") + public double ty; + + @JsonProperty("typ") + public double ty_pixels; + + public LimelightTarget_Classifier() {} + } + + /** Represents a Neural Detector Pipeline Result extracted from JSON Output */ + public static class LimelightTarget_Detector { + + @JsonProperty("class") + public String className; + + @JsonProperty("classID") + public double classID; + + @JsonProperty("conf") + public double confidence; + + @JsonProperty("ta") + public double ta; + + @JsonProperty("tx") + public double tx; + + @JsonProperty("ty") + public double ty; + + @JsonProperty("txp") + public double tx_pixels; + + @JsonProperty("typ") + public double ty_pixels; + + @JsonProperty("tx_nocross") + public double tx_nocrosshair; + + @JsonProperty("ty_nocross") + public double ty_nocrosshair; + + public LimelightTarget_Detector() {} + } + + /** Limelight Results object, parsed from a Limelight's JSON results output. */ + public static class LimelightResults { + + public String error; + + @JsonProperty("pID") + public double pipelineID; + + @JsonProperty("tl") + public double latency_pipeline; + + @JsonProperty("cl") + public double latency_capture; + + public double latency_jsonParse; + + @JsonProperty("ts") + public double timestamp_LIMELIGHT_publish; + + @JsonProperty("ts_rio") + public double timestamp_RIOFPGA_capture; + + @JsonProperty("v") + @JsonFormat(shape = Shape.NUMBER) + public boolean valid; + + @JsonProperty("botpose") + public double[] botpose; + + @JsonProperty("botpose_wpired") + public double[] botpose_wpired; + + @JsonProperty("botpose_wpiblue") + public double[] botpose_wpiblue; + + @JsonProperty("botpose_tagcount") + public double botpose_tagcount; + + @JsonProperty("botpose_span") + public double botpose_span; + + @JsonProperty("botpose_avgdist") + public double botpose_avgdist; + + @JsonProperty("botpose_avgarea") + public double botpose_avgarea; + + @JsonProperty("t6c_rs") + public double[] camerapose_robotspace; + + public Pose3d getBotPose3d() { + return toPose3D(botpose); + } + + public Pose3d getBotPose3d_wpiRed() { + return toPose3D(botpose_wpired); + } + + public Pose3d getBotPose3d_wpiBlue() { + return toPose3D(botpose_wpiblue); + } + + public Pose2d getBotPose2d() { + return toPose2D(botpose); + } + + public Pose2d getBotPose2d_wpiRed() { + return toPose2D(botpose_wpired); + } + + public Pose2d getBotPose2d_wpiBlue() { + return toPose2D(botpose_wpiblue); + } + + @JsonProperty("Retro") + public LimelightTarget_Retro[] targets_Retro; + + @JsonProperty("Fiducial") + public LimelightTarget_Fiducial[] targets_Fiducials; + + @JsonProperty("Classifier") + public LimelightTarget_Classifier[] targets_Classifier; + + @JsonProperty("Detector") + public LimelightTarget_Detector[] targets_Detector; + + @JsonProperty("Barcode") + public LimelightTarget_Barcode[] targets_Barcode; + + public LimelightResults() { + botpose = new double[6]; + botpose_wpired = new double[6]; + botpose_wpiblue = new double[6]; + camerapose_robotspace = new double[6]; + targets_Retro = new LimelightTarget_Retro[0]; + targets_Fiducials = new LimelightTarget_Fiducial[0]; + targets_Classifier = new LimelightTarget_Classifier[0]; + targets_Detector = new LimelightTarget_Detector[0]; + targets_Barcode = new LimelightTarget_Barcode[0]; + } + } + + /** Represents a Limelight Raw Fiducial result from Limelight's NetworkTables output. */ + public static class RawFiducial { + public int id = 0; + public double txnc = 0; + public double tync = 0; + public double ta = 0; + public double distToCamera = 0; + public double distToRobot = 0; + public double ambiguity = 0; + + public RawFiducial( + int id, + double txnc, + double tync, + double ta, + double distToCamera, + double distToRobot, + double ambiguity) { + this.id = id; + this.txnc = txnc; + this.tync = tync; + this.ta = ta; + this.distToCamera = distToCamera; + this.distToRobot = distToRobot; + this.ambiguity = ambiguity; + } + + @Override + public boolean equals(Object obj) { + if (this == obj) return true; + if (obj == null || getClass() != obj.getClass()) return false; + RawFiducial other = (RawFiducial) obj; + return id == other.id + && Double.compare(txnc, other.txnc) == 0 + && Double.compare(tync, other.tync) == 0 + && Double.compare(ta, other.ta) == 0 + && Double.compare(distToCamera, other.distToCamera) == 0 + && Double.compare(distToRobot, other.distToRobot) == 0 + && Double.compare(ambiguity, other.ambiguity) == 0; + } + } + + /** Represents a Limelight Raw Neural Detector result from Limelight's NetworkTables output. */ + public static class RawDetection { + public int classId = 0; + public double txnc = 0; + public double tync = 0; + public double ta = 0; + public double corner0_X = 0; + public double corner0_Y = 0; + public double corner1_X = 0; + public double corner1_Y = 0; + public double corner2_X = 0; + public double corner2_Y = 0; + public double corner3_X = 0; + public double corner3_Y = 0; + + public RawDetection( + int classId, + double txnc, + double tync, + double ta, + double corner0_X, + double corner0_Y, + double corner1_X, + double corner1_Y, + double corner2_X, + double corner2_Y, + double corner3_X, + double corner3_Y) { + this.classId = classId; + this.txnc = txnc; + this.tync = tync; + this.ta = ta; + this.corner0_X = corner0_X; + this.corner0_Y = corner0_Y; + this.corner1_X = corner1_X; + this.corner1_Y = corner1_Y; + this.corner2_X = corner2_X; + this.corner2_Y = corner2_Y; + this.corner3_X = corner3_X; + this.corner3_Y = corner3_Y; + } + } + + /** Represents a 3D Pose Estimate. */ + public static class PoseEstimate { + public Pose2d pose; + public double timestampSeconds; + public double latency; + public int tagCount; + public double tagSpan; + public double avgTagDist; + public double avgTagArea; + + public RawFiducial[] rawFiducials; + public boolean isMegaTag2; + + /** Instantiates a PoseEstimate object with default values */ + public PoseEstimate() { + this.pose = new Pose2d(); + this.timestampSeconds = 0; + this.latency = 0; + this.tagCount = 0; + this.tagSpan = 0; + this.avgTagDist = 0; + this.avgTagArea = 0; + this.rawFiducials = new RawFiducial[] {}; + this.isMegaTag2 = false; + } + + public PoseEstimate( + Pose2d pose, + double timestampSeconds, + double latency, + int tagCount, + double tagSpan, + double avgTagDist, + double avgTagArea, + RawFiducial[] rawFiducials, + boolean isMegaTag2) { + + this.pose = pose; + this.timestampSeconds = timestampSeconds; + this.latency = latency; + this.tagCount = tagCount; + this.tagSpan = tagSpan; + this.avgTagDist = avgTagDist; + this.avgTagArea = avgTagArea; + this.rawFiducials = rawFiducials; + this.isMegaTag2 = isMegaTag2; + } + + @Override + public boolean equals(Object obj) { + if (this == obj) return true; + if (obj == null || getClass() != obj.getClass()) return false; + PoseEstimate that = (PoseEstimate) obj; + // We don't compare the timestampSeconds as it isn't relevant for equality and makes + // unit testing harder + return Double.compare(that.latency, latency) == 0 + && tagCount == that.tagCount + && Double.compare(that.tagSpan, tagSpan) == 0 + && Double.compare(that.avgTagDist, avgTagDist) == 0 + && Double.compare(that.avgTagArea, avgTagArea) == 0 + && pose.equals(that.pose) + && Arrays.equals(rawFiducials, that.rawFiducials); + } + } + + /** Encapsulates the state of an internal Limelight IMU. */ + public static class IMUData { + public double robotYaw = 0.0; + public double Roll = 0.0; + public double Pitch = 0.0; + public double Yaw = 0.0; + public double gyroX = 0.0; + public double gyroY = 0.0; + public double gyroZ = 0.0; + public double accelX = 0.0; + public double accelY = 0.0; + public double accelZ = 0.0; + + public IMUData() {} + + public IMUData(double[] imuData) { + if (imuData != null && imuData.length >= 10) { + this.robotYaw = imuData[0]; + this.Roll = imuData[1]; + this.Pitch = imuData[2]; + this.Yaw = imuData[3]; + this.gyroX = imuData[4]; + this.gyroY = imuData[5]; + this.gyroZ = imuData[6]; + this.accelX = imuData[7]; + this.accelY = imuData[8]; + this.accelZ = imuData[9]; + } + } + } + + private static ObjectMapper mapper; + + /** Print JSON Parse time to the console in milliseconds */ + static boolean profileJSON = false; + + static final String sanitizeName(String name) { + if ("".equals(name) || name == null) { + return "limelight"; + } + return name; + } + + /** + * Takes a 6-length array of pose data and converts it to a Pose3d object. Array format: [x, y, z, + * roll, pitch, yaw] where angles are in degrees. + * + * @param inData Array containing pose data [x, y, z, roll, pitch, yaw] + * @return Pose3d object representing the pose, or empty Pose3d if invalid data + */ + public static Pose3d toPose3D(double[] inData) { + if (inData.length < 6) { + // System.err.println("Bad LL 3D Pose Data!"); + return new Pose3d(); + } + return new Pose3d( + new Translation3d(inData[0], inData[1], inData[2]), + new Rotation3d( + Units.degreesToRadians(inData[3]), + Units.degreesToRadians(inData[4]), + Units.degreesToRadians(inData[5]))); + } + + /** + * Takes a 6-length array of pose data and converts it to a Pose2d object. Uses only x, y, and yaw + * components, ignoring z, roll, and pitch. Array format: [x, y, z, roll, pitch, yaw] where angles + * are in degrees. + * + * @param inData Array containing pose data [x, y, z, roll, pitch, yaw] + * @return Pose2d object representing the pose, or empty Pose2d if invalid data + */ + public static Pose2d toPose2D(double[] inData) { + if (inData.length < 6) { + // System.err.println("Bad LL 2D Pose Data!"); + return new Pose2d(); + } + Translation2d tran2d = new Translation2d(inData[0], inData[1]); + Rotation2d r2d = new Rotation2d(Units.degreesToRadians(inData[5])); + return new Pose2d(tran2d, r2d); + } + + /** + * Converts a Pose3d object to an array of doubles in the format [x, y, z, roll, pitch, yaw]. + * Translation components are in meters, rotation components are in degrees. + * + * @param pose The Pose3d object to convert + * @return A 6-element array containing [x, y, z, roll, pitch, yaw] + */ + public static double[] pose3dToArray(Pose3d pose) { + double[] result = new double[6]; + result[0] = pose.getTranslation().getX(); + result[1] = pose.getTranslation().getY(); + result[2] = pose.getTranslation().getZ(); + result[3] = Units.radiansToDegrees(pose.getRotation().getX()); + result[4] = Units.radiansToDegrees(pose.getRotation().getY()); + result[5] = Units.radiansToDegrees(pose.getRotation().getZ()); + return result; + } + + /** + * Converts a Pose2d object to an array of doubles in the format [x, y, z, roll, pitch, yaw]. + * Translation components are in meters, rotation components are in degrees. Note: z, roll, and + * pitch will be 0 since Pose2d only contains x, y, and yaw. + * + * @param pose The Pose2d object to convert + * @return A 6-element array containing [x, y, 0, 0, 0, yaw] + */ + public static double[] pose2dToArray(Pose2d pose) { + double[] result = new double[6]; + result[0] = pose.getTranslation().getX(); + result[1] = pose.getTranslation().getY(); + result[2] = 0; + result[3] = Units.radiansToDegrees(0); + result[4] = Units.radiansToDegrees(0); + result[5] = Units.radiansToDegrees(pose.getRotation().getRadians()); + return result; + } + + private static double extractArrayEntry(double[] inData, int position) { + if (inData.length < position + 1) { + return 0; + } + return inData[position]; + } + + private static PoseEstimate getBotPoseEstimate( + String limelightName, String entryName, boolean isMegaTag2) { + DoubleArrayEntry poseEntry = + LimelightHelpers.getLimelightDoubleArrayEntry(limelightName, entryName); + + TimestampedDoubleArray tsValue = poseEntry.getAtomic(); + double[] poseArray = tsValue.value; + long timestamp = tsValue.timestamp; + + if (poseArray.length == 0) { + // Handle the case where no data is available + return null; // or some default PoseEstimate + } + + var pose = toPose2D(poseArray); + double latency = extractArrayEntry(poseArray, 6); + int tagCount = (int) extractArrayEntry(poseArray, 7); + double tagSpan = extractArrayEntry(poseArray, 8); + double tagDist = extractArrayEntry(poseArray, 9); + double tagArea = extractArrayEntry(poseArray, 10); + + // Convert server timestamp from microseconds to seconds and adjust for latency + double adjustedTimestamp = (timestamp / 1000000.0) - (latency / 1000.0); + + RawFiducial[] rawFiducials = new RawFiducial[tagCount]; + int valsPerFiducial = 7; + int expectedTotalVals = 11 + valsPerFiducial * tagCount; + + if (poseArray.length != expectedTotalVals) { + // Don't populate fiducials + } else { + for (int i = 0; i < tagCount; i++) { + int baseIndex = 11 + (i * valsPerFiducial); + int id = (int) poseArray[baseIndex]; + double txnc = poseArray[baseIndex + 1]; + double tync = poseArray[baseIndex + 2]; + double ta = poseArray[baseIndex + 3]; + double distToCamera = poseArray[baseIndex + 4]; + double distToRobot = poseArray[baseIndex + 5]; + double ambiguity = poseArray[baseIndex + 6]; + rawFiducials[i] = new RawFiducial(id, txnc, tync, ta, distToCamera, distToRobot, ambiguity); + } + } + + return new PoseEstimate( + pose, + adjustedTimestamp, + latency, + tagCount, + tagSpan, + tagDist, + tagArea, + rawFiducials, + isMegaTag2); + } + + /** + * Gets the latest raw fiducial/AprilTag detection results from NetworkTables. + * + * @param limelightName Name/identifier of the Limelight + * @return Array of RawFiducial objects containing detection details + */ + public static RawFiducial[] getRawFiducials(String limelightName) { + var entry = LimelightHelpers.getLimelightNTTableEntry(limelightName, "rawfiducials"); + var rawFiducialArray = entry.getDoubleArray(new double[0]); + int valsPerEntry = 7; + if (rawFiducialArray.length % valsPerEntry != 0) { + return new RawFiducial[0]; + } + + int numFiducials = rawFiducialArray.length / valsPerEntry; + RawFiducial[] rawFiducials = new RawFiducial[numFiducials]; + + for (int i = 0; i < numFiducials; i++) { + int baseIndex = i * valsPerEntry; + int id = (int) extractArrayEntry(rawFiducialArray, baseIndex); + double txnc = extractArrayEntry(rawFiducialArray, baseIndex + 1); + double tync = extractArrayEntry(rawFiducialArray, baseIndex + 2); + double ta = extractArrayEntry(rawFiducialArray, baseIndex + 3); + double distToCamera = extractArrayEntry(rawFiducialArray, baseIndex + 4); + double distToRobot = extractArrayEntry(rawFiducialArray, baseIndex + 5); + double ambiguity = extractArrayEntry(rawFiducialArray, baseIndex + 6); + + rawFiducials[i] = new RawFiducial(id, txnc, tync, ta, distToCamera, distToRobot, ambiguity); + } + + return rawFiducials; + } + + /** + * Gets the latest raw neural detector results from NetworkTables + * + * @param limelightName Name/identifier of the Limelight + * @return Array of RawDetection objects containing detection details + */ + public static RawDetection[] getRawDetections(String limelightName) { + var entry = LimelightHelpers.getLimelightNTTableEntry(limelightName, "rawdetections"); + var rawDetectionArray = entry.getDoubleArray(new double[0]); + int valsPerEntry = 12; + if (rawDetectionArray.length % valsPerEntry != 0) { + return new RawDetection[0]; + } + + int numDetections = rawDetectionArray.length / valsPerEntry; + RawDetection[] rawDetections = new RawDetection[numDetections]; + + for (int i = 0; i < numDetections; i++) { + int baseIndex = i * valsPerEntry; // Starting index for this detection's data + int classId = (int) extractArrayEntry(rawDetectionArray, baseIndex); + double txnc = extractArrayEntry(rawDetectionArray, baseIndex + 1); + double tync = extractArrayEntry(rawDetectionArray, baseIndex + 2); + double ta = extractArrayEntry(rawDetectionArray, baseIndex + 3); + double corner0_X = extractArrayEntry(rawDetectionArray, baseIndex + 4); + double corner0_Y = extractArrayEntry(rawDetectionArray, baseIndex + 5); + double corner1_X = extractArrayEntry(rawDetectionArray, baseIndex + 6); + double corner1_Y = extractArrayEntry(rawDetectionArray, baseIndex + 7); + double corner2_X = extractArrayEntry(rawDetectionArray, baseIndex + 8); + double corner2_Y = extractArrayEntry(rawDetectionArray, baseIndex + 9); + double corner3_X = extractArrayEntry(rawDetectionArray, baseIndex + 10); + double corner3_Y = extractArrayEntry(rawDetectionArray, baseIndex + 11); + + rawDetections[i] = + new RawDetection( + classId, txnc, tync, ta, corner0_X, corner0_Y, corner1_X, corner1_Y, corner2_X, + corner2_Y, corner3_X, corner3_Y); + } + + return rawDetections; + } + + /** + * Prints detailed information about a PoseEstimate to standard output. Includes timestamp, + * latency, tag count, tag span, average tag distance, average tag area, and detailed information + * about each detected fiducial. + * + * @param pose The PoseEstimate object to print. If null, prints "No PoseEstimate available." + */ + public static void printPoseEstimate(PoseEstimate pose) { + if (pose == null) { + System.out.println("No PoseEstimate available."); + return; + } + + System.out.printf("Pose Estimate Information:%n"); + System.out.printf("Timestamp (Seconds): %.3f%n", pose.timestampSeconds); + System.out.printf("Latency: %.3f ms%n", pose.latency); + System.out.printf("Tag Count: %d%n", pose.tagCount); + System.out.printf("Tag Span: %.2f meters%n", pose.tagSpan); + System.out.printf("Average Tag Distance: %.2f meters%n", pose.avgTagDist); + System.out.printf("Average Tag Area: %.2f%% of image%n", pose.avgTagArea); + System.out.printf("Is MegaTag2: %b%n", pose.isMegaTag2); + System.out.println(); + + if (pose.rawFiducials == null || pose.rawFiducials.length == 0) { + System.out.println("No RawFiducials data available."); + return; + } + + System.out.println("Raw Fiducials Details:"); + for (int i = 0; i < pose.rawFiducials.length; i++) { + RawFiducial fiducial = pose.rawFiducials[i]; + System.out.printf(" Fiducial #%d:%n", i + 1); + System.out.printf(" ID: %d%n", fiducial.id); + System.out.printf(" TXNC: %.2f%n", fiducial.txnc); + System.out.printf(" TYNC: %.2f%n", fiducial.tync); + System.out.printf(" TA: %.2f%n", fiducial.ta); + System.out.printf(" Distance to Camera: %.2f meters%n", fiducial.distToCamera); + System.out.printf(" Distance to Robot: %.2f meters%n", fiducial.distToRobot); + System.out.printf(" Ambiguity: %.2f%n", fiducial.ambiguity); + System.out.println(); + } + } + + public static Boolean validPoseEstimate(PoseEstimate pose) { + return pose != null && pose.rawFiducials != null && pose.rawFiducials.length != 0; + } + + public static NetworkTable getLimelightNTTable(String tableName) { + return NetworkTableInstance.getDefault().getTable(sanitizeName(tableName)); + } + + public static void Flush() { + NetworkTableInstance.getDefault().flush(); + } + + public static NetworkTableEntry getLimelightNTTableEntry(String tableName, String entryName) { + return getLimelightNTTable(tableName).getEntry(entryName); + } + + public static DoubleArrayEntry getLimelightDoubleArrayEntry(String tableName, String entryName) { + String key = tableName + "/" + entryName; + return doubleArrayEntries.computeIfAbsent( + key, + k -> { + NetworkTable table = getLimelightNTTable(tableName); + return table.getDoubleArrayTopic(entryName).getEntry(new double[0]); + }); + } + + public static double getLimelightNTDouble(String tableName, String entryName) { + return getLimelightNTTableEntry(tableName, entryName).getDouble(0.0); + } + + public static void setLimelightNTDouble(String tableName, String entryName, double val) { + getLimelightNTTableEntry(tableName, entryName).setDouble(val); + } + + public static void setLimelightNTDoubleArray(String tableName, String entryName, double[] val) { + getLimelightNTTableEntry(tableName, entryName).setDoubleArray(val); + } + + public static double[] getLimelightNTDoubleArray(String tableName, String entryName) { + return getLimelightNTTableEntry(tableName, entryName).getDoubleArray(new double[0]); + } + + public static String getLimelightNTString(String tableName, String entryName) { + return getLimelightNTTableEntry(tableName, entryName).getString(""); + } + + public static String[] getLimelightNTStringArray(String tableName, String entryName) { + return getLimelightNTTableEntry(tableName, entryName).getStringArray(new String[0]); + } + + public static URL getLimelightURLString(String tableName, String request) { + String urlString = "http://" + sanitizeName(tableName) + ".local:5807/" + request; + URL url; + try { + url = new URL(urlString); + return url; + } catch (MalformedURLException e) { + System.err.println("bad LL URL"); + } + return null; + } + + ///// + ///// + + /** + * Does the Limelight have a valid target? + * + * @param limelightName Name of the Limelight camera ("" for default) + * @return True if a valid target is present, false otherwise + */ + public static boolean getTV(String limelightName) { + return 1.0 == getLimelightNTDouble(limelightName, "tv"); + } + + /** + * Gets the horizontal offset from the crosshair to the target in degrees. + * + * @param limelightName Name of the Limelight camera ("" for default) + * @return Horizontal offset angle in degrees + */ + public static double getTX(String limelightName) { + return getLimelightNTDouble(limelightName, "tx"); + } + + /** + * Gets the vertical offset from the crosshair to the target in degrees. + * + * @param limelightName Name of the Limelight camera ("" for default) + * @return Vertical offset angle in degrees + */ + public static double getTY(String limelightName) { + return getLimelightNTDouble(limelightName, "ty"); + } + + /** + * Gets the horizontal offset from the principal pixel/point to the target in degrees. This is the + * most accurate 2d metric if you are using a calibrated camera and you don't need adjustable + * crosshair functionality. + * + * @param limelightName Name of the Limelight camera ("" for default) + * @return Horizontal offset angle in degrees + */ + public static double getTXNC(String limelightName) { + return getLimelightNTDouble(limelightName, "txnc"); + } + + /** + * Gets the vertical offset from the principal pixel/point to the target in degrees. This is the + * most accurate 2d metric if you are using a calibrated camera and you don't need adjustable + * crosshair functionality. + * + * @param limelightName Name of the Limelight camera ("" for default) + * @return Vertical offset angle in degrees + */ + public static double getTYNC(String limelightName) { + return getLimelightNTDouble(limelightName, "tync"); + } + + /** + * Gets the target area as a percentage of the image (0-100%). + * + * @param limelightName Name of the Limelight camera ("" for default) + * @return Target area percentage (0-100) + */ + public static double getTA(String limelightName) { + return getLimelightNTDouble(limelightName, "ta"); + } + + /** + * T2D is an array that contains several targeting metrcis + * + * @param limelightName Name of the Limelight camera + * @return Array containing [targetValid, targetCount, targetLatency, captureLatency, tx, ty, + * txnc, tync, ta, tid, targetClassIndexDetector, targetClassIndexClassifier, + * targetLongSidePixels, targetShortSidePixels, targetHorizontalExtentPixels, + * targetVerticalExtentPixels, targetSkewDegrees] + */ + public static double[] getT2DArray(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "t2d"); + } + + /** + * Gets the number of targets currently detected. + * + * @param limelightName Name of the Limelight camera + * @return Number of detected targets + */ + public static int getTargetCount(String limelightName) { + double[] t2d = getT2DArray(limelightName); + if (t2d.length == 17) { + return (int) t2d[1]; + } + return 0; + } + + /** + * Gets the classifier class index from the currently running neural classifier pipeline + * + * @param limelightName Name of the Limelight camera + * @return Class index from classifier pipeline + */ + public static int getClassifierClassIndex(String limelightName) { + double[] t2d = getT2DArray(limelightName); + if (t2d.length == 17) { + return (int) t2d[10]; + } + return 0; + } + + /** + * Gets the detector class index from the primary result of the currently running neural detector + * pipeline. + * + * @param limelightName Name of the Limelight camera + * @return Class index from detector pipeline + */ + public static int getDetectorClassIndex(String limelightName) { + double[] t2d = getT2DArray(limelightName); + if (t2d.length == 17) { + return (int) t2d[11]; + } + return 0; + } + + /** + * Gets the current neural classifier result class name. + * + * @param limelightName Name of the Limelight camera + * @return Class name string from classifier pipeline + */ + public static String getClassifierClass(String limelightName) { + return getLimelightNTString(limelightName, "tcclass"); + } + + /** + * Gets the primary neural detector result class name. + * + * @param limelightName Name of the Limelight camera + * @return Class name string from detector pipeline + */ + public static String getDetectorClass(String limelightName) { + return getLimelightNTString(limelightName, "tdclass"); + } + + /** + * Gets the pipeline's processing latency contribution. + * + * @param limelightName Name of the Limelight camera + * @return Pipeline latency in milliseconds + */ + public static double getLatency_Pipeline(String limelightName) { + return getLimelightNTDouble(limelightName, "tl"); + } + + /** + * Gets the capture latency. + * + * @param limelightName Name of the Limelight camera + * @return Capture latency in milliseconds + */ + public static double getLatency_Capture(String limelightName) { + return getLimelightNTDouble(limelightName, "cl"); + } + + /** + * Gets the active pipeline index. + * + * @param limelightName Name of the Limelight camera + * @return Current pipeline index (0-9) + */ + public static double getCurrentPipelineIndex(String limelightName) { + return getLimelightNTDouble(limelightName, "getpipe"); + } + + /** + * Gets the current pipeline type. + * + * @param limelightName Name of the Limelight camera + * @return Pipeline type string (e.g. "retro", "apriltag", etc) + */ + public static String getCurrentPipelineType(String limelightName) { + return getLimelightNTString(limelightName, "getpipetype"); + } + + /** + * Gets the full JSON results dump. + * + * @param limelightName Name of the Limelight camera + * @return JSON string containing all current results + */ + public static String getJSONDump(String limelightName) { + return getLimelightNTString(limelightName, "json"); + } + + /** + * Switch to getBotPose + * + * @param limelightName + * @return + */ + @Deprecated + public static double[] getBotpose(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose"); + } + + /** + * Switch to getBotPose_wpiRed + * + * @param limelightName + * @return + */ + @Deprecated + public static double[] getBotpose_wpiRed(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_wpired"); + } + + /** + * Switch to getBotPose_wpiBlue + * + * @param limelightName + * @return + */ + @Deprecated + public static double[] getBotpose_wpiBlue(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); + } + + public static double[] getBotPose(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose"); + } + + public static double[] getBotPose_wpiRed(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_wpired"); + } + + public static double[] getBotPose_wpiBlue(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); + } + + public static double[] getBotPose_TargetSpace(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_targetspace"); + } + + public static double[] getCameraPose_TargetSpace(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "camerapose_targetspace"); + } + + public static double[] getTargetPose_CameraSpace(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "targetpose_cameraspace"); + } + + public static double[] getTargetPose_RobotSpace(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "targetpose_robotspace"); + } + + public static double[] getTargetColor(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "tc"); + } + + public static double getFiducialID(String limelightName) { + return getLimelightNTDouble(limelightName, "tid"); + } + + public static String getNeuralClassID(String limelightName) { + return getLimelightNTString(limelightName, "tclass"); + } + + public static String[] getRawBarcodeData(String limelightName) { + return getLimelightNTStringArray(limelightName, "rawbarcodes"); + } + + ///// + ///// + + public static Pose3d getBotPose3d(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose"); + return toPose3D(poseArray); + } + + /** + * (Not Recommended) Gets the robot's 3D pose in the WPILib Red Alliance Coordinate System. + * + * @param limelightName Name/identifier of the Limelight + * @return Pose3d object representing the robot's position and orientation in Red Alliance field + * space + */ + public static Pose3d getBotPose3d_wpiRed(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_wpired"); + return toPose3D(poseArray); + } + + /** + * (Recommended) Gets the robot's 3D pose in the WPILib Blue Alliance Coordinate System. + * + * @param limelightName Name/identifier of the Limelight + * @return Pose3d object representing the robot's position and orientation in Blue Alliance field + * space + */ + public static Pose3d getBotPose3d_wpiBlue(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); + return toPose3D(poseArray); + } + + /** + * Gets the robot's 3D pose with respect to the currently tracked target's coordinate system. + * + * @param limelightName Name/identifier of the Limelight + * @return Pose3d object representing the robot's position and orientation relative to the target + */ + public static Pose3d getBotPose3d_TargetSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_targetspace"); + return toPose3D(poseArray); + } + + /** + * Gets the camera's 3D pose with respect to the currently tracked target's coordinate system. + * + * @param limelightName Name/identifier of the Limelight + * @return Pose3d object representing the camera's position and orientation relative to the target + */ + public static Pose3d getCameraPose3d_TargetSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "camerapose_targetspace"); + return toPose3D(poseArray); + } + + /** + * Gets the target's 3D pose with respect to the camera's coordinate system. + * + * @param limelightName Name/identifier of the Limelight + * @return Pose3d object representing the target's position and orientation relative to the camera + */ + public static Pose3d getTargetPose3d_CameraSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "targetpose_cameraspace"); + return toPose3D(poseArray); + } + + /** + * Gets the target's 3D pose with respect to the robot's coordinate system. + * + * @param limelightName Name/identifier of the Limelight + * @return Pose3d object representing the target's position and orientation relative to the robot + */ + public static Pose3d getTargetPose3d_RobotSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "targetpose_robotspace"); + return toPose3D(poseArray); + } + + /** + * Gets the camera's 3D pose with respect to the robot's coordinate system. + * + * @param limelightName Name/identifier of the Limelight + * @return Pose3d object representing the camera's position and orientation relative to the robot + */ + public static Pose3d getCameraPose3d_RobotSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "camerapose_robotspace"); + return toPose3D(poseArray); + } + + /** + * Gets the Pose2d for easy use with Odometry vision pose estimator (addVisionMeasurement) + * + * @param limelightName + * @return + */ + public static Pose2d getBotPose2d_wpiBlue(String limelightName) { + + double[] result = getBotPose_wpiBlue(limelightName); + return toPose2D(result); + } + + /** + * Gets the MegaTag1 Pose2d and timestamp for use with WPILib pose estimator + * (addVisionMeasurement) in the WPILib Blue alliance coordinate system. + * + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiBlue(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_wpiblue", false); + } + + /** + * Gets the MegaTag2 Pose2d and timestamp for use with WPILib pose estimator + * (addVisionMeasurement) in the WPILib Blue alliance coordinate system. Make sure you are calling + * setRobotOrientation() before calling this method. + * + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiBlue_MegaTag2(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_orb_wpiblue", true); + } + + /** + * Gets the Pose2d for easy use with Odometry vision pose estimator (addVisionMeasurement) + * + * @param limelightName + * @return + */ + public static Pose2d getBotPose2d_wpiRed(String limelightName) { + + double[] result = getBotPose_wpiRed(limelightName); + return toPose2D(result); + } + + /** + * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when + * you are on the RED alliance + * + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiRed(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_wpired", false); + } + + /** + * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when + * you are on the RED alliance + * + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiRed_MegaTag2(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_orb_wpired", true); + } + + /** + * Gets the Pose2d for easy use with Odometry vision pose estimator (addVisionMeasurement) + * + * @param limelightName + * @return + */ + public static Pose2d getBotPose2d(String limelightName) { + + double[] result = getBotPose(limelightName); + return toPose2D(result); + } + + /** + * Gets the current IMU data from NetworkTables. IMU data is formatted as [robotYaw, Roll, Pitch, + * Yaw, gyroX, gyroY, gyroZ, accelX, accelY, accelZ]. Returns all zeros if data is invalid or + * unavailable. + * + * @param limelightName Name/identifier of the Limelight + * @return IMUData object containing all current IMU data + */ + public static IMUData getIMUData(String limelightName) { + double[] imuData = getLimelightNTDoubleArray(limelightName, "imu"); + if (imuData == null || imuData.length < 10) { + return new IMUData(); // Returns object with all zeros + } + return new IMUData(imuData); + } + + ///// + ///// + + public static void setPipelineIndex(String limelightName, int pipelineIndex) { + setLimelightNTDouble(limelightName, "pipeline", pipelineIndex); + } + + public static void setPriorityTagID(String limelightName, int ID) { + setLimelightNTDouble(limelightName, "priorityid", ID); + } + + /** + * Sets LED mode to be controlled by the current pipeline. + * + * @param limelightName Name of the Limelight camera + */ + public static void setLEDMode_PipelineControl(String limelightName) { + setLimelightNTDouble(limelightName, "ledMode", 0); + } + + public static void setLEDMode_ForceOff(String limelightName) { + setLimelightNTDouble(limelightName, "ledMode", 1); + } + + public static void setLEDMode_ForceBlink(String limelightName) { + setLimelightNTDouble(limelightName, "ledMode", 2); + } + + public static void setLEDMode_ForceOn(String limelightName) { + setLimelightNTDouble(limelightName, "ledMode", 3); + } + + /** + * Enables standard side-by-side stream mode. + * + * @param limelightName Name of the Limelight camera + */ + public static void setStreamMode_Standard(String limelightName) { + setLimelightNTDouble(limelightName, "stream", 0); + } + + /** + * Enables Picture-in-Picture mode with secondary stream in the corner. + * + * @param limelightName Name of the Limelight camera + */ + public static void setStreamMode_PiPMain(String limelightName) { + setLimelightNTDouble(limelightName, "stream", 1); + } + + /** + * Enables Picture-in-Picture mode with primary stream in the corner. + * + * @param limelightName Name of the Limelight camera + */ + public static void setStreamMode_PiPSecondary(String limelightName) { + setLimelightNTDouble(limelightName, "stream", 2); + } + + /** + * Sets the crop window for the camera. The crop window in the UI must be completely open. + * + * @param limelightName Name of the Limelight camera + * @param cropXMin Minimum X value (-1 to 1) + * @param cropXMax Maximum X value (-1 to 1) + * @param cropYMin Minimum Y value (-1 to 1) + * @param cropYMax Maximum Y value (-1 to 1) + */ + public static void setCropWindow( + String limelightName, double cropXMin, double cropXMax, double cropYMin, double cropYMax) { + double[] entries = new double[4]; + entries[0] = cropXMin; + entries[1] = cropXMax; + entries[2] = cropYMin; + entries[3] = cropYMax; + setLimelightNTDoubleArray(limelightName, "crop", entries); + } + + /** Sets 3D offset point for easy 3D targeting. */ + public static void setFiducial3DOffset( + String limelightName, double offsetX, double offsetY, double offsetZ) { + double[] entries = new double[3]; + entries[0] = offsetX; + entries[1] = offsetY; + entries[2] = offsetZ; + setLimelightNTDoubleArray(limelightName, "fiducial_offset_set", entries); + } + + /** + * Sets robot orientation values used by MegaTag2 localization algorithm. + * + * @param limelightName Name/identifier of the Limelight + * @param yaw Robot yaw in degrees. 0 = robot facing red alliance wall in FRC + * @param yawRate (Unnecessary) Angular velocity of robot yaw in degrees per second + * @param pitch (Unnecessary) Robot pitch in degrees + * @param pitchRate (Unnecessary) Angular velocity of robot pitch in degrees per second + * @param roll (Unnecessary) Robot roll in degrees + * @param rollRate (Unnecessary) Angular velocity of robot roll in degrees per second + */ + public static void SetRobotOrientation( + String limelightName, + double yaw, + double yawRate, + double pitch, + double pitchRate, + double roll, + double rollRate) { + SetRobotOrientation_INTERNAL( + limelightName, yaw, yawRate, pitch, pitchRate, roll, rollRate, true); + } + + public static void SetRobotOrientation_NoFlush( + String limelightName, + double yaw, + double yawRate, + double pitch, + double pitchRate, + double roll, + double rollRate) { + SetRobotOrientation_INTERNAL( + limelightName, yaw, yawRate, pitch, pitchRate, roll, rollRate, false); + } + + private static void SetRobotOrientation_INTERNAL( + String limelightName, + double yaw, + double yawRate, + double pitch, + double pitchRate, + double roll, + double rollRate, + boolean flush) { + + double[] entries = new double[6]; + entries[0] = yaw; + entries[1] = yawRate; + entries[2] = pitch; + entries[3] = pitchRate; + entries[4] = roll; + entries[5] = rollRate; + setLimelightNTDoubleArray(limelightName, "robot_orientation_set", entries); + if (flush) { + Flush(); + } + } + + /** + * Configures the IMU mode for MegaTag2 Localization + * + * @param limelightName Name/identifier of the Limelight + * @param mode IMU mode. + */ + public static void SetIMUMode(String limelightName, int mode) { + setLimelightNTDouble(limelightName, "imumode_set", mode); + } + + /** + * Configures the complementary filter alpha value for IMU Assist Modes (Modes 3 and 4) + * + * @param limelightName Name/identifier of the Limelight + * @param alpha Defaults to .001. Higher values will cause the internal IMU to converge onto the + * assist source more rapidly. + */ + public static void SetIMUAssistAlpha(String limelightName, double alpha) { + setLimelightNTDouble(limelightName, "imuassistalpha_set", alpha); + } + + /** + * Configures the throttle value. Set to 100-200 while disabled to reduce thermal + * output/temperature. + * + * @param limelightName Name/identifier of the Limelight + * @param throttle Defaults to 0. Your Limelgiht will process one frame after skipping + * frames. + */ + public static void SetThrottle(String limelightName, int throttle) { + setLimelightNTDouble(limelightName, "throttle_set", throttle); + } + + /** + * Sets the 3D point-of-interest offset for the current fiducial pipeline. + * https://docs.limelightvision.io/docs/docs-limelight/pipeline-apriltag/apriltag-3d#point-of-interest-tracking + * + * @param limelightName Name/identifier of the Limelight + * @param x X offset in meters + * @param y Y offset in meters + * @param z Z offset in meters + */ + public static void SetFidcuial3DOffset(String limelightName, double x, double y, double z) { + + double[] entries = new double[3]; + entries[0] = x; + entries[1] = y; + entries[2] = z; + setLimelightNTDoubleArray(limelightName, "fiducial_offset_set", entries); + } + + /** + * Overrides the valid AprilTag IDs that will be used for localization. Tags not in this list will + * be ignored for robot pose estimation. + * + * @param limelightName Name/identifier of the Limelight + * @param validIDs Array of valid AprilTag IDs to track + */ + public static void SetFiducialIDFiltersOverride(String limelightName, int[] validIDs) { + double[] validIDsDouble = new double[validIDs.length]; + for (int i = 0; i < validIDs.length; i++) { + validIDsDouble[i] = validIDs[i]; + } + setLimelightNTDoubleArray(limelightName, "fiducial_id_filters_set", validIDsDouble); + } + + /** + * Sets the downscaling factor for AprilTag detection. Increasing downscale can improve + * performance at the cost of potentially reduced detection range. + * + * @param limelightName Name/identifier of the Limelight + * @param downscale Downscale factor. Valid values: 1.0 (no downscale), 1.5, 2.0, 3.0, 4.0. Set to + * 0 for pipeline control. + */ + public static void SetFiducialDownscalingOverride(String limelightName, float downscale) { + int d = 0; // pipeline + if (downscale == 1.0) { + d = 1; + } + if (downscale == 1.5) { + d = 2; + } + if (downscale == 2) { + d = 3; + } + if (downscale == 3) { + d = 4; + } + if (downscale == 4) { + d = 5; + } + setLimelightNTDouble(limelightName, "fiducial_downscale_set", d); + } + + /** + * Sets the camera pose relative to the robot. + * + * @param limelightName Name of the Limelight camera + * @param forward Forward offset in meters + * @param side Side offset in meters + * @param up Up offset in meters + * @param roll Roll angle in degrees + * @param pitch Pitch angle in degrees + * @param yaw Yaw angle in degrees + */ + public static void setCameraPose_RobotSpace( + String limelightName, + double forward, + double side, + double up, + double roll, + double pitch, + double yaw) { + double[] entries = new double[6]; + entries[0] = forward; + entries[1] = side; + entries[2] = up; + entries[3] = roll; + entries[4] = pitch; + entries[5] = yaw; + setLimelightNTDoubleArray(limelightName, "camerapose_robotspace_set", entries); + } + + ///// + ///// + + public static void setPythonScriptData(String limelightName, double[] outgoingPythonData) { + setLimelightNTDoubleArray(limelightName, "llrobot", outgoingPythonData); + } + + public static double[] getPythonScriptData(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "llpython"); + } + + ///// + ///// + + /** Asynchronously take snapshot. */ + public static CompletableFuture takeSnapshot(String tableName, String snapshotName) { + return CompletableFuture.supplyAsync( + () -> { + return SYNCH_TAKESNAPSHOT(tableName, snapshotName); + }); + } + + private static boolean SYNCH_TAKESNAPSHOT(String tableName, String snapshotName) { + URL url = getLimelightURLString(tableName, "capturesnapshot"); + try { + HttpURLConnection connection = (HttpURLConnection) url.openConnection(); + connection.setRequestMethod("GET"); + if (snapshotName != null && !"".equals(snapshotName)) { + connection.setRequestProperty("snapname", snapshotName); + } + + int responseCode = connection.getResponseCode(); + if (responseCode == 200) { + return true; + } else { + System.err.println("Bad LL Request"); + } + } catch (IOException e) { + System.err.println(e.getMessage()); + } + return false; + } + + /** + * Gets the latest JSON results output and returns a LimelightResults object. + * + * @param limelightName Name of the Limelight camera + * @return LimelightResults object containing all current target data + */ + public static LimelightResults getLatestResults(String limelightName) { + + long start = System.nanoTime(); + LimelightHelpers.LimelightResults results = new LimelightHelpers.LimelightResults(); + if (mapper == null) { + mapper = + new ObjectMapper().configure(DeserializationFeature.FAIL_ON_UNKNOWN_PROPERTIES, false); + } + + try { + results = mapper.readValue(getJSONDump(limelightName), LimelightResults.class); + } catch (JsonProcessingException e) { + results.error = "lljson error: " + e.getMessage(); + } + + long end = System.nanoTime(); + double millis = (end - start) * .000001; + results.latency_jsonParse = millis; + if (profileJSON) { + System.out.printf("lljson: %.2f\r\n", millis); + } + + return results; + } +} From b41c8a72b5af2d6210bdc406cbd2e3254335d2d4 Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Tue, 25 Mar 2025 15:40:00 -0400 Subject: [PATCH 72/99] small pr fixed, changed from radians to degrees --- .../frc/robot/subsystems/limelight/Vision.java | 7 ++----- .../java/frc/robot/utils/GamePieceLocate.java | 17 ++++++++++++----- .../frc/robot/utils/GamePieceLocateTest.java | 4 +--- 3 files changed, 15 insertions(+), 13 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/limelight/Vision.java b/src/main/java/frc/robot/subsystems/limelight/Vision.java index 5bba6afc..8b3019d7 100644 --- a/src/main/java/frc/robot/subsystems/limelight/Vision.java +++ b/src/main/java/frc/robot/subsystems/limelight/Vision.java @@ -4,7 +4,6 @@ package frc.robot.subsystems.limelight; -import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.constants.AlgaePositions; @@ -72,14 +71,12 @@ private void locateGamePieces() { if (className.equalsIgnoreCase("algae")) { BranchPositions coralBranch = GamePieceLocate.findCoralBranch( - pose2dSupplier.get(), - VecBuilder.fill(system.getInputs().tx[i], system.getInputs().ty[i])); + pose2dSupplier.get(), system.getInputs().tx[i], system.getInputs().ty[i]); currentCoralPositions.add(coralBranch); } else if (className.equalsIgnoreCase("coral")) { AlgaePositions algaePos = GamePieceLocate.findAlgaePos( - pose2dSupplier.get(), - VecBuilder.fill(system.getInputs().tx[i], system.getInputs().ty[i])); + pose2dSupplier.get(), system.getInputs().tx[i], system.getInputs().ty[i]); currentAlgaePosition.add(algaePos); } } diff --git a/src/main/java/frc/robot/utils/GamePieceLocate.java b/src/main/java/frc/robot/utils/GamePieceLocate.java index 410ea600..457a464e 100644 --- a/src/main/java/frc/robot/utils/GamePieceLocate.java +++ b/src/main/java/frc/robot/utils/GamePieceLocate.java @@ -5,7 +5,6 @@ import edu.wpi.first.math.Vector; import edu.wpi.first.math.geometry.*; import edu.wpi.first.math.numbers.N1; -import edu.wpi.first.math.numbers.N2; import edu.wpi.first.math.numbers.N3; import frc.robot.constants.AlgaePositions; import frc.robot.constants.BranchPositions; @@ -29,12 +28,17 @@ public class GamePieceLocate { .toArray(Vector[]::new); // Piece Pos is in Radians - public static BranchPositions findCoralBranch(Pose2d robotPos, Vector piecePos) { + public static BranchPositions findCoralBranch( + Pose2d robotPos, double piecePosTXDeg, double piecePosTYDeg) { final Pose3d cameraPos = new Pose3d(robotPos).transformBy(Constants.LIMELIGHT_TO_ROBOT); final Vector cameraPosVec = cameraPos.getTranslation().toVector(); final Matrix invCameraRotation = cameraPos.getRotation().unaryMinus().toMatrix(); final Vector pieceVec = - VecBuilder.fill(1, -Math.tan(piecePos.get(1)), -Math.tan(piecePos.get(0))).unit(); + VecBuilder.fill( + 1, + -Math.tan(piecePosTXDeg * Math.PI / 180), + -Math.tan(piecePosTYDeg * Math.PI / 180)) + .unit(); double maxDot = Constants.MINIMUM_PIECE_DETECTION_DOT; BranchPositions closest = null; int n = CenterPositions.getClosest(robotPos); @@ -56,12 +60,15 @@ public static BranchPositions findCoralBranch(Pose2d robotPos, Vector pieceP } // piece pos is in Radians - public static AlgaePositions findAlgaePos(Pose2d robotPos, Vector piecePos) { + public static AlgaePositions findAlgaePos( + Pose2d robotPos, double piecePosXDeg, double piecePosYDeg) { final Pose3d cameraPos = new Pose3d(robotPos).transformBy(Constants.LIMELIGHT_TO_ROBOT); final Vector cameraPosVec = cameraPos.getTranslation().toVector(); final Matrix invCameraRotation = cameraPos.getRotation().unaryMinus().toMatrix(); final Vector pieceVec = - VecBuilder.fill(1, -Math.tan(piecePos.get(1)), -Math.tan(piecePos.get(0))); + VecBuilder.fill( + 1, -Math.tan(piecePosXDeg * Math.PI / 180), -Math.tan(piecePosYDeg * Math.PI / 180)) + .unit(); double maxDot = Constants.MINIMUM_PIECE_DETECTION_DOT; AlgaePositions closest = null; int n = CenterPositions.getClosest(robotPos); diff --git a/src/test/java/frc/robot/utils/GamePieceLocateTest.java b/src/test/java/frc/robot/utils/GamePieceLocateTest.java index 631037a4..405e809c 100644 --- a/src/test/java/frc/robot/utils/GamePieceLocateTest.java +++ b/src/test/java/frc/robot/utils/GamePieceLocateTest.java @@ -2,7 +2,6 @@ import static org.junit.jupiter.api.Assertions.*; -import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import frc.robot.constants.AlgaePositions; @@ -15,8 +14,7 @@ void findAlgaePos() { double x = 0.31119220563058896357; double y = 0.06719517620178168871; AlgaePositions algaePos = - GamePieceLocate.findAlgaePos( - new Pose2d(2.614524485, 4.0259, new Rotation2d(0)), VecBuilder.fill(x, y)); + GamePieceLocate.findAlgaePos(new Pose2d(2.614524485, 4.0259, new Rotation2d(0)), x, y); assertEquals(5.144949195, algaePos.getPosition().getX()); assertEquals(4.0259, algaePos.getPosition().getY()); } From 30003bc178c3290cea9a480d5237c9304a3004c0 Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Tue, 25 Mar 2025 15:44:23 -0400 Subject: [PATCH 73/99] small changes --- src/main/java/frc/robot/utils/GamePieceLocate.java | 4 +--- src/test/java/frc/robot/utils/GamePieceLocateTest.java | 2 +- 2 files changed, 2 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/utils/GamePieceLocate.java b/src/main/java/frc/robot/utils/GamePieceLocate.java index 457a464e..a76e9762 100644 --- a/src/main/java/frc/robot/utils/GamePieceLocate.java +++ b/src/main/java/frc/robot/utils/GamePieceLocate.java @@ -35,9 +35,7 @@ public static BranchPositions findCoralBranch( final Matrix invCameraRotation = cameraPos.getRotation().unaryMinus().toMatrix(); final Vector pieceVec = VecBuilder.fill( - 1, - -Math.tan(piecePosTXDeg * Math.PI / 180), - -Math.tan(piecePosTYDeg * Math.PI / 180)) + 1, -Math.tan(Math.toRadians(piecePosTXDeg)), -Math.tan(Math.toRadians(piecePosTYDeg))) .unit(); double maxDot = Constants.MINIMUM_PIECE_DETECTION_DOT; BranchPositions closest = null; diff --git a/src/test/java/frc/robot/utils/GamePieceLocateTest.java b/src/test/java/frc/robot/utils/GamePieceLocateTest.java index 405e809c..bd500fa0 100644 --- a/src/test/java/frc/robot/utils/GamePieceLocateTest.java +++ b/src/test/java/frc/robot/utils/GamePieceLocateTest.java @@ -8,7 +8,7 @@ import org.junit.jupiter.api.Test; class GamePieceLocateTest { - + // These are wrong I think @Test void findAlgaePos() { double x = 0.31119220563058896357; From ae91005e8748f1a62a11f4d2587865104e6bc7db Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Tue, 25 Mar 2025 15:47:35 -0400 Subject: [PATCH 74/99] last --- src/main/java/frc/robot/utils/GamePieceLocate.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/utils/GamePieceLocate.java b/src/main/java/frc/robot/utils/GamePieceLocate.java index a76e9762..bba4a672 100644 --- a/src/main/java/frc/robot/utils/GamePieceLocate.java +++ b/src/main/java/frc/robot/utils/GamePieceLocate.java @@ -27,7 +27,7 @@ public class GamePieceLocate { .map(algae -> algae.getPosition().getTranslation().toVector()) .toArray(Vector[]::new); - // Piece Pos is in Radians + // piece pos is in DEGREES, not RD public static BranchPositions findCoralBranch( Pose2d robotPos, double piecePosTXDeg, double piecePosTYDeg) { final Pose3d cameraPos = new Pose3d(robotPos).transformBy(Constants.LIMELIGHT_TO_ROBOT); @@ -57,7 +57,7 @@ public static BranchPositions findCoralBranch( return closest; } - // piece pos is in Radians + // piece pos is in DEGREES, not RD public static AlgaePositions findAlgaePos( Pose2d robotPos, double piecePosXDeg, double piecePosYDeg) { final Pose3d cameraPos = new Pose3d(robotPos).transformBy(Constants.LIMELIGHT_TO_ROBOT); From d4660c5fab038076cf65b7afc675f5791536a1a5 Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Tue, 25 Mar 2025 15:49:03 -0400 Subject: [PATCH 75/99] last --- src/main/java/frc/robot/utils/GamePieceLocate.java | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/utils/GamePieceLocate.java b/src/main/java/frc/robot/utils/GamePieceLocate.java index bba4a672..db39f013 100644 --- a/src/main/java/frc/robot/utils/GamePieceLocate.java +++ b/src/main/java/frc/robot/utils/GamePieceLocate.java @@ -59,14 +59,14 @@ public static BranchPositions findCoralBranch( // piece pos is in DEGREES, not RD public static AlgaePositions findAlgaePos( - Pose2d robotPos, double piecePosXDeg, double piecePosYDeg) { + Pose2d robotPos, double piecePosTXDeg, double piecePosTYDeg) { final Pose3d cameraPos = new Pose3d(robotPos).transformBy(Constants.LIMELIGHT_TO_ROBOT); final Vector cameraPosVec = cameraPos.getTranslation().toVector(); final Matrix invCameraRotation = cameraPos.getRotation().unaryMinus().toMatrix(); final Vector pieceVec = - VecBuilder.fill( - 1, -Math.tan(piecePosXDeg * Math.PI / 180), -Math.tan(piecePosYDeg * Math.PI / 180)) - .unit(); + VecBuilder.fill( + 1, -Math.tan(Math.toRadians(piecePosTXDeg)), -Math.tan(Math.toRadians(piecePosTYDeg))) + .unit(); double maxDot = Constants.MINIMUM_PIECE_DETECTION_DOT; AlgaePositions closest = null; int n = CenterPositions.getClosest(robotPos); From e21394d25f33280613e281de8b2d126cd8d5a057 Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Tue, 25 Mar 2025 15:49:14 -0400 Subject: [PATCH 76/99] last --- src/main/java/frc/robot/utils/GamePieceLocate.java | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/utils/GamePieceLocate.java b/src/main/java/frc/robot/utils/GamePieceLocate.java index db39f013..5a68ccf9 100644 --- a/src/main/java/frc/robot/utils/GamePieceLocate.java +++ b/src/main/java/frc/robot/utils/GamePieceLocate.java @@ -35,7 +35,9 @@ public static BranchPositions findCoralBranch( final Matrix invCameraRotation = cameraPos.getRotation().unaryMinus().toMatrix(); final Vector pieceVec = VecBuilder.fill( - 1, -Math.tan(Math.toRadians(piecePosTXDeg)), -Math.tan(Math.toRadians(piecePosTYDeg))) + 1, + -Math.tan(Math.toRadians(piecePosTXDeg)), + -Math.tan(Math.toRadians(piecePosTYDeg))) .unit(); double maxDot = Constants.MINIMUM_PIECE_DETECTION_DOT; BranchPositions closest = null; @@ -64,9 +66,11 @@ public static AlgaePositions findAlgaePos( final Vector cameraPosVec = cameraPos.getTranslation().toVector(); final Matrix invCameraRotation = cameraPos.getRotation().unaryMinus().toMatrix(); final Vector pieceVec = - VecBuilder.fill( - 1, -Math.tan(Math.toRadians(piecePosTXDeg)), -Math.tan(Math.toRadians(piecePosTYDeg))) - .unit(); + VecBuilder.fill( + 1, + -Math.tan(Math.toRadians(piecePosTXDeg)), + -Math.tan(Math.toRadians(piecePosTYDeg))) + .unit(); double maxDot = Constants.MINIMUM_PIECE_DETECTION_DOT; AlgaePositions closest = null; int n = CenterPositions.getClosest(robotPos); From 6d565ac684a221c9acc351e966831643c7df3390 Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Tue, 25 Mar 2025 15:56:34 -0400 Subject: [PATCH 77/99] fixed --- src/main/java/frc/robot/utils/GamePieceLocate.java | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/utils/GamePieceLocate.java b/src/main/java/frc/robot/utils/GamePieceLocate.java index 5a68ccf9..6e71aef3 100644 --- a/src/main/java/frc/robot/utils/GamePieceLocate.java +++ b/src/main/java/frc/robot/utils/GamePieceLocate.java @@ -36,8 +36,8 @@ public static BranchPositions findCoralBranch( final Vector pieceVec = VecBuilder.fill( 1, - -Math.tan(Math.toRadians(piecePosTXDeg)), - -Math.tan(Math.toRadians(piecePosTYDeg))) + -Math.tan(Math.toRadians(piecePosTYDeg)), + -Math.tan(Math.toRadians(piecePosTXDeg))) .unit(); double maxDot = Constants.MINIMUM_PIECE_DETECTION_DOT; BranchPositions closest = null; @@ -68,8 +68,8 @@ public static AlgaePositions findAlgaePos( final Vector pieceVec = VecBuilder.fill( 1, - -Math.tan(Math.toRadians(piecePosTXDeg)), - -Math.tan(Math.toRadians(piecePosTYDeg))) + -Math.tan(Math.toRadians(piecePosTYDeg)), + -Math.tan(Math.toRadians(piecePosTXDeg))) .unit(); double maxDot = Constants.MINIMUM_PIECE_DETECTION_DOT; AlgaePositions closest = null; From bd584a29faf4b2aa7d3ccfb535c70848eca6fc65 Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Tue, 25 Mar 2025 16:00:55 -0400 Subject: [PATCH 78/99] done --- src/main/java/frc/robot/utils/GamePieceLocate.java | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/utils/GamePieceLocate.java b/src/main/java/frc/robot/utils/GamePieceLocate.java index 6e71aef3..12048e9c 100644 --- a/src/main/java/frc/robot/utils/GamePieceLocate.java +++ b/src/main/java/frc/robot/utils/GamePieceLocate.java @@ -36,8 +36,8 @@ public static BranchPositions findCoralBranch( final Vector pieceVec = VecBuilder.fill( 1, - -Math.tan(Math.toRadians(piecePosTYDeg)), - -Math.tan(Math.toRadians(piecePosTXDeg))) + -Math.tan(Math.toRadians(piecePosTXDeg)), + Math.tan(Math.toRadians(piecePosTYDeg))) .unit(); double maxDot = Constants.MINIMUM_PIECE_DETECTION_DOT; BranchPositions closest = null; @@ -68,8 +68,8 @@ public static AlgaePositions findAlgaePos( final Vector pieceVec = VecBuilder.fill( 1, - -Math.tan(Math.toRadians(piecePosTYDeg)), - -Math.tan(Math.toRadians(piecePosTXDeg))) + -Math.tan(Math.toRadians(piecePosTXDeg)), + Math.tan(Math.toRadians(piecePosTYDeg))) .unit(); double maxDot = Constants.MINIMUM_PIECE_DETECTION_DOT; AlgaePositions closest = null; From 639624da4d61be92bb25ef963e2807b9ae04e0e5 Mon Sep 17 00:00:00 2001 From: codetoad Date: Tue, 25 Mar 2025 16:03:18 -0400 Subject: [PATCH 79/99] unit test --- .../java/frc/robot/constants/GameConstants.java | 5 ++--- .../java/frc/robot/utils/GamePieceLocateTest.java | 14 ++++++++------ 2 files changed, 10 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java index 7a71d507..70d895f0 100644 --- a/src/main/java/frc/robot/constants/GameConstants.java +++ b/src/main/java/frc/robot/constants/GameConstants.java @@ -227,8 +227,7 @@ public enum Mode { // Limelight public static final Transform3d LIMELIGHT_TO_ROBOT = - new Transform3d( - 0.0, 0.0, 0.720725, new Rotation3d(0.0, Math.PI / 6, 0.0)); // TODO Change Later + new Transform3d(0.0, 0.0, 0, new Rotation3d(0.0, Math.PI / 6, 0.0)); // z = 0.720725 public static final String LIMELIGHT_IP_ADDRESS = "10.40.48.104"; // TODO Change Later - public static final double MINIMUM_PIECE_DETECTION_DOT = 0.8; + public static final double MINIMUM_PIECE_DETECTION_DOT = 0; } diff --git a/src/test/java/frc/robot/utils/GamePieceLocateTest.java b/src/test/java/frc/robot/utils/GamePieceLocateTest.java index bd500fa0..c651683e 100644 --- a/src/test/java/frc/robot/utils/GamePieceLocateTest.java +++ b/src/test/java/frc/robot/utils/GamePieceLocateTest.java @@ -8,14 +8,16 @@ import org.junit.jupiter.api.Test; class GamePieceLocateTest { - // These are wrong I think + @Test void findAlgaePos() { - double x = 0.31119220563058896357; - double y = 0.06719517620178168871; + double robotX = Apriltag.EIGHTEEN.getX() - 0.889; + double robotY = Apriltag.EIGHTEEN.getY(); + double visionX = 5; // degrees + double visionY = -1; // degrees AlgaePositions algaePos = - GamePieceLocate.findAlgaePos(new Pose2d(2.614524485, 4.0259, new Rotation2d(0)), x, y); - assertEquals(5.144949195, algaePos.getPosition().getX()); - assertEquals(4.0259, algaePos.getPosition().getY()); + GamePieceLocate.findAlgaePos( + new Pose2d(robotX, robotY, new Rotation2d(0)), visionX, visionY); + assertEquals(AlgaePositions.Algae_AB_LOW, algaePos); } } From 2e1a40f1394012577479b9b2e74f5a7961f72b7d Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Tue, 25 Mar 2025 16:04:57 -0400 Subject: [PATCH 80/99] fixed algae to be more consistent --- .../java/frc/robot/utils/GamePieceLocate.java | 53 +------------------ 1 file changed, 1 insertion(+), 52 deletions(-) diff --git a/src/main/java/frc/robot/utils/GamePieceLocate.java b/src/main/java/frc/robot/utils/GamePieceLocate.java index 12048e9c..f0cf2ab6 100644 --- a/src/main/java/frc/robot/utils/GamePieceLocate.java +++ b/src/main/java/frc/robot/utils/GamePieceLocate.java @@ -74,58 +74,8 @@ public static AlgaePositions findAlgaePos( double maxDot = Constants.MINIMUM_PIECE_DETECTION_DOT; AlgaePositions closest = null; int n = CenterPositions.getClosest(robotPos); - if (n == 0) { - for (int i = 0; i < 6; i++) { - Matrix locationVec = - (invCameraRotation.times(PRECOMPUTED_ALGAE_VECS[i].minus(cameraPosVec))); - double dot = - pieceVec.dot( - VecBuilder.fill(locationVec.get(0, 0), locationVec.get(1, 0), locationVec.get(2, 0)) - .unit()); - if (dot > maxDot) { - maxDot = dot; - closest = ALGAES[i]; - } - } - for (int i = 10; i < 12; i++) { - Matrix locationVec = - (invCameraRotation.times(PRECOMPUTED_ALGAE_VECS[i].minus(cameraPosVec))); - double dot = - pieceVec.dot( - VecBuilder.fill(locationVec.get(0, 0), locationVec.get(1, 0), locationVec.get(2, 0)) - .unit()); - if (dot > maxDot) { - maxDot = dot; - closest = ALGAES[i]; - } - } - } else if (n == 5) { - for (int i = 0; i < 3; i++) { - Matrix locationVec = - (invCameraRotation.times(PRECOMPUTED_ALGAE_VECS[i].minus(cameraPosVec))); - double dot = - pieceVec.dot( - VecBuilder.fill(locationVec.get(0, 0), locationVec.get(1, 0), locationVec.get(2, 0)) - .unit()); - if (dot > maxDot) { - maxDot = dot; - closest = ALGAES[i]; - } - } - for (int i = 8; i < 12; i++) { - Matrix locationVec = - (invCameraRotation.times(PRECOMPUTED_ALGAE_VECS[i].minus(cameraPosVec))); - double dot = - pieceVec.dot( - VecBuilder.fill(locationVec.get(0, 0), locationVec.get(1, 0), locationVec.get(2, 0)) - .unit()); - if (dot > maxDot) { - maxDot = dot; - closest = ALGAES[i]; - } - } - } else { for (int i = 2 * n - 2; i < 2 * n + 4; i++) { + int f = i % ALGAES.length; Matrix locationVec = (invCameraRotation.times(PRECOMPUTED_ALGAE_VECS[i].minus(cameraPosVec))); double dot = @@ -136,7 +86,6 @@ public static AlgaePositions findAlgaePos( maxDot = dot; closest = ALGAES[i]; } - } } return closest; } From 2d20b22c45340b6c4c2dd004a49e86f76fb89a1f Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Tue, 25 Mar 2025 16:05:38 -0400 Subject: [PATCH 81/99] fixed out of range --- .../java/frc/robot/utils/GamePieceLocate.java | 24 +++++++++---------- 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/robot/utils/GamePieceLocate.java b/src/main/java/frc/robot/utils/GamePieceLocate.java index f0cf2ab6..6a69f2f0 100644 --- a/src/main/java/frc/robot/utils/GamePieceLocate.java +++ b/src/main/java/frc/robot/utils/GamePieceLocate.java @@ -74,18 +74,18 @@ public static AlgaePositions findAlgaePos( double maxDot = Constants.MINIMUM_PIECE_DETECTION_DOT; AlgaePositions closest = null; int n = CenterPositions.getClosest(robotPos); - for (int i = 2 * n - 2; i < 2 * n + 4; i++) { - int f = i % ALGAES.length; - Matrix locationVec = - (invCameraRotation.times(PRECOMPUTED_ALGAE_VECS[i].minus(cameraPosVec))); - double dot = - pieceVec.dot( - VecBuilder.fill(locationVec.get(0, 0), locationVec.get(1, 0), locationVec.get(2, 0)) - .unit()); - if (dot > maxDot) { - maxDot = dot; - closest = ALGAES[i]; - } + for (int i = 2 * n - 2; i < 2 * n + 4; i++) { + int f = i % ALGAES.length; + Matrix locationVec = + (invCameraRotation.times(PRECOMPUTED_ALGAE_VECS[f].minus(cameraPosVec))); + double dot = + pieceVec.dot( + VecBuilder.fill(locationVec.get(0, 0), locationVec.get(1, 0), locationVec.get(2, 0)) + .unit()); + if (dot > maxDot) { + maxDot = dot; + closest = ALGAES[f]; + } } return closest; } From cec8d7945289f53c463e00d90f350f5ee62eaa5d Mon Sep 17 00:00:00 2001 From: codetoad Date: Tue, 25 Mar 2025 16:11:20 -0400 Subject: [PATCH 82/99] use Math.floorMod() because % does not work with negatives in java --- src/main/java/frc/robot/utils/GamePieceLocate.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/utils/GamePieceLocate.java b/src/main/java/frc/robot/utils/GamePieceLocate.java index 6a69f2f0..2eba35da 100644 --- a/src/main/java/frc/robot/utils/GamePieceLocate.java +++ b/src/main/java/frc/robot/utils/GamePieceLocate.java @@ -43,7 +43,7 @@ public static BranchPositions findCoralBranch( BranchPositions closest = null; int n = CenterPositions.getClosest(robotPos); for (int i = 6 * n - 3; i < 6 * n + 9; i++) { - int f = i % BRANCHES.length; + int f = Math.floorMod(i, BRANCHES.length); Matrix locationVec = (invCameraRotation.times(PRECOMPUTED_BRANCH_VECS[f].minus(cameraPosVec))); double dot = @@ -75,7 +75,7 @@ public static AlgaePositions findAlgaePos( AlgaePositions closest = null; int n = CenterPositions.getClosest(robotPos); for (int i = 2 * n - 2; i < 2 * n + 4; i++) { - int f = i % ALGAES.length; + int f = Math.floorMod(i, ALGAES.length); Matrix locationVec = (invCameraRotation.times(PRECOMPUTED_ALGAE_VECS[f].minus(cameraPosVec))); double dot = From 34c50a820b66b30223a0ebaa43238f46c00f970c Mon Sep 17 00:00:00 2001 From: codetoad Date: Tue, 25 Mar 2025 21:28:49 -0400 Subject: [PATCH 83/99] map to right bumper --- src/main/java/frc/robot/RobotContainer.java | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 27a95272..754d3fa2 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -227,10 +227,9 @@ private void configureBindings() { JoystickButton joyLeft2 = new JoystickButton(joyleft, 2); JoystickButton joyRight1 = new JoystickButton(joyright, 1); - JoystickButton joyLeft8 = new JoystickButton(joyleft, 8); RobotSlide robotSlide = new RobotSlide(drivetrain, joyleft::getX, joyleft::getY); joyLeft2.whileTrue(robotSlide); - + controller.rightBumper().onTrue(new AlignClosestBranch(drivetrain)); controller .leftTrigger() .onTrue( @@ -276,7 +275,6 @@ private void configureBindings() { // hihiExtender)); .onTrue(new CancelAll(byebyeTilt, byebyeRoller, elevatorSubsystem, climber)); joyRight1.onTrue(new ShootCoral(coralSubsystem, Constants.CORAL_SHOOTER_SPEED)); - joyLeft8.onTrue(new AlignClosestBranch(drivetrain)); // climber on Right Trigger // if (Constants.COMMAND_DEBUG) { // SmartDashboard.putData("Roll Algae", new RollAlgae(hihiRoller, 0.5)); From 54d0ab34607714013d76a8fa70926ff5ada4ab0c Mon Sep 17 00:00:00 2001 From: codetoad Date: Tue, 25 Mar 2025 21:31:27 -0400 Subject: [PATCH 84/99] wrap in try catch --- .../alignment/AlignClosestBranch.java | 37 +++++++++++-------- 1 file changed, 22 insertions(+), 15 deletions(-) diff --git a/src/main/java/frc/robot/commands/alignment/AlignClosestBranch.java b/src/main/java/frc/robot/commands/alignment/AlignClosestBranch.java index ab215618..7779cf81 100644 --- a/src/main/java/frc/robot/commands/alignment/AlignClosestBranch.java +++ b/src/main/java/frc/robot/commands/alignment/AlignClosestBranch.java @@ -1,6 +1,7 @@ package frc.robot.commands.alignment; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.CommandScheduler; import frc.robot.constants.AlignmentPosition; @@ -23,24 +24,30 @@ public AlignClosestBranch(SwerveDrivetrain drivetrain) { @Override public void initialize() { timer.restart(); - AlignmentPosition alignmentTarget = - AlignmentPosition.getClosest(drivetrain.getPose().getTranslation()); - Logger.recordOutput("TargetReefAlignment", alignmentTarget); - Pose2d position = alignmentTarget.getPosition(); - Logger.recordOutput("TargetReefPose", position); - drivetrain.setFocusedApriltag(alignmentTarget.getTag()); - // spotless:off (shhhh don't tell anyone about the linter bypass) - followTrajectory = LoggableCommandWrapper.wrap( - drivetrain.generateTrajectoryCommand(position) - ).withBasicName("FollowAlignToBranchTrajectory"); - // spotless:on - followTrajectory.setParent(this); - followTrajectory.schedule(); + try{ + AlignmentPosition alignmentTarget = + AlignmentPosition.getClosest(drivetrain.getPose().getTranslation()); + Logger.recordOutput("TargetReefAlignment", alignmentTarget); + Pose2d position = alignmentTarget.getPosition(); + Logger.recordOutput("TargetReefPose", position); + drivetrain.setFocusedApriltag(alignmentTarget.getTag()); + // spotless:off (shhhh don't tell anyone about the linter bypass) + followTrajectory = LoggableCommandWrapper.wrap( + drivetrain.generateTrajectoryCommand(position) + ).withBasicName("FollowAlignToBranchTrajectory"); + // spotless:on + followTrajectory.setParent(this); + followTrajectory.schedule(); + }catch (Exception e){ + DriverStation.reportError(e.getMessage(), true); + followTrajectory = null; + } + } @Override public void end(boolean interrupted) { - if (CommandScheduler.getInstance().isScheduled(followTrajectory)) { + if (followTrajectory != null && CommandScheduler.getInstance().isScheduled(followTrajectory)) { CommandScheduler.getInstance().cancel(followTrajectory); } drivetrain.exitFocusMode(); @@ -48,6 +55,6 @@ public void end(boolean interrupted) { @Override public boolean isFinished() { - return followTrajectory.isFinished() || timer.hasElapsed(Constants.AUTO_ALIGN_TIMEOUT); + return followTrajectory == null || followTrajectory.isFinished() || timer.hasElapsed(Constants.AUTO_ALIGN_TIMEOUT); } } From 341c0667fa8177686bfeacee02589e5d19a68980 Mon Sep 17 00:00:00 2001 From: codetoad Date: Tue, 25 Mar 2025 23:06:47 -0400 Subject: [PATCH 85/99] fixed tag mirroring --- .../commands/alignment/AlignClosestBranch.java | 11 ++++++----- .../frc/robot/constants/AlignmentPosition.java | 16 ++++++++-------- 2 files changed, 14 insertions(+), 13 deletions(-) diff --git a/src/main/java/frc/robot/commands/alignment/AlignClosestBranch.java b/src/main/java/frc/robot/commands/alignment/AlignClosestBranch.java index 7779cf81..2aa5beab 100644 --- a/src/main/java/frc/robot/commands/alignment/AlignClosestBranch.java +++ b/src/main/java/frc/robot/commands/alignment/AlignClosestBranch.java @@ -24,9 +24,9 @@ public AlignClosestBranch(SwerveDrivetrain drivetrain) { @Override public void initialize() { timer.restart(); - try{ + try { AlignmentPosition alignmentTarget = - AlignmentPosition.getClosest(drivetrain.getPose().getTranslation()); + AlignmentPosition.getClosest(drivetrain.getPose().getTranslation()); Logger.recordOutput("TargetReefAlignment", alignmentTarget); Pose2d position = alignmentTarget.getPosition(); Logger.recordOutput("TargetReefPose", position); @@ -38,11 +38,10 @@ public void initialize() { // spotless:on followTrajectory.setParent(this); followTrajectory.schedule(); - }catch (Exception e){ + } catch (Exception e) { DriverStation.reportError(e.getMessage(), true); followTrajectory = null; } - } @Override @@ -55,6 +54,8 @@ public void end(boolean interrupted) { @Override public boolean isFinished() { - return followTrajectory == null || followTrajectory.isFinished() || timer.hasElapsed(Constants.AUTO_ALIGN_TIMEOUT); + return followTrajectory == null + || followTrajectory.isFinished() + || timer.hasElapsed(Constants.AUTO_ALIGN_TIMEOUT); } } diff --git a/src/main/java/frc/robot/constants/AlignmentPosition.java b/src/main/java/frc/robot/constants/AlignmentPosition.java index bb508e36..4c3bccb4 100644 --- a/src/main/java/frc/robot/constants/AlignmentPosition.java +++ b/src/main/java/frc/robot/constants/AlignmentPosition.java @@ -13,16 +13,16 @@ public enum AlignmentPosition { // spotless:off Although this makes it harder to read it makes it easy to copy and paste from sheets BRANCH_A(new Pose2d(3.2076000000000, 4.0614305050000, Rotation2d.fromDegrees(0)), Apriltag.EIGHTEEN, Apriltag.SEVEN), BRANCH_B(new Pose2d(3.2076000000000, 3.7328134950000, Rotation2d.fromRadians(0)), Apriltag.EIGHTEEN, Apriltag.SEVEN), - BRANCH_C(new Pose2d(3.8176981000607, 2.9336485880936, Rotation2d.fromDegrees(60)), Apriltag.SEVENTEEN, Apriltag.SIX), - BRANCH_D(new Pose2d(4.1022887788364, 2.7693400830936, Rotation2d.fromDegrees(60)), Apriltag.SEVENTEEN, Apriltag.SIX), - BRANCH_E(new Pose2d(5.0994349400607, 2.8981180830936, Rotation2d.fromDegrees(120)), Apriltag.TWENTY_TWO, Apriltag.ELEVEN), - BRANCH_F(new Pose2d(5.3840256188364, 3.0624265880936, Rotation2d.fromDegrees(120)), Apriltag.TWENTY_TWO, Apriltag.ELEVEN), + BRANCH_C(new Pose2d(3.8176981000607, 2.9336485880936, Rotation2d.fromDegrees(60)), Apriltag.SEVENTEEN, Apriltag.EIGHT), + BRANCH_D(new Pose2d(4.1022887788364, 2.7693400830936, Rotation2d.fromDegrees(60)), Apriltag.SEVENTEEN, Apriltag.EIGHT), + BRANCH_E(new Pose2d(5.0994349400607, 2.8981180830936, Rotation2d.fromDegrees(120)), Apriltag.TWENTY_TWO, Apriltag.NINE), + BRANCH_F(new Pose2d(5.3840256188364, 3.0624265880936, Rotation2d.fromDegrees(120)), Apriltag.TWENTY_TWO, Apriltag.NINE), BRANCH_G(new Pose2d(5.7710736800000, 3.9903694950000, Rotation2d.fromDegrees(180)), Apriltag.TWENTY_ONE, Apriltag.TEN), BRANCH_H(new Pose2d(5.7710736800000, 4.3189865050000, Rotation2d.fromDegrees(180)), Apriltag.TWENTY_ONE, Apriltag.TEN), - BRANCH_I(new Pose2d(5.1609755799393, 5.1181514119064, Rotation2d.fromDegrees(240)), Apriltag.TWENTY, Apriltag.NINE), - BRANCH_J(new Pose2d(4.8763849011636, 5.2824599169064, Rotation2d.fromDegrees(240)), Apriltag.TWENTY, Apriltag.NINE), - BRANCH_K(new Pose2d(3.8792387399393, 5.1536819169064, Rotation2d.fromDegrees(300)), Apriltag.NINETEEN, Apriltag.EIGHT), - BRANCH_L(new Pose2d(3.5946480611636, 4.9893734119064, Rotation2d.fromDegrees(300)), Apriltag.NINETEEN, Apriltag.EIGHT); + BRANCH_I(new Pose2d(5.1609755799393, 5.1181514119064, Rotation2d.fromDegrees(240)), Apriltag.TWENTY, Apriltag.ELEVEN), + BRANCH_J(new Pose2d(4.8763849011636, 5.2824599169064, Rotation2d.fromDegrees(240)), Apriltag.TWENTY, Apriltag.ELEVEN), + BRANCH_K(new Pose2d(3.8792387399393, 5.1536819169064, Rotation2d.fromDegrees(300)), Apriltag.NINETEEN, Apriltag.SIX), + BRANCH_L(new Pose2d(3.5946480611636, 4.9893734119064, Rotation2d.fromDegrees(300)), Apriltag.NINETEEN, Apriltag.SIX); // spotless:on private final Pose2d bluePosition; From 9864d1db2742974e58822d65f7d50f265813a7da Mon Sep 17 00:00:00 2001 From: codetoad Date: Tue, 25 Mar 2025 23:21:50 -0400 Subject: [PATCH 86/99] fixed sign error --- src/main/java/frc/robot/constants/AlignmentPosition.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/constants/AlignmentPosition.java b/src/main/java/frc/robot/constants/AlignmentPosition.java index 4c3bccb4..1ec06b1d 100644 --- a/src/main/java/frc/robot/constants/AlignmentPosition.java +++ b/src/main/java/frc/robot/constants/AlignmentPosition.java @@ -32,7 +32,7 @@ public enum AlignmentPosition { AlignmentPosition(Pose2d position, Apriltag blueTag, Apriltag redTag) { this.bluePosition = position; - Rotation2d redRotation = Rotation2d.fromRadians(Math.PI - position.getRotation().getRadians()); + Rotation2d redRotation = Rotation2d.fromRadians(Math.PI + position.getRotation().getRadians()); double redX = FieldLocation.LENGTH_OF_FIELD - bluePosition.getX(); double redY = FieldLocation.HEIGHT_OF_FIELD - bluePosition.getY(); this.redPosition = new Pose2d(redX, redY, redRotation); From b06c20d34b846b457a8e7f450066da7572e0ed8c Mon Sep 17 00:00:00 2001 From: codetoad Date: Wed, 26 Mar 2025 10:09:31 -0400 Subject: [PATCH 87/99] omg it works!!!! --- src/main/java/frc/robot/constants/GameConstants.java | 2 +- src/test/java/frc/robot/utils/GamePieceLocateTest.java | 7 +++---- 2 files changed, 4 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java index 70d895f0..7b5d58b2 100644 --- a/src/main/java/frc/robot/constants/GameConstants.java +++ b/src/main/java/frc/robot/constants/GameConstants.java @@ -227,7 +227,7 @@ public enum Mode { // Limelight public static final Transform3d LIMELIGHT_TO_ROBOT = - new Transform3d(0.0, 0.0, 0, new Rotation3d(0.0, Math.PI / 6, 0.0)); // z = 0.720725 + new Transform3d(0.0, 0.0, 0, new Rotation3d(0.0, -Math.PI / 6, 0.0)); // z = 0.720725 public static final String LIMELIGHT_IP_ADDRESS = "10.40.48.104"; // TODO Change Later public static final double MINIMUM_PIECE_DETECTION_DOT = 0; } diff --git a/src/test/java/frc/robot/utils/GamePieceLocateTest.java b/src/test/java/frc/robot/utils/GamePieceLocateTest.java index c651683e..8ce71917 100644 --- a/src/test/java/frc/robot/utils/GamePieceLocateTest.java +++ b/src/test/java/frc/robot/utils/GamePieceLocateTest.java @@ -13,11 +13,10 @@ class GamePieceLocateTest { void findAlgaePos() { double robotX = Apriltag.EIGHTEEN.getX() - 0.889; double robotY = Apriltag.EIGHTEEN.getY(); - double visionX = 5; // degrees - double visionY = -1; // degrees + double txDeg = 0; // degrees + double dyDeg = 5; // degrees AlgaePositions algaePos = - GamePieceLocate.findAlgaePos( - new Pose2d(robotX, robotY, new Rotation2d(0)), visionX, visionY); + GamePieceLocate.findAlgaePos(new Pose2d(robotX, robotY, new Rotation2d(0)), txDeg, dyDeg); assertEquals(AlgaePositions.Algae_AB_LOW, algaePos); } } From c6f47d5a04aa0a99eb54b0cda1295ceacf911b8d Mon Sep 17 00:00:00 2001 From: codetoad Date: Wed, 26 Mar 2025 10:11:28 -0400 Subject: [PATCH 88/99] removed limelight feed --- .../frc/robot/subsystems/limelight/RealVisionIO.java | 9 --------- 1 file changed, 9 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/limelight/RealVisionIO.java b/src/main/java/frc/robot/subsystems/limelight/RealVisionIO.java index 6bacb1b6..0aa11b50 100644 --- a/src/main/java/frc/robot/subsystems/limelight/RealVisionIO.java +++ b/src/main/java/frc/robot/subsystems/limelight/RealVisionIO.java @@ -20,15 +20,6 @@ public RealVisionIO() { Robot.getDiagnostics().addDiagnosable(new DiagLimelight("Vision", "Piece Seen")); NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight"); ledModeEntry = table.getEntry("ledMode"); - HttpCamera limelightFeed = - new HttpCamera( - "limelight", "http://" + Constants.LIMELIGHT_IP_ADDRESS + ":5800/stream.mjpg"); - ShuffleboardTab dashboardTab = Shuffleboard.getTab("Driver"); - dashboardTab - .add("Limelight feed", limelightFeed) - .withSize(6, 4) - .withPosition(2, 0) - .withProperties(Map.of("Show Crosshair", false, "Show Controls", false)); } @Override From f733205b3edbf6d02af3c11202357ab5dc077f48 Mon Sep 17 00:00:00 2001 From: codetoad Date: Wed, 26 Mar 2025 10:13:48 -0400 Subject: [PATCH 89/99] added more tests --- .../subsystems/limelight/RealVisionIO.java | 5 ----- .../frc/robot/utils/GamePieceLocateTest.java | 20 ++++++++++++++++++- 2 files changed, 19 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/limelight/RealVisionIO.java b/src/main/java/frc/robot/subsystems/limelight/RealVisionIO.java index 0aa11b50..214d4233 100644 --- a/src/main/java/frc/robot/subsystems/limelight/RealVisionIO.java +++ b/src/main/java/frc/robot/subsystems/limelight/RealVisionIO.java @@ -1,16 +1,11 @@ package frc.robot.subsystems.limelight; -import edu.wpi.first.cscore.HttpCamera; import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableEntry; import edu.wpi.first.networktables.NetworkTableInstance; -import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; -import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import frc.robot.Robot; -import frc.robot.constants.Constants; import frc.robot.utils.LimelightHelpers; import frc.robot.utils.diag.DiagLimelight; -import java.util.Map; public class RealVisionIO implements VisionIO { private LimelightHelpers.LimelightResults results; diff --git a/src/test/java/frc/robot/utils/GamePieceLocateTest.java b/src/test/java/frc/robot/utils/GamePieceLocateTest.java index 8ce71917..f59effb6 100644 --- a/src/test/java/frc/robot/utils/GamePieceLocateTest.java +++ b/src/test/java/frc/robot/utils/GamePieceLocateTest.java @@ -14,9 +14,27 @@ void findAlgaePos() { double robotX = Apriltag.EIGHTEEN.getX() - 0.889; double robotY = Apriltag.EIGHTEEN.getY(); double txDeg = 0; // degrees - double dyDeg = 5; // degrees + double dyDeg = -5; // degrees AlgaePositions algaePos = GamePieceLocate.findAlgaePos(new Pose2d(robotX, robotY, new Rotation2d(0)), txDeg, dyDeg); assertEquals(AlgaePositions.Algae_AB_LOW, algaePos); + + txDeg = 0; // degrees + dyDeg = 20; // degrees + algaePos = + GamePieceLocate.findAlgaePos(new Pose2d(robotX, robotY, new Rotation2d(0)), txDeg, dyDeg); + assertEquals(AlgaePositions.Algae_AB_HIGH, algaePos); + + txDeg = 10; // degrees + dyDeg = 0; // degrees + algaePos = + GamePieceLocate.findAlgaePos(new Pose2d(robotX, robotY, new Rotation2d(0)), txDeg, dyDeg); + assertEquals(AlgaePositions.Algae_CD_LOW, algaePos); + + txDeg = -10; // degrees + dyDeg = 0; // degrees + algaePos = + GamePieceLocate.findAlgaePos(new Pose2d(robotX, robotY, new Rotation2d(0)), txDeg, dyDeg); + assertEquals(AlgaePositions.Algae_KL_LOW, algaePos); } } From e4f0ad57bbc1f37eb734e5282237137abd53b821 Mon Sep 17 00:00:00 2001 From: codetoad Date: Wed, 26 Mar 2025 10:25:56 -0400 Subject: [PATCH 90/99] built code --- .../java/frc/robot/subsystems/limelight/RealVisionIO.java | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/limelight/RealVisionIO.java b/src/main/java/frc/robot/subsystems/limelight/RealVisionIO.java index 214d4233..de07ec50 100644 --- a/src/main/java/frc/robot/subsystems/limelight/RealVisionIO.java +++ b/src/main/java/frc/robot/subsystems/limelight/RealVisionIO.java @@ -12,7 +12,9 @@ public class RealVisionIO implements VisionIO { private final NetworkTableEntry ledModeEntry; public RealVisionIO() { - Robot.getDiagnostics().addDiagnosable(new DiagLimelight("Vision", "Piece Seen")); + Robot.getDiagnostics().addDiagnosable( + + new DiagLimelight("Vision", "Piece Seen")); NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight"); ledModeEntry = table.getEntry("ledMode"); } From fc93d55b121c2bc211c8e6dd3035b06716949f55 Mon Sep 17 00:00:00 2001 From: codetoad Date: Wed, 26 Mar 2025 12:16:57 -0400 Subject: [PATCH 91/99] I dont know anymore --- src/main/java/frc/robot/RobotContainer.java | 1 - .../commands/SetSuperAutoScorePosition.java | 50 +++++++++++ .../frc/robot/commands/SuperAutoScore.java | 17 ++++ .../robot/constants/AlignmentPosition.java | 36 +++++--- .../frc/robot/constants/BranchPositions.java | 86 ++++++++++++++++++- .../frc/robot/constants/ElevatorPosition.java | 21 +++-- .../subsystems/limelight/RealVisionIO.java | 3 +- 7 files changed, 191 insertions(+), 23 deletions(-) create mode 100644 src/main/java/frc/robot/commands/SetSuperAutoScorePosition.java create mode 100644 src/main/java/frc/robot/commands/SuperAutoScore.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 4bb91f75..347c3371 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -86,7 +86,6 @@ import frc.robot.utils.logging.LoggableIO; import frc.robot.utils.motor.Gain; import frc.robot.utils.motor.PID; -import frc.robot.utils.shuffleboard.SmartShuffleboard; import frc.robot.utils.simulation.RobotVisualizer; import frc.robot.utils.simulation.SwerveSimulationUtils; import java.util.function.Consumer; diff --git a/src/main/java/frc/robot/commands/SetSuperAutoScorePosition.java b/src/main/java/frc/robot/commands/SetSuperAutoScorePosition.java new file mode 100644 index 00000000..b7c38bd9 --- /dev/null +++ b/src/main/java/frc/robot/commands/SetSuperAutoScorePosition.java @@ -0,0 +1,50 @@ +package frc.robot.commands; + +import frc.robot.constants.AlignmentPosition; +import frc.robot.constants.BranchPositions; +import frc.robot.constants.ElevatorPosition; +import frc.robot.subsystems.elevator.ElevatorSubsystem; +import frc.robot.subsystems.limelight.Vision; +import frc.robot.subsystems.swervev3.SwerveDrivetrain; +import frc.robot.utils.logging.commands.LoggableCommand; + +import java.util.Arrays; +import java.util.Comparator; + +public class SetSuperAutoScorePosition extends LoggableCommand { + private final SwerveDrivetrain drivetrain; + private final ElevatorSubsystem elevatorSubsystem; + private final Vision vision; + + public SetSuperAutoScorePosition( + SwerveDrivetrain drivetrain, ElevatorSubsystem elevatorSubsystem, Vision vision) { + this.drivetrain = drivetrain; + this.elevatorSubsystem = elevatorSubsystem; + this.vision = vision; + } + + @Override + public void execute() { + AlignmentPosition closest = AlignmentPosition.getClosest(drivetrain.getPose().getTranslation()); + BranchPositions[] relevantPoses = + Arrays.stream(vision.getAllBranchPosition()) + .filter(b -> b.getTrunk() == closest) + .sorted(Comparator.comparingInt(v -> v.getElevatorLevel().getWeight())) + .toArray(BranchPositions[]::new); + if (relevantPoses.length == 0 || relevantPoses[0].getElevatorLevel() != ElevatorPosition.LEVEL4) { + elevatorSubsystem.setElevatorPosition(ElevatorPosition.LEVEL4.getElevatorHeight()); + } else if (relevantPoses.length == 1 || relevantPoses[1].getElevatorLevel() != ElevatorPosition.LEVEL3) { + elevatorSubsystem.setElevatorPosition(ElevatorPosition.LEVEL3.getElevatorHeight()); + } else if (relevantPoses.length == 2 || relevantPoses[2].getElevatorLevel() != ElevatorPosition.LEVEL2) { + elevatorSubsystem.setElevatorPosition(ElevatorPosition.LEVEL3.getElevatorHeight()); + }else if (relevantPoses.length == 3 || relevantPoses[3].getElevatorLevel() != ElevatorPosition.LEVEL1) { + elevatorSubsystem.setElevatorPosition(ElevatorPosition.LEVEL3.getElevatorHeight()); + } + + } + + @Override + public boolean isFinished() { + return true; + } +} diff --git a/src/main/java/frc/robot/commands/SuperAutoScore.java b/src/main/java/frc/robot/commands/SuperAutoScore.java new file mode 100644 index 00000000..6010b878 --- /dev/null +++ b/src/main/java/frc/robot/commands/SuperAutoScore.java @@ -0,0 +1,17 @@ +package frc.robot.commands; + +import frc.robot.subsystems.coral.CoralSubsystem; +import frc.robot.subsystems.elevator.ElevatorSubsystem; +import frc.robot.subsystems.limelight.Vision; +import frc.robot.subsystems.swervev3.SwerveDrivetrain; +import frc.robot.utils.logging.commands.LoggableSequentialCommandGroup; + +public class SuperAutoScore extends LoggableSequentialCommandGroup { + public SuperAutoScore( + SwerveDrivetrain drivetrain, + ElevatorSubsystem elevator, + CoralSubsystem coralSubsystem, + Vision vision) { + super(new SetSuperAutoScorePosition(drivetrain, elevator, vision)); + } +} diff --git a/src/main/java/frc/robot/constants/AlignmentPosition.java b/src/main/java/frc/robot/constants/AlignmentPosition.java index 1ec06b1d..f02f8165 100644 --- a/src/main/java/frc/robot/constants/AlignmentPosition.java +++ b/src/main/java/frc/robot/constants/AlignmentPosition.java @@ -11,18 +11,30 @@ public enum AlignmentPosition { // spotless:off Although this makes it harder to read it makes it easy to copy and paste from sheets - BRANCH_A(new Pose2d(3.2076000000000, 4.0614305050000, Rotation2d.fromDegrees(0)), Apriltag.EIGHTEEN, Apriltag.SEVEN), - BRANCH_B(new Pose2d(3.2076000000000, 3.7328134950000, Rotation2d.fromRadians(0)), Apriltag.EIGHTEEN, Apriltag.SEVEN), - BRANCH_C(new Pose2d(3.8176981000607, 2.9336485880936, Rotation2d.fromDegrees(60)), Apriltag.SEVENTEEN, Apriltag.EIGHT), - BRANCH_D(new Pose2d(4.1022887788364, 2.7693400830936, Rotation2d.fromDegrees(60)), Apriltag.SEVENTEEN, Apriltag.EIGHT), - BRANCH_E(new Pose2d(5.0994349400607, 2.8981180830936, Rotation2d.fromDegrees(120)), Apriltag.TWENTY_TWO, Apriltag.NINE), - BRANCH_F(new Pose2d(5.3840256188364, 3.0624265880936, Rotation2d.fromDegrees(120)), Apriltag.TWENTY_TWO, Apriltag.NINE), - BRANCH_G(new Pose2d(5.7710736800000, 3.9903694950000, Rotation2d.fromDegrees(180)), Apriltag.TWENTY_ONE, Apriltag.TEN), - BRANCH_H(new Pose2d(5.7710736800000, 4.3189865050000, Rotation2d.fromDegrees(180)), Apriltag.TWENTY_ONE, Apriltag.TEN), - BRANCH_I(new Pose2d(5.1609755799393, 5.1181514119064, Rotation2d.fromDegrees(240)), Apriltag.TWENTY, Apriltag.ELEVEN), - BRANCH_J(new Pose2d(4.8763849011636, 5.2824599169064, Rotation2d.fromDegrees(240)), Apriltag.TWENTY, Apriltag.ELEVEN), - BRANCH_K(new Pose2d(3.8792387399393, 5.1536819169064, Rotation2d.fromDegrees(300)), Apriltag.NINETEEN, Apriltag.SIX), - BRANCH_L(new Pose2d(3.5946480611636, 4.9893734119064, Rotation2d.fromDegrees(300)), Apriltag.NINETEEN, Apriltag.SIX); + + A(new Pose2d(3.2076000000000, 4.0614305050000, Rotation2d.fromDegrees(0)), Apriltag.EIGHTEEN, Apriltag.SEVEN), + + B(new Pose2d(3.2076000000000, 3.7328134950000, Rotation2d.fromRadians(0)), Apriltag.EIGHTEEN, Apriltag.SEVEN), + + C(new Pose2d(3.8176981000607, 2.9336485880936, Rotation2d.fromDegrees(60)), Apriltag.SEVENTEEN, Apriltag.EIGHT), + + D(new Pose2d(4.1022887788364, 2.7693400830936, Rotation2d.fromDegrees(60)), Apriltag.SEVENTEEN, Apriltag.EIGHT), + + E(new Pose2d(5.0994349400607, 2.8981180830936, Rotation2d.fromDegrees(120)), Apriltag.TWENTY_TWO, Apriltag.NINE), + + F(new Pose2d(5.3840256188364, 3.0624265880936, Rotation2d.fromDegrees(120)), Apriltag.TWENTY_TWO, Apriltag.NINE), + + G(new Pose2d(5.7710736800000, 3.9903694950000, Rotation2d.fromDegrees(180)), Apriltag.TWENTY_ONE, Apriltag.TEN), + + H(new Pose2d(5.7710736800000, 4.3189865050000, Rotation2d.fromDegrees(180)), Apriltag.TWENTY_ONE, Apriltag.TEN), + + I(new Pose2d(5.1609755799393, 5.1181514119064, Rotation2d.fromDegrees(240)), Apriltag.TWENTY, Apriltag.ELEVEN), + + J(new Pose2d(4.8763849011636, 5.2824599169064, Rotation2d.fromDegrees(240)), Apriltag.TWENTY, Apriltag.ELEVEN), + + K(new Pose2d(3.8792387399393, 5.1536819169064, Rotation2d.fromDegrees(300)), Apriltag.NINETEEN, Apriltag.SIX), + + L(new Pose2d(3.5946480611636, 4.9893734119064, Rotation2d.fromDegrees(300)), Apriltag.NINETEEN, Apriltag.SIX); // spotless:on private final Pose2d bluePosition; diff --git a/src/main/java/frc/robot/constants/BranchPositions.java b/src/main/java/frc/robot/constants/BranchPositions.java index ff961848..cf8c8d25 100644 --- a/src/main/java/frc/robot/constants/BranchPositions.java +++ b/src/main/java/frc/robot/constants/BranchPositions.java @@ -6,121 +6,205 @@ // TODO: change all of these later to the correct values public enum BranchPositions { BRANCH_A_2( + AlignmentPosition.A, + ElevatorPosition.LEVEL2, new Pose3d( 3.8337244850000, 4.1902085050000, 0.706978325, new Rotation3d(0, 0, 3.1415926535898))), BRANCH_A_3( + AlignmentPosition.A, + ElevatorPosition.LEVEL3, new Pose3d( 3.8337244850000, 4.1902085050000, 1.111645305, new Rotation3d(0, 0, 3.1415926535898))), BRANCH_A_4( + AlignmentPosition.A, + ElevatorPosition.LEVEL4, new Pose3d( 3.6896079100000, 4.1902085050000, 1.736182305, new Rotation3d(0, 0, 3.1415926535898))), BRANCH_B_2( + AlignmentPosition.B, + ElevatorPosition.LEVEL2, new Pose3d( 3.8337244850000, 3.8615914950000, 0.706978325, new Rotation3d(0, 0, 3.1415926535898))), BRANCH_B_3( + AlignmentPosition.B, + ElevatorPosition.LEVEL3, new Pose3d( 3.8337244850000, 3.8615914950000, 1.111645305, new Rotation3d(0, 0, 3.1415926535898))), BRANCH_B_4( + AlignmentPosition.B, + ElevatorPosition.LEVEL4, new Pose3d( 3.6896079100000, 3.8615914950000, 1.736182305, new Rotation3d(0, 0, 3.1415926535898))), BRANCH_C_2( + AlignmentPosition.C, + ElevatorPosition.LEVEL2, new Pose3d( 4.0192353231122, 3.5402772980351, 0.706978325, new Rotation3d(0, 0, 4.1887902047864))), BRANCH_C_3( + AlignmentPosition.C, + ElevatorPosition.LEVEL3, new Pose3d( 4.0192353231122, 3.5402772980351, 1.111645305, new Rotation3d(0, 0, 4.1887902047864))), BRANCH_C_4( + AlignmentPosition.C, + ElevatorPosition.LEVEL4, new Pose3d( 3.9471770356122, 3.4154686829787, 1.736182305, new Rotation3d(0, 0, 4.1887902047864))), BRANCH_D_2( + AlignmentPosition.D, + ElevatorPosition.LEVEL2, new Pose3d( 4.3038260018878, 3.3759687930351, 0.706978325, new Rotation3d(0, 0, 4.1887902047864))), BRANCH_D_3( + AlignmentPosition.D, + ElevatorPosition.LEVEL3, new Pose3d( 4.3038260018878, 3.3759687930351, 1.111645305, new Rotation3d(0, 0, 4.1887902047864))), BRANCH_D_4( + AlignmentPosition.D, + ElevatorPosition.LEVEL4, new Pose3d( 4.2317677143878, 3.2511601779787, 1.736182305, new Rotation3d(0, 0, 4.1887902047864))), BRANCH_E_2( + AlignmentPosition.E, + ElevatorPosition.LEVEL2, new Pose3d( 4.6748476781122, 3.3759687930351, 0.706978325, new Rotation3d(0, 0, 5.2359877559830))), BRANCH_E_3( + AlignmentPosition.E, + ElevatorPosition.LEVEL3, new Pose3d( 4.6748476781122, 3.3759687930351, 1.111645305, new Rotation3d(0, 0, 5.2359877559830))), BRANCH_E_4( + AlignmentPosition.E, + ElevatorPosition.LEVEL4, new Pose3d( 4.7469059656122, 3.2511601779787, 1.736182305, new Rotation3d(0, 0, 5.2359877559830))), BRANCH_F_2( + AlignmentPosition.F, + ElevatorPosition.LEVEL2, new Pose3d( 4.9594383568878, 3.5402772980351, 0.706978325, new Rotation3d(0, 0, 5.2359877559830))), BRANCH_F_3( + AlignmentPosition.F, + ElevatorPosition.LEVEL3, new Pose3d( 4.9594383568878, 3.5402772980351, 1.111645305, new Rotation3d(0, 0, 5.2359877559830))), BRANCH_F_4( + AlignmentPosition.F, + ElevatorPosition.LEVEL4, new Pose3d( 5.0314966443878, 3.4154686829787, 1.736182305, new Rotation3d(0, 0, 5.2359877559830))), BRANCH_G_2( + AlignmentPosition.G, + ElevatorPosition.LEVEL2, new Pose3d( 5.1449491950000, 3.8615914950000, 0.706978325, new Rotation3d(0, 0, 6.2831853071796))), BRANCH_G_3( + AlignmentPosition.G, + ElevatorPosition.LEVEL3, new Pose3d( 5.1449491950000, 3.8615914950000, 1.111645305, new Rotation3d(0, 0, 6.2831853071796))), BRANCH_G_4( + AlignmentPosition.G, + ElevatorPosition.LEVEL4, new Pose3d( 5.2890657700000, 3.8615914950000, 1.736182305, new Rotation3d(0, 0, 6.2831853071796))), BRANCH_H_2( + AlignmentPosition.H, + ElevatorPosition.LEVEL2, new Pose3d( 5.1449491950000, 4.1902085050000, 0.706978325, new Rotation3d(0, 0, 6.2831853071796))), BRANCH_H_3( + AlignmentPosition.H, + ElevatorPosition.LEVEL3, new Pose3d( 5.1449491950000, 4.1902085050000, 1.111645305, new Rotation3d(0, 0, 6.2831853071796))), BRANCH_H_4( + AlignmentPosition.H, + ElevatorPosition.LEVEL4, new Pose3d( 5.2890657700000, 4.1902085050000, 1.736182305, new Rotation3d(0, 0, 6.2831853071796))), BRANCH_I_2( + AlignmentPosition.I, + ElevatorPosition.LEVEL2, new Pose3d( 4.9594383568878, 4.5115227019649, 0.706978325, new Rotation3d(0, 0, 7.3303828583762))), BRANCH_I_3( + AlignmentPosition.I, + ElevatorPosition.LEVEL3, new Pose3d( 4.9594383568878, 4.5115227019649, 1.111645305, new Rotation3d(0, 0, 7.3303828583762))), BRANCH_I_4( + AlignmentPosition.I, + ElevatorPosition.LEVEL4, new Pose3d( 5.0314966443878, 4.6363313170214, 1.736182305, new Rotation3d(0, 0, 7.3303828583762))), BRANCH_J_2( + AlignmentPosition.J, + ElevatorPosition.LEVEL2, new Pose3d( 4.6748476781122, 4.6758312069649, 0.706978325, new Rotation3d(0, 0, 7.3303828583762))), BRANCH_J_3( + AlignmentPosition.J, + ElevatorPosition.LEVEL3, new Pose3d( 4.6748476781122, 4.6758312069649, 1.111645305, new Rotation3d(0, 0, 7.3303828583762))), BRANCH_J_4( + AlignmentPosition.J, + ElevatorPosition.LEVEL4, new Pose3d( 4.7469059656122, 4.8006398220214, 1.736182305, new Rotation3d(0, 0, 7.3303828583762))), BRANCH_K_2( + AlignmentPosition.K, + ElevatorPosition.LEVEL2, new Pose3d( 4.3038260018878, 4.6758312069649, 0.706978325, new Rotation3d(0, 0, 8.3775804095728))), BRANCH_K_3( + AlignmentPosition.K, + ElevatorPosition.LEVEL3, new Pose3d( 4.3038260018878, 4.6758312069649, 1.111645305, new Rotation3d(0, 0, 8.3775804095728))), BRANCH_K_4( + AlignmentPosition.K, + ElevatorPosition.LEVEL4, new Pose3d( 4.2317677143878, 4.8006398220214, 1.736182305, new Rotation3d(0, 0, 8.3775804095728))), BRANCH_L_2( + AlignmentPosition.L, + ElevatorPosition.LEVEL2, new Pose3d( 4.0192353231122, 4.5115227019649, 0.706978325, new Rotation3d(0, 0, 8.3775804095728))), BRANCH_L_3( + AlignmentPosition.L, + ElevatorPosition.LEVEL3, new Pose3d( 4.0192353231122, 4.5115227019649, 1.111645305, new Rotation3d(0, 0, 8.3775804095728))), BRANCH_L_4( + AlignmentPosition.L, + ElevatorPosition.LEVEL4, new Pose3d( 3.9471770356122, 4.6363313170214, 1.736182305, new Rotation3d(0, 0, 8.3775804095728))); + private final AlignmentPosition trunk; + private final ElevatorPosition elevatorPosition; private final Pose3d position; - BranchPositions(Pose3d position) { + BranchPositions(AlignmentPosition trunk, ElevatorPosition elevatorPosition, Pose3d position) { + this.trunk = trunk; + this.elevatorPosition = elevatorPosition; this.position = position; } public Pose3d getPosition() { return position; } + + public AlignmentPosition getTrunk() { + return trunk; + } + + public ElevatorPosition getElevatorLevel() { + return elevatorPosition; + } } diff --git a/src/main/java/frc/robot/constants/ElevatorPosition.java b/src/main/java/frc/robot/constants/ElevatorPosition.java index b7f33c15..3e0ccb1f 100644 --- a/src/main/java/frc/robot/constants/ElevatorPosition.java +++ b/src/main/java/frc/robot/constants/ElevatorPosition.java @@ -1,20 +1,27 @@ package frc.robot.constants; public enum ElevatorPosition { - CORAL_INTAKE(-1), - CLIMB(-5.2), - LEVEL1(-3.0), - LEVEL2(-9.28), - LEVEL3(-24), - LEVEL4(-47.4); + CORAL_INTAKE(-1, 0), + CLIMB(-5.2, 0), + LEVEL1(-3.0, 1), + LEVEL2(-9.28, 2), + LEVEL3(-24, 3), + LEVEL4(-47.4, 4); private final double heightElevator; + private final int weight; - ElevatorPosition(double heightElevator) { + ElevatorPosition(double heightElevator, int wight) { this.heightElevator = heightElevator; + this.weight = wight; + } + + public int getWeight() { + return weight; } public double getElevatorHeight() { + return heightElevator; } } diff --git a/src/main/java/frc/robot/subsystems/limelight/RealVisionIO.java b/src/main/java/frc/robot/subsystems/limelight/RealVisionIO.java index de07ec50..134c691a 100644 --- a/src/main/java/frc/robot/subsystems/limelight/RealVisionIO.java +++ b/src/main/java/frc/robot/subsystems/limelight/RealVisionIO.java @@ -12,9 +12,8 @@ public class RealVisionIO implements VisionIO { private final NetworkTableEntry ledModeEntry; public RealVisionIO() { - Robot.getDiagnostics().addDiagnosable( + Robot.getDiagnostics().addDiagnosable(new DiagLimelight("Vision", "Piece Seen")); - new DiagLimelight("Vision", "Piece Seen")); NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight"); ledModeEntry = table.getEntry("ledMode"); } From 303330d5a70493fa51ad2acbf9cbeeb70b689a26 Mon Sep 17 00:00:00 2001 From: codetoad Date: Wed, 26 Mar 2025 12:23:45 -0400 Subject: [PATCH 92/99] added super auto score --- src/main/java/frc/robot/RobotContainer.java | 4 ++ .../commands/SetSuperAutoScorePosition.java | 66 ++++++++++--------- .../frc/robot/commands/SuperAutoScore.java | 13 +++- 3 files changed, 50 insertions(+), 33 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 347c3371..f200ec55 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -26,6 +26,7 @@ import frc.robot.autochooser.FieldLocation; import frc.robot.autochooser.chooser.AutoChooser2025; import frc.robot.autochooser.event.RealAutoEventProvider; +import frc.robot.commands.SuperAutoScore; import frc.robot.commands.alignment.AlignClosestBranch; import frc.robot.commands.byebye.ByeByeToFwrLimit; import frc.robot.commands.byebye.ByeByeToRevLimit; @@ -233,6 +234,9 @@ private void configureBindings() { RobotSlide robotSlide = new RobotSlide(drivetrain, joyleft::getX, joyleft::getY); joyLeft2.whileTrue(robotSlide); controller.rightBumper().onTrue(new AlignClosestBranch(drivetrain)); + controller + .rightTrigger() + .onTrue(new SuperAutoScore(drivetrain, elevatorSubsystem, coralSubsystem, vision)); controller .leftTrigger() .onTrue( diff --git a/src/main/java/frc/robot/commands/SetSuperAutoScorePosition.java b/src/main/java/frc/robot/commands/SetSuperAutoScorePosition.java index b7c38bd9..c0935baa 100644 --- a/src/main/java/frc/robot/commands/SetSuperAutoScorePosition.java +++ b/src/main/java/frc/robot/commands/SetSuperAutoScorePosition.java @@ -7,44 +7,46 @@ import frc.robot.subsystems.limelight.Vision; import frc.robot.subsystems.swervev3.SwerveDrivetrain; import frc.robot.utils.logging.commands.LoggableCommand; - import java.util.Arrays; import java.util.Comparator; public class SetSuperAutoScorePosition extends LoggableCommand { - private final SwerveDrivetrain drivetrain; - private final ElevatorSubsystem elevatorSubsystem; - private final Vision vision; - - public SetSuperAutoScorePosition( - SwerveDrivetrain drivetrain, ElevatorSubsystem elevatorSubsystem, Vision vision) { - this.drivetrain = drivetrain; - this.elevatorSubsystem = elevatorSubsystem; - this.vision = vision; - } + private final SwerveDrivetrain drivetrain; + private final ElevatorSubsystem elevatorSubsystem; + private final Vision vision; - @Override - public void execute() { - AlignmentPosition closest = AlignmentPosition.getClosest(drivetrain.getPose().getTranslation()); - BranchPositions[] relevantPoses = - Arrays.stream(vision.getAllBranchPosition()) - .filter(b -> b.getTrunk() == closest) - .sorted(Comparator.comparingInt(v -> v.getElevatorLevel().getWeight())) - .toArray(BranchPositions[]::new); - if (relevantPoses.length == 0 || relevantPoses[0].getElevatorLevel() != ElevatorPosition.LEVEL4) { - elevatorSubsystem.setElevatorPosition(ElevatorPosition.LEVEL4.getElevatorHeight()); - } else if (relevantPoses.length == 1 || relevantPoses[1].getElevatorLevel() != ElevatorPosition.LEVEL3) { - elevatorSubsystem.setElevatorPosition(ElevatorPosition.LEVEL3.getElevatorHeight()); - } else if (relevantPoses.length == 2 || relevantPoses[2].getElevatorLevel() != ElevatorPosition.LEVEL2) { - elevatorSubsystem.setElevatorPosition(ElevatorPosition.LEVEL3.getElevatorHeight()); - }else if (relevantPoses.length == 3 || relevantPoses[3].getElevatorLevel() != ElevatorPosition.LEVEL1) { - elevatorSubsystem.setElevatorPosition(ElevatorPosition.LEVEL3.getElevatorHeight()); - } + public SetSuperAutoScorePosition( + SwerveDrivetrain drivetrain, ElevatorSubsystem elevatorSubsystem, Vision vision) { + this.drivetrain = drivetrain; + this.elevatorSubsystem = elevatorSubsystem; + this.vision = vision; + } + @Override + public void execute() { + AlignmentPosition closest = AlignmentPosition.getClosest(drivetrain.getPose().getTranslation()); + BranchPositions[] relevantPoses = + Arrays.stream(vision.getAllBranchPosition()) + .filter(b -> b.getTrunk() == closest) + .sorted(Comparator.comparingInt(v -> v.getElevatorLevel().getWeight())) + .toArray(BranchPositions[]::new); + if (relevantPoses.length == 0 + || relevantPoses[0].getElevatorLevel() != ElevatorPosition.LEVEL4) { + elevatorSubsystem.setElevatorPosition(ElevatorPosition.LEVEL4.getElevatorHeight()); + } else if (relevantPoses.length == 1 + || relevantPoses[1].getElevatorLevel() != ElevatorPosition.LEVEL3) { + elevatorSubsystem.setElevatorPosition(ElevatorPosition.LEVEL3.getElevatorHeight()); + } else if (relevantPoses.length == 2 + || relevantPoses[2].getElevatorLevel() != ElevatorPosition.LEVEL2) { + elevatorSubsystem.setElevatorPosition(ElevatorPosition.LEVEL3.getElevatorHeight()); + } else if (relevantPoses.length == 3 + || relevantPoses[3].getElevatorLevel() != ElevatorPosition.LEVEL1) { + elevatorSubsystem.setElevatorPosition(ElevatorPosition.LEVEL3.getElevatorHeight()); } + } - @Override - public boolean isFinished() { - return true; - } + @Override + public boolean isFinished() { + return true; + } } diff --git a/src/main/java/frc/robot/commands/SuperAutoScore.java b/src/main/java/frc/robot/commands/SuperAutoScore.java index 6010b878..251068c9 100644 --- a/src/main/java/frc/robot/commands/SuperAutoScore.java +++ b/src/main/java/frc/robot/commands/SuperAutoScore.java @@ -1,10 +1,16 @@ package frc.robot.commands; +import frc.robot.commands.alignment.AlignClosestBranch; +import frc.robot.commands.coral.ShootCoral; +import frc.robot.commands.elevator.ElevatorToStoredPosition; +import frc.robot.constants.Constants; import frc.robot.subsystems.coral.CoralSubsystem; import frc.robot.subsystems.elevator.ElevatorSubsystem; import frc.robot.subsystems.limelight.Vision; import frc.robot.subsystems.swervev3.SwerveDrivetrain; +import frc.robot.utils.logging.commands.LoggableParallelCommandGroup; import frc.robot.utils.logging.commands.LoggableSequentialCommandGroup; +import frc.robot.utils.logging.commands.LoggableWaitCommand; public class SuperAutoScore extends LoggableSequentialCommandGroup { public SuperAutoScore( @@ -12,6 +18,11 @@ public SuperAutoScore( ElevatorSubsystem elevator, CoralSubsystem coralSubsystem, Vision vision) { - super(new SetSuperAutoScorePosition(drivetrain, elevator, vision)); + super( + new SetSuperAutoScorePosition(drivetrain, elevator, vision), + new LoggableParallelCommandGroup( + new AlignClosestBranch(drivetrain), new ElevatorToStoredPosition(elevator)), + new LoggableWaitCommand(0.2), + new ShootCoral(coralSubsystem, Constants.CORAL_SHOOTER_SPEED)); } } From f13159833ab0581b351de26c9bcf588c427150e0 Mon Sep 17 00:00:00 2001 From: codetoad Date: Wed, 26 Mar 2025 12:25:48 -0400 Subject: [PATCH 93/99] deleted unused --- src/main/java/frc/robot/Robot.java | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 68960c63..caa66636 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -142,8 +142,6 @@ public void disabledExit() {} public void autonomousInit() { mode.set(RobotMode.AUTONOMOUS); new SetInitOdom(robotContainer.getDrivetrain(), robotContainer.getAutoChooser()).schedule(); - new SetBaseVisionStd(robotContainer.getDrivetrain(), VecBuilder.fill(0.45, 0.45, 0.1)) - .schedule(); autoCommand = robotContainer.getAutonomousCommand(); if (autoCommand != null) { autoCommand.schedule(); From c95598211606c0d7d5a5c999b8dfa7f7eed44a66 Mon Sep 17 00:00:00 2001 From: codetoad Date: Wed, 26 Mar 2025 12:27:51 -0400 Subject: [PATCH 94/99] dont know how simMode got changed --- src/main/java/frc/robot/constants/GameConstants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java index 8189e175..43a049dd 100644 --- a/src/main/java/frc/robot/constants/GameConstants.java +++ b/src/main/java/frc/robot/constants/GameConstants.java @@ -95,7 +95,7 @@ public class GameConstants { public static final int HIHI_EXTENDER_TICK_LIMIT = 10; // Mode - public static final Mode simMode = Mode.REAL; + public static final Mode simMode = Mode.SIM; public static final Mode currentMode = RobotBase.isReal() ? Mode.REAL : simMode; public enum Mode { From b1fe92113857ba2a50771d25293676df5bc180ca Mon Sep 17 00:00:00 2001 From: codetoad Date: Wed, 26 Mar 2025 12:32:14 -0400 Subject: [PATCH 95/99] removed unneeded std change --- src/main/java/frc/robot/Robot.java | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index caa66636..ea9e91b9 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -156,8 +156,6 @@ public void autonomousExit() {} @Override public void teleopInit() { - new SetBaseVisionStd(robotContainer.getDrivetrain(), PoseEstimator.visionMeasurementStdDevs2) - .schedule(); mode.set(RobotMode.TELEOP); diagnostics.reset(); if (autoCommand != null) { From c3538103f6baa1340aa4392bc19e85c5b5360f27 Mon Sep 17 00:00:00 2001 From: codetoad Date: Wed, 26 Mar 2025 17:29:13 -0400 Subject: [PATCH 96/99] use return instead of if. Will be usefull when we add algae checks --- src/main/java/frc/robot/Robot.java | 3 --- .../robot/commands/SetSuperAutoScorePosition.java | 12 +++++++++--- 2 files changed, 9 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index ea9e91b9..d545fb16 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -5,18 +5,15 @@ package frc.robot; import com.pathplanner.lib.pathfinding.Pathfinding; -import edu.wpi.first.math.VecBuilder; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import frc.robot.camera.CameraThread; import frc.robot.commands.drivetrain.ResetGyro; -import frc.robot.commands.drivetrain.SetBaseVisionStd; import frc.robot.commands.drivetrain.SetInitOdom; import frc.robot.commands.drivetrain.WheelAlign; import frc.robot.constants.Constants; import frc.robot.constants.GameConstants; -import frc.robot.subsystems.swervev3.estimation.PoseEstimator; import frc.robot.utils.RobotMode; import frc.robot.utils.diag.Diagnostics; import frc.robot.utils.logging.commands.CommandLogger; diff --git a/src/main/java/frc/robot/commands/SetSuperAutoScorePosition.java b/src/main/java/frc/robot/commands/SetSuperAutoScorePosition.java index c0935baa..33c1a17c 100644 --- a/src/main/java/frc/robot/commands/SetSuperAutoScorePosition.java +++ b/src/main/java/frc/robot/commands/SetSuperAutoScorePosition.java @@ -33,13 +33,19 @@ public void execute() { if (relevantPoses.length == 0 || relevantPoses[0].getElevatorLevel() != ElevatorPosition.LEVEL4) { elevatorSubsystem.setElevatorPosition(ElevatorPosition.LEVEL4.getElevatorHeight()); - } else if (relevantPoses.length == 1 + return; + } + if (relevantPoses.length == 1 || relevantPoses[1].getElevatorLevel() != ElevatorPosition.LEVEL3) { elevatorSubsystem.setElevatorPosition(ElevatorPosition.LEVEL3.getElevatorHeight()); - } else if (relevantPoses.length == 2 + return; + } + if (relevantPoses.length == 2 || relevantPoses[2].getElevatorLevel() != ElevatorPosition.LEVEL2) { elevatorSubsystem.setElevatorPosition(ElevatorPosition.LEVEL3.getElevatorHeight()); - } else if (relevantPoses.length == 3 + return; + } + if (relevantPoses.length == 3 || relevantPoses[3].getElevatorLevel() != ElevatorPosition.LEVEL1) { elevatorSubsystem.setElevatorPosition(ElevatorPosition.LEVEL3.getElevatorHeight()); } From ebc8de6f5405c41e5e5b70bc37f302b72939ebee Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Wed, 26 Mar 2025 19:24:42 -0400 Subject: [PATCH 97/99] added some small stuff --- .../robot/commands/ChangeLimelightLight.java | 23 +++++++++++++++++++ .../subsystems/limelight/RealVisionIO.java | 3 +++ .../robot/subsystems/limelight/VisionIO.java | 4 +++- 3 files changed, 29 insertions(+), 1 deletion(-) create mode 100644 src/main/java/frc/robot/commands/ChangeLimelightLight.java diff --git a/src/main/java/frc/robot/commands/ChangeLimelightLight.java b/src/main/java/frc/robot/commands/ChangeLimelightLight.java new file mode 100644 index 00000000..cbbda2d1 --- /dev/null +++ b/src/main/java/frc/robot/commands/ChangeLimelightLight.java @@ -0,0 +1,23 @@ +package frc.robot.commands; + +import frc.robot.subsystems.limelight.RealVisionIO; +import frc.robot.subsystems.limelight.VisionIO; +import frc.robot.utils.logging.commands.LoggableCommand; + +public class ChangeLimelightLight extends LoggableCommand { + private final VisionIO io; + private boolean on; + public ChangeLimelightLight(boolean on, VisionIO io) { + this.io = io; + } + + @Override + public void initialize() { + io.setLedMode(on ? 3 : 1); + } + + @Override + public boolean isFinished() { + return true; + } +} diff --git a/src/main/java/frc/robot/subsystems/limelight/RealVisionIO.java b/src/main/java/frc/robot/subsystems/limelight/RealVisionIO.java index 134c691a..8706219c 100644 --- a/src/main/java/frc/robot/subsystems/limelight/RealVisionIO.java +++ b/src/main/java/frc/robot/subsystems/limelight/RealVisionIO.java @@ -52,4 +52,7 @@ public void updateInputs(VisionInputs inputs) { inputs.valid = results.valid; inputs.ledMode = (int) ledModeEntry.getInteger(-1); } + public void setLedMode(int mode) { + ledModeEntry.setNumber(mode); + } } diff --git a/src/main/java/frc/robot/subsystems/limelight/VisionIO.java b/src/main/java/frc/robot/subsystems/limelight/VisionIO.java index 990d52a4..e771da56 100644 --- a/src/main/java/frc/robot/subsystems/limelight/VisionIO.java +++ b/src/main/java/frc/robot/subsystems/limelight/VisionIO.java @@ -2,4 +2,6 @@ import frc.robot.utils.logging.LoggableIO; -public interface VisionIO extends LoggableIO {} +public interface VisionIO extends LoggableIO { + void setLedMode(int mode); +} From b83fa7ddf9aed11ab1460e6656de2471293eedb1 Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Wed, 26 Mar 2025 19:25:41 -0400 Subject: [PATCH 98/99] added some small stuff --- .../robot/commands/ChangeLimelightLight.java | 28 +++++++++---------- .../subsystems/limelight/MockVisionIO.java | 3 ++ .../subsystems/limelight/RealVisionIO.java | 1 + .../robot/subsystems/limelight/VisionIO.java | 2 +- 4 files changed, 19 insertions(+), 15 deletions(-) diff --git a/src/main/java/frc/robot/commands/ChangeLimelightLight.java b/src/main/java/frc/robot/commands/ChangeLimelightLight.java index cbbda2d1..cf9cdc53 100644 --- a/src/main/java/frc/robot/commands/ChangeLimelightLight.java +++ b/src/main/java/frc/robot/commands/ChangeLimelightLight.java @@ -1,23 +1,23 @@ package frc.robot.commands; -import frc.robot.subsystems.limelight.RealVisionIO; import frc.robot.subsystems.limelight.VisionIO; import frc.robot.utils.logging.commands.LoggableCommand; public class ChangeLimelightLight extends LoggableCommand { - private final VisionIO io; - private boolean on; - public ChangeLimelightLight(boolean on, VisionIO io) { - this.io = io; - } + private final VisionIO io; + private boolean on; - @Override - public void initialize() { - io.setLedMode(on ? 3 : 1); - } + public ChangeLimelightLight(boolean on, VisionIO io) { + this.io = io; + } - @Override - public boolean isFinished() { - return true; - } + @Override + public void initialize() { + io.setLedMode(on ? 3 : 1); + } + + @Override + public boolean isFinished() { + return true; + } } diff --git a/src/main/java/frc/robot/subsystems/limelight/MockVisionIO.java b/src/main/java/frc/robot/subsystems/limelight/MockVisionIO.java index 634f7193..57c2eaf4 100644 --- a/src/main/java/frc/robot/subsystems/limelight/MockVisionIO.java +++ b/src/main/java/frc/robot/subsystems/limelight/MockVisionIO.java @@ -3,4 +3,7 @@ public class MockVisionIO implements VisionIO { @Override public void updateInputs(VisionInputs inputs) {} + + @Override + public void setLedMode(int mode) {} } diff --git a/src/main/java/frc/robot/subsystems/limelight/RealVisionIO.java b/src/main/java/frc/robot/subsystems/limelight/RealVisionIO.java index 8706219c..a92fbbcc 100644 --- a/src/main/java/frc/robot/subsystems/limelight/RealVisionIO.java +++ b/src/main/java/frc/robot/subsystems/limelight/RealVisionIO.java @@ -52,6 +52,7 @@ public void updateInputs(VisionInputs inputs) { inputs.valid = results.valid; inputs.ledMode = (int) ledModeEntry.getInteger(-1); } + public void setLedMode(int mode) { ledModeEntry.setNumber(mode); } diff --git a/src/main/java/frc/robot/subsystems/limelight/VisionIO.java b/src/main/java/frc/robot/subsystems/limelight/VisionIO.java index e771da56..9582f551 100644 --- a/src/main/java/frc/robot/subsystems/limelight/VisionIO.java +++ b/src/main/java/frc/robot/subsystems/limelight/VisionIO.java @@ -3,5 +3,5 @@ import frc.robot.utils.logging.LoggableIO; public interface VisionIO extends LoggableIO { - void setLedMode(int mode); + void setLedMode(int mode); } From 57c134cc9a6f3c4f9b1fd3efd26dfc6523ac3e7b Mon Sep 17 00:00:00 2001 From: codetoad Date: Wed, 26 Mar 2025 23:52:39 -0400 Subject: [PATCH 99/99] removed merge artifact --- src/main/java/frc/robot/subsystems/limelight/Vision.java | 3 --- 1 file changed, 3 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/limelight/Vision.java b/src/main/java/frc/robot/subsystems/limelight/Vision.java index d23d4ffd..7cfbe916 100644 --- a/src/main/java/frc/robot/subsystems/limelight/Vision.java +++ b/src/main/java/frc/robot/subsystems/limelight/Vision.java @@ -13,7 +13,6 @@ import frc.robot.utils.logging.subsystem.LoggableSystem; import java.util.ArrayList; import java.util.function.Supplier; -import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; public class Vision extends SubsystemBase { @@ -45,12 +44,10 @@ public boolean isPieceSeen() { return system.getInputs().valid; } - @AutoLogOutput public BranchPositions[] getAllBranchPosition() { return currentCoralPositions.toArray(BranchPositions[]::new); } - @AutoLogOutput public AlgaePositions[] getAllAlgaePosition() { return currentAlgaePosition.toArray(AlgaePositions[]::new); }