From 69602ceb1640e4c1fff5c5e50b3445aeb511e28a Mon Sep 17 00:00:00 2001 From: YHerm <0549405422.yh@gmail.com> Date: Wed, 2 Apr 2025 16:20:34 +0300 Subject: [PATCH 1/2] started --- src/main/java/frc/JoysticksBindings.java | 24 +++++++------- src/main/java/frc/robot/Robot.java | 33 +++++++++++++++---- .../DoubleJointedArmVisualizer.java | 5 +-- .../robot/visualizers/FollowPathCommand.java | 17 +++++----- 4 files changed, 52 insertions(+), 27 deletions(-) diff --git a/src/main/java/frc/JoysticksBindings.java b/src/main/java/frc/JoysticksBindings.java index 9fbf98459c..3a4f7eacf3 100644 --- a/src/main/java/frc/JoysticksBindings.java +++ b/src/main/java/frc/JoysticksBindings.java @@ -37,25 +37,27 @@ private static void mainJoystickButtons(Robot robot) { // bindings... // robot.getArm().applyTestBinds(usedJoystick); usedJoystick.A.onTrue( - new InstantCommand(() -> robot.getArmVisualizer().showPath(Robot.pathDown)).andThen(new WaitCommand(0.3)) + new InstantCommand(() -> robot.getArmVisualizer().showPath(Robot.pathDown.getStates())).andThen(new WaitCommand(0.3)) .andThen(new FollowPathCommand(Robot.pathDown, p -> robot.getArm().setPosition(p, true))) ); usedJoystick.B.onTrue( - new InstantCommand(() -> robot.getArmVisualizer().showPath(Robot.pathLeft)).andThen(new WaitCommand(0.3)) + new InstantCommand(() -> robot.getArmVisualizer().showPath(Robot.pathLeft.getStates())).andThen(new WaitCommand(0.3)) .andThen(new FollowPathCommand(Robot.pathLeft, p -> robot.getArm().setPosition(p, false))) ); usedJoystick.Y.onTrue( - new InstantCommand(() -> robot.getArmVisualizer().showPath(Robot.pathUp)).andThen(new WaitCommand(0.3)) - .andThen(new FollowPathCommand(Robot.pathUp, p -> robot.getArm().setPosition(p, false))) + new InstantCommand(() -> robot.getArmVisualizer().showPath(Robot.pathUp.getStates())).andThen(new WaitCommand(0.3)) + .andThen( + new FollowPathCommand(Robot.pathUp, p -> robot.getArm().setPosition(p, false)) + ) ); - List> flipArm = PathGenerator - .flipArm(Robot.pathLeft.get(Robot.pathLeft.size() - 1), false, DoubleJointedArm.TOTAL_LENGTH_METERS); - usedJoystick.X.onTrue( - new InstantCommand(() -> robot.getArmVisualizer().showPath(PathGenerator.createTranslation2dList(flipArm))) - .andThen(new WaitCommand(0.3)) - .andThen(new FollowPathWithSideCommand(flipArm, p -> robot.getArm().setPosition(p.getFirst(), p.getSecond()))) - ); +// List> flipArm = PathGenerator +// .flipArm(Robot.pathLeft.get(Robot.pathLeft.size() - 1), false, DoubleJointedArm.TOTAL_LENGTH_METERS); +// usedJoystick.X.onTrue( +// new InstantCommand(() -> robot.getArmVisualizer().showPath(PathGenerator.createTranslation2dList(flipArm))) +// .andThen(new WaitCommand(0.3)) +// .andThen(new FollowPathWithSideCommand(flipArm, p -> robot.getArm().setPosition(p.getFirst(), p.getSecond()))) +// ); } private static void secondJoystickButtons(Robot robot) { diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index ba7bb6da02..edfad809e2 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -4,7 +4,12 @@ package frc.robot; +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.math.trajectory.Trajectory; +import edu.wpi.first.math.trajectory.TrajectoryConfig; +import edu.wpi.first.math.trajectory.TrajectoryGenerator; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.InstantCommand; @@ -35,16 +40,32 @@ public class Robot { DoubleJointedArm.SECOND_JOINT_LENGTH_METERS ); - public static List pathUp; - public static List pathLeft; - public static List pathDown; + public static Trajectory pathUp; + public static Trajectory pathLeft; + public static Trajectory pathDown; public Robot() { BatteryUtil.scheduleLimiter(); - pathUp = PathGenerator.straightLine(new Translation2d(-0.4, 0.05), new Translation2d(-0.4, 1)); - pathLeft = PathGenerator.straightLine(new Translation2d(-0.4, 1), new Translation2d(0.4, 1)); - pathDown = PathGenerator.straightLine(new Translation2d(0.4, 1), new Translation2d(0.4, 0.05)); + pathUp = TrajectoryGenerator.generateTrajectory( + new Pose2d(new Translation2d(-0.4, 0.05), Rotation2d.fromDegrees(90)), + List.of(), + new Pose2d(new Translation2d(-0.4, 1), Rotation2d.fromDegrees(90)), + new TrajectoryConfig(2,2).setStartVelocity(0).setEndVelocity(0) + ); + + pathLeft = TrajectoryGenerator.generateTrajectory( + new Pose2d(new Translation2d(-0.4, 1), Rotation2d.fromDegrees(0)), + List.of(), + new Pose2d(new Translation2d(0.4, 1), Rotation2d.fromDegrees(0)), + new TrajectoryConfig(2,2).setStartVelocity(0).setEndVelocity(0) + ); + pathDown =TrajectoryGenerator.generateTrajectory( + new Pose2d(new Translation2d(0.4, 1), Rotation2d.fromDegrees(-90)), + List.of(), + new Pose2d(new Translation2d(0.4, 0.05), Rotation2d.fromDegrees(-90)), + new TrajectoryConfig(2,2).setStartVelocity(0).setEndVelocity(0) + ); } public void periodic() { diff --git a/src/main/java/frc/robot/visualizers/DoubleJointedArmVisualizer.java b/src/main/java/frc/robot/visualizers/DoubleJointedArmVisualizer.java index d9f05d22ff..d2053b2ade 100644 --- a/src/main/java/frc/robot/visualizers/DoubleJointedArmVisualizer.java +++ b/src/main/java/frc/robot/visualizers/DoubleJointedArmVisualizer.java @@ -2,6 +2,7 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.trajectory.Trajectory; import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d; @@ -63,11 +64,11 @@ public void showTargetPosition(boolean show) { TARGET_POSITION_MARK.setLineWeight(show ? DEFAULT_LINE_WIDTH : 0); } - public void showPath(List path) { + public void showPath(List path) { ArrayList pathVisualize = new ArrayList<>(path.size()); for (int i = 0; i < path.size(); i++) { MechanismRoot2d root = mechanism - .getRoot(paths.size() + ", " + i, path.get(i).getX() + armRootPosition.getX(), path.get(i).getY() + armRootPosition.getY()); + .getRoot(paths.size() + ", " + i, path.get(i).poseMeters.getX() + armRootPosition.getX(), path.get(i).poseMeters.getY() + armRootPosition.getY()); MechanismLigament2d mark = new MechanismLigament2d( paths.size() + ", " + i + " mark", 0.01, diff --git a/src/main/java/frc/robot/visualizers/FollowPathCommand.java b/src/main/java/frc/robot/visualizers/FollowPathCommand.java index f323ed589d..af38f90aae 100644 --- a/src/main/java/frc/robot/visualizers/FollowPathCommand.java +++ b/src/main/java/frc/robot/visualizers/FollowPathCommand.java @@ -1,7 +1,9 @@ package frc.robot.visualizers; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.trajectory.Trajectory; import edu.wpi.first.wpilibj2.command.Command; +import frc.utils.time.TimeUtil; import java.util.List; import java.util.function.Consumer; @@ -9,29 +11,28 @@ public class FollowPathCommand extends Command { private final Consumer setPosition; - private final List path; + private final Trajectory path; - private int currentPoint = 0; + private double startTime = 0; - public FollowPathCommand(List path, Consumer setPosition) { - this.path = path; + public FollowPathCommand(Trajectory trajectory, Consumer setPosition) { + this.path = trajectory; this.setPosition = setPosition; } @Override public void initialize() { - currentPoint = 0; + startTime = TimeUtil.getCurrentTimeSeconds(); } @Override public void execute() { - setPosition.accept(path.get(currentPoint)); - currentPoint++; + setPosition.accept(path.sample(TimeUtil.getCurrentTimeSeconds() - startTime).poseMeters.getTranslation()); } @Override public boolean isFinished() { - return currentPoint >= path.size(); + return TimeUtil.getCurrentTimeSeconds() - startTime >= path.getTotalTimeSeconds(); } } From 9f1aae06c5ffe0f4edaf2ed8d99228791770d793 Mon Sep 17 00:00:00 2001 From: YHerm <0549405422.yh@gmail.com> Date: Thu, 3 Apr 2025 09:50:17 +0300 Subject: [PATCH 2/2] format --- src/main/java/frc/JoysticksBindings.java | 10 +--------- src/main/java/frc/robot/Robot.java | 9 ++++----- .../robot/visualizers/DoubleJointedArmVisualizer.java | 7 +++++-- .../java/frc/robot/visualizers/FollowPathCommand.java | 1 - 4 files changed, 10 insertions(+), 17 deletions(-) diff --git a/src/main/java/frc/JoysticksBindings.java b/src/main/java/frc/JoysticksBindings.java index 3a4f7eacf3..77408e0d7d 100644 --- a/src/main/java/frc/JoysticksBindings.java +++ b/src/main/java/frc/JoysticksBindings.java @@ -1,18 +1,12 @@ package frc; -import edu.wpi.first.math.Pair; -import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.WaitCommand; import frc.joysticks.JoystickPorts; import frc.joysticks.SmartJoystick; import frc.robot.Robot; -import frc.robot.subsystems.DoubleJointedArm; import frc.robot.visualizers.FollowPathCommand; -import frc.robot.visualizers.FollowPathWithSideCommand; -import frc.robot.visualizers.PathGenerator; -import java.util.List; public class JoysticksBindings { @@ -46,9 +40,7 @@ private static void mainJoystickButtons(Robot robot) { ); usedJoystick.Y.onTrue( new InstantCommand(() -> robot.getArmVisualizer().showPath(Robot.pathUp.getStates())).andThen(new WaitCommand(0.3)) - .andThen( - new FollowPathCommand(Robot.pathUp, p -> robot.getArm().setPosition(p, false)) - ) + .andThen(new FollowPathCommand(Robot.pathUp, p -> robot.getArm().setPosition(p, false))) ); // List> flipArm = PathGenerator diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index edfad809e2..4272051a71 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -17,7 +17,6 @@ import frc.robot.hardware.phoenix6.BusChain; import frc.robot.subsystems.DoubleJointedArm; import frc.robot.visualizers.DoubleJointedArmVisualizer; -import frc.robot.visualizers.PathGenerator; import frc.utils.battery.BatteryUtil; import java.util.List; @@ -51,20 +50,20 @@ public Robot() { new Pose2d(new Translation2d(-0.4, 0.05), Rotation2d.fromDegrees(90)), List.of(), new Pose2d(new Translation2d(-0.4, 1), Rotation2d.fromDegrees(90)), - new TrajectoryConfig(2,2).setStartVelocity(0).setEndVelocity(0) + new TrajectoryConfig(2, 2).setStartVelocity(0).setEndVelocity(0) ); pathLeft = TrajectoryGenerator.generateTrajectory( new Pose2d(new Translation2d(-0.4, 1), Rotation2d.fromDegrees(0)), List.of(), new Pose2d(new Translation2d(0.4, 1), Rotation2d.fromDegrees(0)), - new TrajectoryConfig(2,2).setStartVelocity(0).setEndVelocity(0) + new TrajectoryConfig(2, 2).setStartVelocity(0).setEndVelocity(0) ); - pathDown =TrajectoryGenerator.generateTrajectory( + pathDown = TrajectoryGenerator.generateTrajectory( new Pose2d(new Translation2d(0.4, 1), Rotation2d.fromDegrees(-90)), List.of(), new Pose2d(new Translation2d(0.4, 0.05), Rotation2d.fromDegrees(-90)), - new TrajectoryConfig(2,2).setStartVelocity(0).setEndVelocity(0) + new TrajectoryConfig(2, 2).setStartVelocity(0).setEndVelocity(0) ); } diff --git a/src/main/java/frc/robot/visualizers/DoubleJointedArmVisualizer.java b/src/main/java/frc/robot/visualizers/DoubleJointedArmVisualizer.java index d2053b2ade..fa040bab44 100644 --- a/src/main/java/frc/robot/visualizers/DoubleJointedArmVisualizer.java +++ b/src/main/java/frc/robot/visualizers/DoubleJointedArmVisualizer.java @@ -67,8 +67,11 @@ public void showTargetPosition(boolean show) { public void showPath(List path) { ArrayList pathVisualize = new ArrayList<>(path.size()); for (int i = 0; i < path.size(); i++) { - MechanismRoot2d root = mechanism - .getRoot(paths.size() + ", " + i, path.get(i).poseMeters.getX() + armRootPosition.getX(), path.get(i).poseMeters.getY() + armRootPosition.getY()); + MechanismRoot2d root = mechanism.getRoot( + paths.size() + ", " + i, + path.get(i).poseMeters.getX() + armRootPosition.getX(), + path.get(i).poseMeters.getY() + armRootPosition.getY() + ); MechanismLigament2d mark = new MechanismLigament2d( paths.size() + ", " + i + " mark", 0.01, diff --git a/src/main/java/frc/robot/visualizers/FollowPathCommand.java b/src/main/java/frc/robot/visualizers/FollowPathCommand.java index af38f90aae..c3085b73c2 100644 --- a/src/main/java/frc/robot/visualizers/FollowPathCommand.java +++ b/src/main/java/frc/robot/visualizers/FollowPathCommand.java @@ -5,7 +5,6 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.utils.time.TimeUtil; -import java.util.List; import java.util.function.Consumer; public class FollowPathCommand extends Command {