Clean

src/main/java/frc/JoysticksBindings.java

@@ -5,7 +5,7 @@
 import frc.joysticks.JoystickPorts;
 import frc.joysticks.SmartJoystick;
 import frc.robot.Robot;
-import frc.robot.visualizers.FollowPathCommand;
+import frc.robot.structures.FollowTrajectoryDemo;
 
 
 public class JoysticksBindings {
@@ -32,15 +32,15 @@ private static void mainJoystickButtons(Robot robot) {
 //		robot.getArm().applyTestBinds(usedJoystick);
 		usedJoystick.A.onTrue(
 			new InstantCommand(() -> robot.getArmVisualizer().showPath(Robot.pathDown.getStates())).andThen(new WaitCommand(0.3))
-				.andThen(new FollowPathCommand(Robot.pathDown, p -> robot.getArm().setPosition(p, true)))
+				.andThen(new FollowTrajectoryDemo(Robot.pathDown, p -> robot.getArm().setPosition(p, true)))
 		);
 		usedJoystick.B.onTrue(
 			new InstantCommand(() -> robot.getArmVisualizer().showPath(Robot.pathLeft.getStates())).andThen(new WaitCommand(0.3))
-				.andThen(new FollowPathCommand(Robot.pathLeft, p -> robot.getArm().setPosition(p, false)))
+				.andThen(new FollowTrajectoryDemo(Robot.pathLeft, p -> robot.getArm().setPosition(p, false)))
 		);
 		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 FollowTrajectoryDemo(Robot.pathUp, p -> robot.getArm().setPosition(p, false)))
 		);
 
 //		List<Pair<Translation2d, Boolean>> flipArm = PathGenerator

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

@@ -15,8 +15,8 @@
 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.structures.doublejointedarm.DoubleJointedArm;
+import frc.robot.structures.doublejointedarm.DoubleJointedArmVisualizer;
 import frc.utils.battery.BatteryUtil;
 
 import java.util.List;
@@ -35,8 +35,8 @@ public class Robot {
 		"",
 		2.5,
 		2.5,
-		DoubleJointedArm.FIRST_JOINT_LENGTH_METERS,
-		DoubleJointedArm.SECOND_JOINT_LENGTH_METERS
+		DoubleJointedArm.ELBOW_LENGTH_METERS,
+		DoubleJointedArm.WRIST_LENGTH_METERS
 	);
 
 	public static Trajectory pathUp;
