Straight and Flip Arm paths

src/main/java/frc/JoysticksBindings.java

@@ -1,8 +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 {
 
@@ -25,7 +35,27 @@ 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 InstantCommand(() -> robot.getArmVisualizer().showPath(Robot.pathDown)).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))
+				.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)))
+		);
+
+		List<Pair<Translation2d, Boolean>> 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) {

src/main/java/frc/robot/Robot.java

@@ -4,15 +4,19 @@
 
 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;
 import frc.RobotManager;
 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;
+
 /**
  * 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 +35,16 @@ public class Robot {
 		DoubleJointedArm.SECOND_JOINT_LENGTH_METERS
 	);
 
+	public static List<Translation2d> pathUp;
+	public static List<Translation2d> pathLeft;
+	public static List<Translation2d> 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));
 	}
 
 	public void periodic() {
@@ -53,4 +65,8 @@ public DoubleJointedArm getArm() {
 		return arm;
 	}
 
+	public DoubleJointedArmVisualizer getArmVisualizer() {
+		return armVisualizer;
+	}
+
 }

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();

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<ArrayList<MechanismLigament2d>> paths = new ArrayList<>();
 
 	public DoubleJointedArmVisualizer(
 		String name,
@@ -56,7 +60,30 @@ 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<Translation2d> path) {
+		ArrayList<MechanismLigament2d> 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).setLineWeight(0);
+			}
+		}
+		paths.add(pathVisualize);
 	}
 
 	private static Rotation2d toFloorRelative(Rotation2d floorRelativeAngle, Rotation2d jointRelativeAngle) {

src/main/java/frc/robot/visualizers/FollowPathCommand.java

@@ -0,0 +1,37 @@
+package frc.robot.visualizers;
+
+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 FollowPathCommand extends Command {
+
+	private final Consumer<Translation2d> setPosition;
+	private final List<Translation2d> path;
+
+	private int currentPoint = 0;
+
+	public FollowPathCommand(List<Translation2d> path, Consumer<Translation2d> setPosition) {
+		this.path = path;
+		this.setPosition = setPosition;
+	}
+
+	@Override
+	public void initialize() {
+		currentPoint = 0;
+	}
+
+	@Override
+	public void execute() {
+		setPosition.accept(path.get(currentPoint));
+		currentPoint++;
+	}
+
+	@Override
+	public boolean isFinished() {
+		return currentPoint >= path.size();
+	}
+
+}

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<Pair<Translation2d, Boolean>> setPositionWithSide;
+	private final List<Pair<Translation2d, Boolean>> path;
+
+	private int currentPoint = 0;
+
+	public FollowPathWithSideCommand(List<Pair<Translation2d, Boolean>> path, Consumer<Pair<Translation2d, Boolean>> 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();
+	}
+
+}

src/main/java/frc/robot/visualizers/PathGenerator.java

@@ -0,0 +1,69 @@
+package frc.robot.visualizers;
+
+import edu.wpi.first.math.Pair;
+import edu.wpi.first.math.geometry.Translation2d;
+
+import java.util.ArrayList;
+import java.util.List;
+
+public class PathGenerator {
+
+
+	public static List<Translation2d> straightLine(Translation2d start, Translation2d end) {
+		int steps = (int) (end.getDistance(start) / 0.015);
+		List<Translation2d> 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;
+	}
+
+	public static List<Pair<Translation2d, Boolean>> straightLine(Translation2d start, Translation2d end, boolean isLeft) {
+		int steps = (int) (end.getDistance(start) / 0.015);
+		List<Pair<Translation2d, Boolean>> 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<Pair<Translation2d, Boolean>> flipArm(Translation2d point, boolean isStartLeft, double length) {
+		Translation2d fullExtendedPoint = new Translation2d(point.getAngle().getCos() * length, point.getAngle().getSin() * length);
+		List<Pair<Translation2d, Boolean>> firstPath = straightLine(point, fullExtendedPoint, isStartLeft);
+		List<Pair<Translation2d, Boolean>> secondPath = straightLine(fullExtendedPoint, point, !isStartLeft);
+
+		firstPath.addAll(secondPath);
+		return firstPath;
+	}
+
+	public static List<Translation2d> createTranslation2dList(List<Pair<Translation2d, Boolean>> flipArm) {
+		List<Translation2d> translationList = new ArrayList<>();
+
+		for (Pair<Translation2d, Boolean> pair : flipArm) {
+			translationList.add(pair.getFirst());
+		}
+
+		return translationList;
+	}
+
+}