Kinematics

src/main/java/frc/JoysticksBindings.java

@@ -25,6 +25,7 @@ public static void configureBindings(Robot robot) {
 	private static void mainJoystickButtons(Robot robot) {
 		SmartJoystick usedJoystick = MAIN_JOYSTICK;
 		// bindings...
+		robot.getArm().applyTestBinds(usedJoystick);
 	}
 
 	private static void secondJoystickButtons(Robot robot) {

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

@@ -40,6 +40,7 @@ public void periodic() {
 		BusChain.logChainsStatuses();
 
 		armVisualizer.setAngles(arm.getFirstJointAngle(), arm.getSecondJointAngle());
+		armVisualizer.setTargetPositionMeters(arm.getPositionMeters());
 
 		CommandScheduler.getInstance().run(); // Should be last
 	}

src/main/java/frc/robot/subsystems/DoubleJointedArm.java

@@ -1,6 +1,7 @@
 package frc.robot.subsystems;
 
 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 org.littletonrobotics.junction.Logger;
@@ -10,6 +11,10 @@ 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_ARM_LENGTH = FIRST_JOINT_LENGTH_METERS + SECOND_JOINT_LENGTH_METERS;
+	public static final double FIRST_JOINT_PERCENT_FROM_ARM = FIRST_JOINT_LENGTH_METERS / TOTAL_ARM_LENGTH;
+	public static final double SECOND_JOINT_PERCENT_FROM_ARM = SECOND_JOINT_LENGTH_METERS / TOTAL_ARM_LENGTH;
+
 	private Rotation2d firstJointAngle = new Rotation2d();
 	private Rotation2d secondJointAngle = new Rotation2d();
 
@@ -21,6 +26,7 @@ public DoubleJointedArm(String logPath) {
 	protected void subsystemPeriodic() {
 		Logger.recordOutput(getLogPath() + "/FirstJointAngleDeg", firstJointAngle.getDegrees());
 		Logger.recordOutput(getLogPath() + "/SecondJointAngleDeg", secondJointAngle.getDegrees());
+		Logger.recordOutput(getLogPath() + "/PositionMeters", getPositionMeters());
 	}
 
 	public Rotation2d getFirstJointAngle() {
@@ -39,16 +45,48 @@ public void setSecondJointAngle(Rotation2d secondJointAngle) {
 		this.secondJointAngle = secondJointAngle;
 	}
 
-	public void setAngles(Rotation2d a1, Rotation2d a2) {
-		setFirstJointAngle(a1);
-		setSecondJointAngle(a2);
+	public void setAngles(Rotation2d firstJointAngle, Rotation2d secondJointAngle) {
+		setFirstJointAngle(firstJointAngle);
+		setSecondJointAngle(secondJointAngle);
+	}
+
+	public Translation2d getPositionMeters() {
+		return toPositionMeters(firstJointAngle, secondJointAngle);
+	}
+
+	public void setPosition(Translation2d positionMeters, boolean firstJointLeft) {
+		double distanceMeters = positionMeters.getDistance(new Translation2d());
+
+		double alphaRads = Math.atan2(positionMeters.getY(), positionMeters.getX());
+		double omegaRads = Math.acos((distanceMeters * FIRST_JOINT_PERCENT_FROM_ARM) / FIRST_JOINT_LENGTH_METERS);
+
+		if (!firstJointLeft) {
+			omegaRads = -omegaRads;
+		}
+
+		setAngles(Rotation2d.fromRadians(alphaRads + omegaRads), Rotation2d.fromRadians(alphaRads - omegaRads));
 	}
 
 	public void applyTestBinds(SmartJoystick joystick) {
-		joystick.A.onTrue(new InstantCommand(() -> setAngles(Rotation2d.fromDegrees(100), Rotation2d.fromDegrees(210))));
-		joystick.B.onTrue(new InstantCommand(() -> setAngles(Rotation2d.fromDegrees(60), Rotation2d.fromDegrees(80))));
+		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(95), Rotation2d.fromDegrees(85))));
+		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)));
+	}
+
+	private static Translation2d toPositionMeters(Rotation2d firstJointAngle, Rotation2d secondJointAngle) {
+		double xMeters = FIRST_JOINT_LENGTH_METERS * firstJointAngle.getCos() + SECOND_JOINT_LENGTH_METERS * secondJointAngle.getCos();
+		double yMeters = FIRST_JOINT_LENGTH_METERS * firstJointAngle.getSin() + SECOND_JOINT_LENGTH_METERS * secondJointAngle.getSin();
+		return new Translation2d(xMeters, yMeters);
 	}
 
 }

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

@@ -1,6 +1,7 @@
 package frc.robot.visualizers;
 
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d;
 import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d;
 import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d;
@@ -10,10 +11,16 @@
 
 public class DoubleJointedArmVisualizer {
 
+	private static final MechanismLigament2d TARGET_POSITION_MARK = new MechanismLigament2d("mark", 0.03, 0, 10.0F, new Color8Bit(Color.kGreen));
+	private static final double DEFAULT_LINE_WIDTH = 10.0F;
+
+	private final Translation2d armRootPosition;
+
 	private final Mechanism2d mechanism;
 	private final MechanismRoot2d root;
 	private final MechanismLigament2d firstJoint;
 	private final MechanismLigament2d secondJoint;
+	private final MechanismRoot2d targetPosition;
 
 	public DoubleJointedArmVisualizer(
 		String name,
@@ -23,12 +30,18 @@ public DoubleJointedArmVisualizer(
 		double secondJointLengthMeters
 	) {
 		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, 10.0F, new Color8Bit(Color.kPurple));
+		this.secondJoint = new MechanismLigament2d("second joint", secondJointLengthMeters, 0, DEFAULT_LINE_WIDTH, new Color8Bit(Color.kPurple));
+		this.targetPosition = mechanism.getRoot(name + " TargetPosition", 0, 0);
 
 		firstJoint.append(secondJoint);
 		root.append(firstJoint);
+		targetPosition.append(TARGET_POSITION_MARK);
+
+		showTargetPosition(false);
+
 		SmartDashboard.putData(name + " DoubleJointedArmMech2d", mechanism);
 	}
 
@@ -37,6 +50,15 @@ public void setAngles(Rotation2d firstJointAngle, Rotation2d secondJointAngle) {
 		secondJoint.setAngle(toFloorRelative(firstJointAngle, secondJointAngle));
 	}
 
+	public void setTargetPositionMeters(Translation2d positionMeters) {
+		targetPosition.setPosition(positionMeters.getX() + armRootPosition.getX(), positionMeters.getY() + armRootPosition.getY());
+		showTargetPosition(true);
+	}
+
+	public void showTargetPosition(boolean show) {
+		TARGET_POSITION_MARK.setLength(show ? 0.01 : 0);
+	}
+
 	private static Rotation2d toFloorRelative(Rotation2d floorRelativeAngle, Rotation2d jointRelativeAngle) {
 		return jointRelativeAngle.minus(floorRelativeAngle);
 	}