@@ -71,7 +71,7 @@ public void periodic() {
 		BatteryUtil.logStatus();
 		BusChain.logChainsStatuses();
 
-		armVisualizer.setAngles(arm.getFirstJointAngle(), arm.getSecondJointAngle());
+		armVisualizer.setAngles(arm.getElbowAngle(), arm.getWristAngle());
 		armVisualizer.setTargetPositionMeters(arm.getPositionMeters());
 
 		CommandScheduler.getInstance().run(); // Should be last

src/main/java/frc/robot/visualizers/FollowPathCommand.java → src/main/java/frc/robot/structures/FollowTrajectoryDemo.java

@@ -1,4 +1,4 @@
-package frc.robot.visualizers;
+package frc.robot.structures;
 
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.trajectory.Trajectory;
@@ -7,14 +7,14 @@
 
 import java.util.function.Consumer;
 
-public class FollowPathCommand extends Command {
+public class FollowTrajectoryDemo extends Command {
 
 	private final Consumer<Translation2d> setPosition;
 	private final Trajectory path;
 
 	private double startTime = 0;
 
-	public FollowPathCommand(Trajectory trajectory, Consumer<Translation2d> setPosition) {
+	public FollowTrajectoryDemo(Trajectory trajectory, Consumer<Translation2d> setPosition) {
 		this.path = trajectory;
 		this.setPosition = setPosition;
 	}

src/main/java/frc/robot/structures/doublejointedarm/ArmAngles.java

@@ -0,0 +1,5 @@
+package frc.robot.structures.doublejointedarm;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+
+public record ArmAngles(Rotation2d elbowAngle, Rotation2d wristAngle) {}

src/main/java/frc/robot/structures/doublejointedarm/DoubleJointedArm.java

@@ -0,0 +1,92 @@
+package frc.robot.structures.doublejointedarm;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import frc.joysticks.SmartJoystick;
+import frc.robot.subsystems.GBSubsystem;
+import frc.utils.alerts.Alert;
+import org.littletonrobotics.junction.Logger;
+
+import java.util.Optional;
+
+public class DoubleJointedArm extends GBSubsystem {
+
+	public static final double ELBOW_LENGTH_METERS = 0.8;
+	public static final double WRIST_LENGTH_METERS = 0.5;
+	public static final double TOTAL_LENGTH_METERS = ELBOW_LENGTH_METERS + WRIST_LENGTH_METERS;
+
+	private Rotation2d elbowAngle = new Rotation2d();
+	private Rotation2d wristAngle = new Rotation2d();
+
+	public DoubleJointedArm(String logPath) {
+		super(logPath);
+	}
+
+	@Override
+	protected void subsystemPeriodic() {
+		Logger.recordOutput(getLogPath() + "/ElbowAngleDeg", elbowAngle.getDegrees());
+		Logger.recordOutput(getLogPath() + "/WristAngleDeg", wristAngle.getDegrees());
+		Logger.recordOutput(getLogPath() + "/PositionMeters", getPositionMeters());
+	}
+
+	public Rotation2d getElbowAngle() {
+		return elbowAngle;
+	}
+
+	public Rotation2d getWristAngle() {
+		return wristAngle;
+	}
+
+	public void setElbowAngle(Rotation2d elbowAngle) {
+		this.elbowAngle = elbowAngle;
+	}
+
+	public void setWristAngle(Rotation2d wristAngle) {
+		this.wristAngle = wristAngle;
+	}
+
+	public void setAngles(Rotation2d elbowAngle, Rotation2d wristAngle) {
+		setElbowAngle(elbowAngle);
+		setWristAngle(wristAngle);
+	}
+
+	public Translation2d getPositionMeters() {
+		return toPositionMeters(elbowAngle, wristAngle);
+	}
+
+	public void setPosition(Translation2d positionMeters, boolean isElbowLeft) {
+		Optional<ArmAngles> targetAngles = toAngles(positionMeters, isElbowLeft);
+		if (targetAngles.isEmpty()) {
+			new Alert(Alert.AlertType.ERROR, getLogPath() + "unreachable position").report();
+			return;
+		}
+		setAngles(targetAngles.get().elbowAngle(), targetAngles.get().wristAngle());
+	}
+
+	private static Optional<ArmAngles> toAngles(Translation2d positionMeters, boolean isElbowLeft) {
+		return DoubleJointedArmKinematics.toAngles(positionMeters, ELBOW_LENGTH_METERS, WRIST_LENGTH_METERS, isElbowLeft);
+	}
+
+	private static Translation2d toPositionMeters(Rotation2d elbowAngle, Rotation2d wristAngle) {
+		return DoubleJointedArmKinematics.toPositionMeters(new ArmAngles(elbowAngle, wristAngle), ELBOW_LENGTH_METERS, WRIST_LENGTH_METERS);
+	}
+
+	public void applyTestBinds(SmartJoystick joystick) {
+		joystick.A.onTrue(new InstantCommand(() -> setAngles(Rotation2d.fromDegrees(140), Rotation2d.fromDegrees(195))));
+		joystick.B.onTrue(new InstantCommand(() -> setAngles(Rotation2d.fromDegrees(50), Rotation2d.fromDegrees(-45))));
+		joystick.X.onTrue(new InstantCommand(() -> setAngles(Rotation2d.fromDegrees(150), Rotation2d.fromDegrees(75))));
+		joystick.Y.onTrue(new InstantCommand(() -> setAngles(Rotation2d.fromDegrees(75), Rotation2d.fromDegrees(120))));
+
+		joystick.POV_DOWN
+			.onTrue(new InstantCommand(() -> setPosition(toPositionMeters(Rotation2d.fromDegrees(140), Rotation2d.fromDegrees(195)), false)));
+		joystick.POV_RIGHT
+			.onTrue(new InstantCommand(() -> setPosition(toPositionMeters(Rotation2d.fromDegrees(50), Rotation2d.fromDegrees(-45)), true)));
+		joystick.POV_LEFT
+			.onTrue(new InstantCommand(() -> setPosition(toPositionMeters(Rotation2d.fromDegrees(150), Rotation2d.fromDegrees(75)), true)));
+		joystick.POV_UP
+			.onTrue(new InstantCommand(() -> setPosition(toPositionMeters(Rotation2d.fromDegrees(75), Rotation2d.fromDegrees(120)), false)));
+	}
+
+
+}

src/main/java/frc/robot/structures/doublejointedarm/DoubleJointedArmKinematics.java

@@ -0,0 +1,54 @@
+package frc.robot.structures.doublejointedarm;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+
+import java.util.Optional;
+
+public class DoubleJointedArmKinematics {
+
+	public static Translation2d toPositionMeters(ArmAngles angles, double elbowLengthMeters, double wristLengthMeters) {
+		return new Translation2d(
+			elbowLengthMeters * angles.elbowAngle().getCos() + wristLengthMeters * angles.wristAngle().getCos(),
+			elbowLengthMeters * angles.elbowAngle().getSin() + wristLengthMeters * angles.wristAngle().getSin()
+		);
+	}
+
+	public static Optional<ArmAngles> toAngles(
+		Translation2d positionMeters,
+		double elbowLengthMeters,
+		double wristLengthMeters,
+		boolean isElbowLeft
+	) {
+		double x = positionMeters.getX();
+		double y = positionMeters.getY();
+
+		Optional<Rotation2d> wristAngleOptional = cosineLaw(Math.sqrt(x * x + y * y), elbowLengthMeters, wristLengthMeters);
+		if (wristAngleOptional.isEmpty()) {
+			return Optional.empty();
+		}
+
+		double wristAngleRads = wristAngleOptional.get().getRadians();
+
+		if (isElbowLeft) {
+			wristAngleRads = -wristAngleRads;
+		}
+
+		double elbowAngleRads = Math.atan2(y, x)
+			- Math.atan2(wristLengthMeters * Math.sin(wristAngleRads), elbowLengthMeters + wristLengthMeters * Math.cos(wristAngleRads));
+
+		return Optional.of(new ArmAngles(Rotation2d.fromRadians(elbowAngleRads), Rotation2d.fromRadians(elbowAngleRads + wristAngleRads)));
+	}
+
+
+	public static Optional<Rotation2d> cosineLaw(double farSide, double nearSide1, double nearSide2) {
+		double cosTheta = (farSide * farSide - nearSide1 * nearSide1 - nearSide2 * nearSide2) / (2 * nearSide1 * nearSide2);
+
+		if (cosTheta < -1 || cosTheta > 1) {
+			return Optional.empty();
+		}
+
+		return Optional.of(Rotation2d.fromRadians(Math.acos(cosTheta)));
+	}
+
+}

src/main/java/frc/robot/visualizers/DoubleJointedArmVisualizer.java → src/main/java/frc/robot/structures/doublejointedarm/DoubleJointedArmVisualizer.java

@@ -1,4 +1,4 @@
-package frc.robot.visualizers;
+package frc.robot.structures.doublejointedarm;
 
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
@@ -22,27 +22,29 @@ public class DoubleJointedArmVisualizer {
 
 	private final Mechanism2d mechanism;
 	private final MechanismRoot2d root;
-	private final MechanismLigament2d firstJoint;
-	private final MechanismLigament2d secondJoint;
+	private final MechanismLigament2d elbow;
+	private final MechanismLigament2d wrist;
 	private final MechanismRoot2d targetPosition;
 	private final ArrayList<ArrayList<MechanismLigament2d>> paths = new ArrayList<>();
 
+	private int pathCounter = 0;
+
 	public DoubleJointedArmVisualizer(
 		String name,
 		double frameXMeters,
 		double frameYMeters,
-		double firstJointLengthMeters,
-		double secondJointLengthMeters
+		double elbowLengthMeters,
+		double wristLengthMeters
 	) {
 		this.mechanism = new Mechanism2d(frameXMeters, frameYMeters);
 		this.armRootPosition = new Translation2d(frameXMeters / 2.0, 0);
 		this.root = mechanism.getRoot(name + " DoubleJointedArm", frameXMeters / 2.0, 0);
-		this.firstJoint = new MechanismLigament2d("first joint", firstJointLengthMeters, 0);
-		this.secondJoint = new MechanismLigament2d("second joint", secondJointLengthMeters, 0, DEFAULT_LINE_WIDTH, new Color8Bit(Color.kPurple));
+		this.elbow = new MechanismLigament2d("elbow", elbowLengthMeters, 0);
+		this.wrist = new MechanismLigament2d("wrist", wristLengthMeters, 0, DEFAULT_LINE_WIDTH, new Color8Bit(Color.kPurple));
 		this.targetPosition = mechanism.getRoot(name + " TargetPosition", 0, 0);
 
-		firstJoint.append(secondJoint);
-		root.append(firstJoint);
+		elbow.append(wrist);
+		root.append(elbow);
 		targetPosition.append(TARGET_POSITION_MARK);
 
 		showTargetPosition(false);
@@ -51,8 +53,8 @@ public DoubleJointedArmVisualizer(
 	}
 
 	public void setAngles(Rotation2d firstJointAngle, Rotation2d secondJointAngle) {
-		firstJoint.setAngle(firstJointAngle);
-		secondJoint.setAngle(toFloorRelative(firstJointAngle, secondJointAngle));
+		elbow.setAngle(firstJointAngle);
+		wrist.setAngle(toFloorRelative(firstJointAngle, secondJointAngle));
 	}
 
 	public void setTargetPositionMeters(Translation2d positionMeters) {
@@ -65,29 +67,33 @@ public void showTargetPosition(boolean show) {
 	}
 
 	public void showPath(List<Trajectory.State> path) {
+		if (!paths.isEmpty()) {
+			for (int i = 0; i < paths.get(0).size(); i++) {
+				paths.get(0).get(i).setLineWeight(0);
+				paths.get(0).get(i).close();
+			}
+			paths.remove(0);
+		}
+
 		ArrayList<MechanismLigament2d> pathVisualize = new ArrayList<>(path.size());
 		for (int i = 0; i < path.size(); i++) {
 			MechanismRoot2d root = mechanism.getRoot(
-				paths.size() + ", " + i,
+				pathCounter + ", " + 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,
+				pathCounter + ", " + i + " mark",
 				0,
-				10.0F,
+				0,
+				DEFAULT_LINE_WIDTH,
 				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);
+		pathCounter++;
 	}
 
 	private static Rotation2d toFloorRelative(Rotation2d floorRelativeAngle, Rotation2d jointRelativeAngle) {