From f48901b706b125ef3ae6d446bc921e1da70e18f6 Mon Sep 17 00:00:00 2001 From: YHerm <0549405422.yh@gmail.com> Date: Wed, 2 Apr 2025 12:06:22 +0300 Subject: [PATCH 1/5] looks nice --- src/main/java/frc/JoysticksBindings.java | 4 +- src/main/java/frc/robot/Robot.java | 29 ++++++++++++ .../DoubleJointedArmVisualizer.java | 27 ++++++++++++ .../robot/visualizers/FollowPathCommand.java | 44 +++++++++++++++++++ 4 files changed, 103 insertions(+), 1 deletion(-) create mode 100644 src/main/java/frc/robot/visualizers/FollowPathCommand.java diff --git a/src/main/java/frc/JoysticksBindings.java b/src/main/java/frc/JoysticksBindings.java index 1c2f5ceeee..860f881a58 100644 --- a/src/main/java/frc/JoysticksBindings.java +++ b/src/main/java/frc/JoysticksBindings.java @@ -3,6 +3,7 @@ import frc.joysticks.JoystickPorts; import frc.joysticks.SmartJoystick; import frc.robot.Robot; +import frc.robot.visualizers.FollowPathCommand; public class JoysticksBindings { @@ -25,7 +26,8 @@ public static void configureBindings(Robot robot) { private static void mainJoystickButtons(Robot robot) { SmartJoystick usedJoystick = MAIN_JOYSTICK; // bindings... - robot.getArm().applyTestBinds(usedJoystick); +// robot.getArm().applyTestBinds(usedJoystick); + usedJoystick.A.onTrue(new FollowPathCommand(Robot.points, p -> robot.getArm().setPosition(p, false))); } 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 c7c909758e..c2477f187e 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -4,6 +4,7 @@ package frc.robot; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.InstantCommand; @@ -13,6 +14,8 @@ import frc.robot.visualizers.DoubleJointedArmVisualizer; import frc.utils.battery.BatteryUtil; +import java.util.ArrayList; + /** * This class is where the bulk of the robot should be declared. Since Command-based is a "declarative" paradigm, very little robot logic should * actually be handled in the {@link RobotManager} periodic methods (other than the scheduler calls). Instead, the structure of the robot @@ -31,8 +34,34 @@ public class Robot { DoubleJointedArm.SECOND_JOINT_LENGTH_METERS ); + public static ArrayList points; + public Robot() { BatteryUtil.scheduleLimiter(); + + points = new ArrayList<>(); + + // First segment: Move up along the Y-axis from (-0.3, 0.2) to (-0.3, 0.8) + double startY = 0.2; + double endY = 1; + double x1 = -0.4; + double step = 0.05; + for (double y = startY; y <= endY; y += step) { + points.add(new Translation2d(x1, y)); + } + + // Second segment: Move right along the X-axis from (-0.3, 0.8) to (0.3, 0.8) + double x2 = 0.4; + for (double x = x1 + step; x <= x2; x += step) { + points.add(new Translation2d(x, endY)); + } + + // Third segment: Move down along the Y-axis from (0.3, 0.8) to (0.3, 0.2) + for (double y = endY - step; y >= startY; y -= step) { + points.add(new Translation2d(x2, y)); + } + + armVisualizer.showPath(points); } public void periodic() { diff --git a/src/main/java/frc/robot/visualizers/DoubleJointedArmVisualizer.java b/src/main/java/frc/robot/visualizers/DoubleJointedArmVisualizer.java index f594cf8c93..184adf7990 100644 --- a/src/main/java/frc/robot/visualizers/DoubleJointedArmVisualizer.java +++ b/src/main/java/frc/robot/visualizers/DoubleJointedArmVisualizer.java @@ -9,6 +9,9 @@ import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj.util.Color8Bit; +import java.util.ArrayList; +import java.util.List; + public class DoubleJointedArmVisualizer { private static final MechanismLigament2d TARGET_POSITION_MARK = new MechanismLigament2d("mark", 0.01, 0, 10.0F, new Color8Bit(Color.kGreen)); @@ -21,6 +24,7 @@ public class DoubleJointedArmVisualizer { private final MechanismLigament2d firstJoint; private final MechanismLigament2d secondJoint; private final MechanismRoot2d targetPosition; + private final ArrayList> paths = new ArrayList<>(); public DoubleJointedArmVisualizer( String name, @@ -59,6 +63,29 @@ public void showTargetPosition(boolean show) { TARGET_POSITION_MARK.setLength(show ? 0.01 : 0); } + 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()); + MechanismLigament2d mark = new MechanismLigament2d( + paths.size() + ", " + i + " mark", + 0.01, + 0, + 10.0F, + new Color8Bit(Color.kBlueViolet) + ); + pathVisualize.add(mark); + root.append(mark); + } + if (!paths.isEmpty()) { + for (int i = 0; i < paths.get(paths.size() - 1).size(); i++) { + paths.get(paths.size() - 1).get(i).setLength(0); + } + } + paths.add(pathVisualize); + } + private static Rotation2d toFloorRelative(Rotation2d floorRelativeAngle, Rotation2d jointRelativeAngle) { return jointRelativeAngle.minus(floorRelativeAngle); } diff --git a/src/main/java/frc/robot/visualizers/FollowPathCommand.java b/src/main/java/frc/robot/visualizers/FollowPathCommand.java new file mode 100644 index 0000000000..6deb2bc106 --- /dev/null +++ b/src/main/java/frc/robot/visualizers/FollowPathCommand.java @@ -0,0 +1,44 @@ +package frc.robot.visualizers; + +import edu.wpi.first.math.geometry.Translation2d; +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 { + + private final double delay = 0.2; + private final Consumer setPosition; + private final List path; + + private double lastSetTime = 0; + private int currentPoint = 0; + + public FollowPathCommand(List path, Consumer setPosition) { + this.path = path; + this.setPosition = setPosition; + } + + @Override + public void initialize() { + currentPoint = 0; + lastSetTime = 0; + } + + @Override + public void execute() { + if (TimeUtil.getCurrentTimeSeconds() - lastSetTime > delay) { + lastSetTime = TimeUtil.getCurrentTimeSeconds(); + setPosition.accept(path.get(currentPoint)); + currentPoint++; + } + } + + @Override + public boolean isFinished() { + return currentPoint >= path.size(); + } + +} From 39d3c649c334dea92fe2cda1ee30f7f948009b36 Mon Sep 17 00:00:00 2001 From: YHerm <0549405422.yh@gmail.com> Date: Wed, 2 Apr 2025 12:30:42 +0300 Subject: [PATCH 2/5] workingggg --- src/main/java/frc/JoysticksBindings.java | 14 ++++- src/main/java/frc/robot/Robot.java | 37 ++++------- .../DoubleJointedArmVisualizer.java | 4 +- .../robot/visualizers/FollowPathCommand.java | 62 +++++++++---------- .../frc/robot/visualizers/PathGenerator.java | 29 +++++++++ 5 files changed, 87 insertions(+), 59 deletions(-) create mode 100644 src/main/java/frc/robot/visualizers/PathGenerator.java diff --git a/src/main/java/frc/JoysticksBindings.java b/src/main/java/frc/JoysticksBindings.java index 860f881a58..7ee174a6cf 100644 --- a/src/main/java/frc/JoysticksBindings.java +++ b/src/main/java/frc/JoysticksBindings.java @@ -1,5 +1,6 @@ package frc; +import edu.wpi.first.wpilibj2.command.InstantCommand; import frc.joysticks.JoystickPorts; import frc.joysticks.SmartJoystick; import frc.robot.Robot; @@ -27,7 +28,18 @@ private static void mainJoystickButtons(Robot robot) { SmartJoystick usedJoystick = MAIN_JOYSTICK; // bindings... // robot.getArm().applyTestBinds(usedJoystick); - usedJoystick.A.onTrue(new FollowPathCommand(Robot.points, p -> robot.getArm().setPosition(p, false))); + usedJoystick.A.onTrue( + new InstantCommand(() -> robot.getArmVisualizer().showPath(Robot.pathDown)) + .andThen(new FollowPathCommand(Robot.pathDown, p -> robot.getArm().setPosition(p, false))) + ); + usedJoystick.B.onTrue( + new InstantCommand(() -> robot.getArmVisualizer().showPath(Robot.pathLeft)) + .andThen(new FollowPathCommand(Robot.pathLeft, p -> robot.getArm().setPosition(p, false))) + ); + usedJoystick.Y.onTrue( + new InstantCommand(() -> robot.getArmVisualizer().showPath(Robot.pathUp)) + .andThen(new FollowPathCommand(Robot.pathUp, p -> robot.getArm().setPosition(p, false))) + ); } 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 c2477f187e..a3bb76ba53 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -12,9 +12,10 @@ 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.ArrayList; +import java.util.List; /** * This class is where the bulk of the robot should be declared. Since Command-based is a "declarative" paradigm, very little robot logic should @@ -34,34 +35,16 @@ public class Robot { DoubleJointedArm.SECOND_JOINT_LENGTH_METERS ); - public static ArrayList points; + public static List pathUp; + public static List pathLeft; + public static List pathDown; public Robot() { BatteryUtil.scheduleLimiter(); - points = new ArrayList<>(); - - // First segment: Move up along the Y-axis from (-0.3, 0.2) to (-0.3, 0.8) - double startY = 0.2; - double endY = 1; - double x1 = -0.4; - double step = 0.05; - for (double y = startY; y <= endY; y += step) { - points.add(new Translation2d(x1, y)); - } - - // Second segment: Move right along the X-axis from (-0.3, 0.8) to (0.3, 0.8) - double x2 = 0.4; - for (double x = x1 + step; x <= x2; x += step) { - points.add(new Translation2d(x, endY)); - } - - // Third segment: Move down along the Y-axis from (0.3, 0.8) to (0.3, 0.2) - for (double y = endY - step; y >= startY; y -= step) { - points.add(new Translation2d(x2, y)); - } - - armVisualizer.showPath(points); + pathUp = PathGenerator.straightLine(new Translation2d(-0.4, 0.05), new Translation2d(-0.4, 1), 8); + pathLeft = PathGenerator.straightLine(new Translation2d(-0.4, 1), new Translation2d(0.4, 1), 8); + pathDown = PathGenerator.straightLine(new Translation2d(0.4, 1), new Translation2d(0.4, 0.05), 8); } public void periodic() { @@ -82,4 +65,8 @@ public DoubleJointedArm getArm() { return arm; } + public DoubleJointedArmVisualizer getArmVisualizer() { + return armVisualizer; + } + } diff --git a/src/main/java/frc/robot/visualizers/DoubleJointedArmVisualizer.java b/src/main/java/frc/robot/visualizers/DoubleJointedArmVisualizer.java index 184adf7990..d9f05d22ff 100644 --- a/src/main/java/frc/robot/visualizers/DoubleJointedArmVisualizer.java +++ b/src/main/java/frc/robot/visualizers/DoubleJointedArmVisualizer.java @@ -60,7 +60,7 @@ public void setTargetPositionMeters(Translation2d positionMeters) { } public void showTargetPosition(boolean show) { - TARGET_POSITION_MARK.setLength(show ? 0.01 : 0); + TARGET_POSITION_MARK.setLineWeight(show ? DEFAULT_LINE_WIDTH : 0); } public void showPath(List path) { @@ -80,7 +80,7 @@ public void showPath(List path) { } if (!paths.isEmpty()) { for (int i = 0; i < paths.get(paths.size() - 1).size(); i++) { - paths.get(paths.size() - 1).get(i).setLength(0); + paths.get(paths.size() - 1).get(i).setLineWeight(0); } } paths.add(pathVisualize); diff --git a/src/main/java/frc/robot/visualizers/FollowPathCommand.java b/src/main/java/frc/robot/visualizers/FollowPathCommand.java index 6deb2bc106..e06403e61a 100644 --- a/src/main/java/frc/robot/visualizers/FollowPathCommand.java +++ b/src/main/java/frc/robot/visualizers/FollowPathCommand.java @@ -9,36 +9,36 @@ public class FollowPathCommand extends Command { - private final double delay = 0.2; - private final Consumer setPosition; - private final List path; - - private double lastSetTime = 0; - private int currentPoint = 0; - - public FollowPathCommand(List path, Consumer setPosition) { - this.path = path; - this.setPosition = setPosition; - } - - @Override - public void initialize() { - currentPoint = 0; - lastSetTime = 0; - } - - @Override - public void execute() { - if (TimeUtil.getCurrentTimeSeconds() - lastSetTime > delay) { - lastSetTime = TimeUtil.getCurrentTimeSeconds(); - setPosition.accept(path.get(currentPoint)); - currentPoint++; - } - } - - @Override - public boolean isFinished() { - return currentPoint >= path.size(); - } + private final double DELAY_SECONDS = 0.2; + private final Consumer setPosition; + private final List path; + + private double lastSetTime = 0; + private int currentPoint = 0; + + public FollowPathCommand(List path, Consumer setPosition) { + this.path = path; + this.setPosition = setPosition; + } + + @Override + public void initialize() { + currentPoint = 0; + lastSetTime = 0; + } + + @Override + public void execute() { + if (TimeUtil.getCurrentTimeSeconds() - lastSetTime > DELAY_SECONDS) { + lastSetTime = TimeUtil.getCurrentTimeSeconds(); + setPosition.accept(path.get(currentPoint)); + currentPoint++; + } + } + + @Override + public boolean isFinished() { + return currentPoint >= path.size(); + } } diff --git a/src/main/java/frc/robot/visualizers/PathGenerator.java b/src/main/java/frc/robot/visualizers/PathGenerator.java new file mode 100644 index 0000000000..d091487933 --- /dev/null +++ b/src/main/java/frc/robot/visualizers/PathGenerator.java @@ -0,0 +1,29 @@ +package frc.robot.visualizers; + +import edu.wpi.first.math.geometry.Translation2d; + +import java.util.ArrayList; +import java.util.List; + +public class PathGenerator { + + + public static List straightLine(Translation2d start, Translation2d end, int steps) { + List points = new ArrayList<>(); + + double deltaX = end.getX() - start.getX(); + double deltaY = end.getY() - start.getY(); + + double stepX = deltaX / (steps - 1); + double stepY = deltaY / (steps - 1); + + for (int i = 0; i < steps; i++) { + double x = start.getX() + i * stepX; + double y = start.getY() + i * stepY; + points.add(new Translation2d(x, y)); + } + + return points; + } + +} From f77f0a7061a61abd5ac5d8e549c243615c9bd067 Mon Sep 17 00:00:00 2001 From: YHerm <0549405422.yh@gmail.com> Date: Wed, 2 Apr 2025 12:44:06 +0300 Subject: [PATCH 3/5] paths --- src/main/java/frc/JoysticksBindings.java | 7 ++++--- src/main/java/frc/robot/Robot.java | 6 +++--- .../java/frc/robot/visualizers/FollowPathCommand.java | 10 ++-------- src/main/java/frc/robot/visualizers/PathGenerator.java | 3 ++- 4 files changed, 11 insertions(+), 15 deletions(-) diff --git a/src/main/java/frc/JoysticksBindings.java b/src/main/java/frc/JoysticksBindings.java index 7ee174a6cf..10fa95548b 100644 --- a/src/main/java/frc/JoysticksBindings.java +++ b/src/main/java/frc/JoysticksBindings.java @@ -1,6 +1,7 @@ package frc; 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; @@ -29,15 +30,15 @@ private static void mainJoystickButtons(Robot robot) { // bindings... // robot.getArm().applyTestBinds(usedJoystick); usedJoystick.A.onTrue( - new InstantCommand(() -> robot.getArmVisualizer().showPath(Robot.pathDown)) + new InstantCommand(() -> robot.getArmVisualizer().showPath(Robot.pathDown)).andThen(new WaitCommand(0.3)) .andThen(new FollowPathCommand(Robot.pathDown, p -> robot.getArm().setPosition(p, false))) ); usedJoystick.B.onTrue( - new InstantCommand(() -> robot.getArmVisualizer().showPath(Robot.pathLeft)) + new InstantCommand(() -> robot.getArmVisualizer().showPath(Robot.pathLeft)).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)) + new InstantCommand(() -> robot.getArmVisualizer().showPath(Robot.pathUp)).andThen(new WaitCommand(0.3)) .andThen(new FollowPathCommand(Robot.pathUp, p -> robot.getArm().setPosition(p, false))) ); } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index a3bb76ba53..ba7bb6da02 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -42,9 +42,9 @@ public class Robot { public Robot() { BatteryUtil.scheduleLimiter(); - pathUp = PathGenerator.straightLine(new Translation2d(-0.4, 0.05), new Translation2d(-0.4, 1), 8); - pathLeft = PathGenerator.straightLine(new Translation2d(-0.4, 1), new Translation2d(0.4, 1), 8); - pathDown = PathGenerator.straightLine(new Translation2d(0.4, 1), new Translation2d(0.4, 0.05), 8); + 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)); } public void periodic() { diff --git a/src/main/java/frc/robot/visualizers/FollowPathCommand.java b/src/main/java/frc/robot/visualizers/FollowPathCommand.java index e06403e61a..8d9a26ab20 100644 --- a/src/main/java/frc/robot/visualizers/FollowPathCommand.java +++ b/src/main/java/frc/robot/visualizers/FollowPathCommand.java @@ -9,11 +9,9 @@ public class FollowPathCommand extends Command { - private final double DELAY_SECONDS = 0.2; private final Consumer setPosition; private final List path; - private double lastSetTime = 0; private int currentPoint = 0; public FollowPathCommand(List path, Consumer setPosition) { @@ -24,16 +22,12 @@ public FollowPathCommand(List path, Consumer setPo @Override public void initialize() { currentPoint = 0; - lastSetTime = 0; } @Override public void execute() { - if (TimeUtil.getCurrentTimeSeconds() - lastSetTime > DELAY_SECONDS) { - lastSetTime = TimeUtil.getCurrentTimeSeconds(); - setPosition.accept(path.get(currentPoint)); - currentPoint++; - } + setPosition.accept(path.get(currentPoint)); + currentPoint++; } @Override diff --git a/src/main/java/frc/robot/visualizers/PathGenerator.java b/src/main/java/frc/robot/visualizers/PathGenerator.java index d091487933..97cbe31040 100644 --- a/src/main/java/frc/robot/visualizers/PathGenerator.java +++ b/src/main/java/frc/robot/visualizers/PathGenerator.java @@ -8,7 +8,8 @@ public class PathGenerator { - public static List straightLine(Translation2d start, Translation2d end, int steps) { + public static List straightLine(Translation2d start, Translation2d end) { + int steps = (int) (end.getDistance(start) / 0.015); List points = new ArrayList<>(); double deltaX = end.getX() - start.getX(); From 438b0d278fb23a9efa10445b1c1762478a82ca3f Mon Sep 17 00:00:00 2001 From: YHerm <0549405422.yh@gmail.com> Date: Wed, 2 Apr 2025 13:16:11 +0300 Subject: [PATCH 4/5] flip arm --- src/main/java/frc/JoysticksBindings.java | 15 ++++++- .../robot/subsystems/DoubleJointedArm.java | 1 + .../FollowPathWithSideCommand.java | 38 ++++++++++++++++++ .../frc/robot/visualizers/PathGenerator.java | 39 +++++++++++++++++++ 4 files changed, 92 insertions(+), 1 deletion(-) create mode 100644 src/main/java/frc/robot/visualizers/FollowPathWithSideCommand.java diff --git a/src/main/java/frc/JoysticksBindings.java b/src/main/java/frc/JoysticksBindings.java index 10fa95548b..d26d69636e 100644 --- a/src/main/java/frc/JoysticksBindings.java +++ b/src/main/java/frc/JoysticksBindings.java @@ -1,11 +1,18 @@ 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 { @@ -31,7 +38,7 @@ private static void mainJoystickButtons(Robot robot) { // robot.getArm().applyTestBinds(usedJoystick); usedJoystick.A.onTrue( new InstantCommand(() -> robot.getArmVisualizer().showPath(Robot.pathDown)).andThen(new WaitCommand(0.3)) - .andThen(new FollowPathCommand(Robot.pathDown, p -> robot.getArm().setPosition(p, false))) + .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)) @@ -41,6 +48,12 @@ private static void mainJoystickButtons(Robot robot) { new InstantCommand(() -> robot.getArmVisualizer().showPath(Robot.pathUp)).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()))) + ); } private static void secondJoystickButtons(Robot robot) { diff --git a/src/main/java/frc/robot/subsystems/DoubleJointedArm.java b/src/main/java/frc/robot/subsystems/DoubleJointedArm.java index 351c375ceb..7c83ae2fa9 100644 --- a/src/main/java/frc/robot/subsystems/DoubleJointedArm.java +++ b/src/main/java/frc/robot/subsystems/DoubleJointedArm.java @@ -11,6 +11,7 @@ public class DoubleJointedArm extends GBSubsystem { public static final double FIRST_JOINT_LENGTH_METERS = 0.8; public static final double SECOND_JOINT_LENGTH_METERS = 0.5; + public static final double TOTAL_LENGTH_METERS = FIRST_JOINT_LENGTH_METERS + SECOND_JOINT_LENGTH_METERS; private Rotation2d firstJointAngle = new Rotation2d(); private Rotation2d secondJointAngle = new Rotation2d(); diff --git a/src/main/java/frc/robot/visualizers/FollowPathWithSideCommand.java b/src/main/java/frc/robot/visualizers/FollowPathWithSideCommand.java new file mode 100644 index 0000000000..1838d947c8 --- /dev/null +++ b/src/main/java/frc/robot/visualizers/FollowPathWithSideCommand.java @@ -0,0 +1,38 @@ +package frc.robot.visualizers; + +import edu.wpi.first.math.Pair; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj2.command.Command; + +import java.util.List; +import java.util.function.Consumer; + +public class FollowPathWithSideCommand extends Command { + + private final Consumer> setPositionWithSide; + private final List> path; + + private int currentPoint = 0; + + public FollowPathWithSideCommand(List> path, Consumer> setPositionWithSide) { + this.path = path; + this.setPositionWithSide = setPositionWithSide; + } + + @Override + public void initialize() { + currentPoint = 0; + } + + @Override + public void execute() { + setPositionWithSide.accept(path.get(currentPoint)); + currentPoint++; + } + + @Override + public boolean isFinished() { + return currentPoint >= path.size(); + } + +} diff --git a/src/main/java/frc/robot/visualizers/PathGenerator.java b/src/main/java/frc/robot/visualizers/PathGenerator.java index 97cbe31040..63635b63eb 100644 --- a/src/main/java/frc/robot/visualizers/PathGenerator.java +++ b/src/main/java/frc/robot/visualizers/PathGenerator.java @@ -1,5 +1,6 @@ package frc.robot.visualizers; +import edu.wpi.first.math.Pair; import edu.wpi.first.math.geometry.Translation2d; import java.util.ArrayList; @@ -27,4 +28,42 @@ public static List straightLine(Translation2d start, Translation2 return points; } + public static List> straightLine(Translation2d start, Translation2d end, boolean isLeft) { + int steps = (int) (end.getDistance(start) / 0.015); + List> points = new ArrayList<>(); + + double deltaX = end.getX() - start.getX(); + double deltaY = end.getY() - start.getY(); + + double stepX = deltaX / (steps - 1); + double stepY = deltaY / (steps - 1); + + for (int i = 0; i < steps; i++) { + double x = start.getX() + i * stepX; + double y = start.getY() + i * stepY; + points.add(new Pair<>(new Translation2d(x, y), isLeft)); + } + + return points; + } + + public static List> flipArm(Translation2d point, boolean isStartLeft, double length) { + Translation2d fullExtendedPoint = new Translation2d(point.getAngle().getCos() * length, point.getAngle().getSin() * length); + List> firstPath = straightLine(point, fullExtendedPoint, isStartLeft); + List> secondPath = straightLine(fullExtendedPoint, point, !isStartLeft); + + firstPath.addAll(secondPath); + return firstPath; + } + + public static List createTranslation2dList(List> flipArm) { + List translationList = new ArrayList<>(); + + for (Pair pair : flipArm) { + translationList.add(pair.getFirst()); + } + + return translationList; + } + } From 533070b814a594c5a75987f59039ce269dde3588 Mon Sep 17 00:00:00 2001 From: YHerm <0549405422.yh@gmail.com> Date: Wed, 2 Apr 2025 13:17:25 +0300 Subject: [PATCH 5/5] format --- src/main/java/frc/JoysticksBindings.java | 16 +++--- .../robot/visualizers/FollowPathCommand.java | 1 - .../FollowPathWithSideCommand.java | 50 +++++++++---------- 3 files changed, 34 insertions(+), 33 deletions(-) diff --git a/src/main/java/frc/JoysticksBindings.java b/src/main/java/frc/JoysticksBindings.java index d26d69636e..9fbf98459c 100644 --- a/src/main/java/frc/JoysticksBindings.java +++ b/src/main/java/frc/JoysticksBindings.java @@ -41,18 +41,20 @@ private static void mainJoystickButtons(Robot robot) { .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)) - .andThen(new FollowPathCommand(Robot.pathLeft, p -> robot.getArm().setPosition(p, false))) + new InstantCommand(() -> robot.getArmVisualizer().showPath(Robot.pathLeft)).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)).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); + 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()))) + 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()))) ); } diff --git a/src/main/java/frc/robot/visualizers/FollowPathCommand.java b/src/main/java/frc/robot/visualizers/FollowPathCommand.java index 8d9a26ab20..f323ed589d 100644 --- a/src/main/java/frc/robot/visualizers/FollowPathCommand.java +++ b/src/main/java/frc/robot/visualizers/FollowPathCommand.java @@ -2,7 +2,6 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj2.command.Command; -import frc.utils.time.TimeUtil; import java.util.List; import java.util.function.Consumer; diff --git a/src/main/java/frc/robot/visualizers/FollowPathWithSideCommand.java b/src/main/java/frc/robot/visualizers/FollowPathWithSideCommand.java index 1838d947c8..a94aa6ea24 100644 --- a/src/main/java/frc/robot/visualizers/FollowPathWithSideCommand.java +++ b/src/main/java/frc/robot/visualizers/FollowPathWithSideCommand.java @@ -9,30 +9,30 @@ public class FollowPathWithSideCommand extends Command { - private final Consumer> setPositionWithSide; - private final List> path; - - private int currentPoint = 0; - - public FollowPathWithSideCommand(List> path, Consumer> setPositionWithSide) { - this.path = path; - this.setPositionWithSide = setPositionWithSide; - } - - @Override - public void initialize() { - currentPoint = 0; - } - - @Override - public void execute() { - setPositionWithSide.accept(path.get(currentPoint)); - currentPoint++; - } - - @Override - public boolean isFinished() { - return currentPoint >= path.size(); - } + private final Consumer> setPositionWithSide; + private final List> path; + + private int currentPoint = 0; + + public FollowPathWithSideCommand(List> path, Consumer> setPositionWithSide) { + this.path = path; + this.setPositionWithSide = setPositionWithSide; + } + + @Override + public void initialize() { + currentPoint = 0; + } + + @Override + public void execute() { + setPositionWithSide.accept(path.get(currentPoint)); + currentPoint++; + } + + @Override + public boolean isFinished() { + return currentPoint >= path.size(); + } }