From b9a3a0b215a2a0eab94c855c138ac9e3a2f72b4d Mon Sep 17 00:00:00 2001 From: YHerm <0549405422.yh@gmail.com> Date: Tue, 1 Apr 2025 19:49:13 +0300 Subject: [PATCH 1/3] clean --- src/main/java/frc/robot/subsystems/DoubleJointedArm.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/DoubleJointedArm.java b/src/main/java/frc/robot/subsystems/DoubleJointedArm.java index 205668bff9..b474b96d7c 100644 --- a/src/main/java/frc/robot/subsystems/DoubleJointedArm.java +++ b/src/main/java/frc/robot/subsystems/DoubleJointedArm.java @@ -55,7 +55,7 @@ public Translation2d getPositionMeters() { } public void setPosition(Translation2d positionMeters, boolean firstJointLeft) { - double distanceMeters = positionMeters.getDistance(new Translation2d()); + double distanceMeters = positionMeters.getNorm(); double alphaRads = Math.atan2(positionMeters.getY(), positionMeters.getX()); double omegaRads = Math.acos((distanceMeters * FIRST_JOINT_PERCENT_FROM_ARM) / FIRST_JOINT_LENGTH_METERS); From d116fc78cafa074eecb0644c06dd3b5bd02335b7 Mon Sep 17 00:00:00 2001 From: YHerm <0549405422.yh@gmail.com> Date: Tue, 1 Apr 2025 20:00:59 +0300 Subject: [PATCH 2/3] fix math --- .../robot/subsystems/DoubleJointedArm.java | 46 ++++++++++++------- 1 file changed, 30 insertions(+), 16 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/DoubleJointedArm.java b/src/main/java/frc/robot/subsystems/DoubleJointedArm.java index b474b96d7c..58397edbcb 100644 --- a/src/main/java/frc/robot/subsystems/DoubleJointedArm.java +++ b/src/main/java/frc/robot/subsystems/DoubleJointedArm.java @@ -1,5 +1,6 @@ package frc.robot.subsystems; +import edu.wpi.first.math.Pair; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj2.command.InstantCommand; @@ -11,10 +12,6 @@ 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(); @@ -55,16 +52,39 @@ public Translation2d getPositionMeters() { } public void setPosition(Translation2d positionMeters, boolean firstJointLeft) { - double distanceMeters = positionMeters.getNorm(); + Pair targetAngles = toAngles(positionMeters, firstJointLeft); + setAngles(targetAngles.getFirst(), targetAngles.getSecond()); + } + + 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); + } + + private static Pair toAngles(Translation2d positionMeters, boolean firstJointLeft) { + double x = positionMeters.getX(); + double y = positionMeters.getY(); - double alphaRads = Math.atan2(positionMeters.getY(), positionMeters.getX()); - double omegaRads = Math.acos((distanceMeters * FIRST_JOINT_PERCENT_FROM_ARM) / FIRST_JOINT_LENGTH_METERS); + double cosTheta2 = (x * x + + y * y + - FIRST_JOINT_LENGTH_METERS * FIRST_JOINT_LENGTH_METERS + - SECOND_JOINT_LENGTH_METERS * SECOND_JOINT_LENGTH_METERS) / (2 * FIRST_JOINT_LENGTH_METERS * SECOND_JOINT_LENGTH_METERS); - if (!firstJointLeft) { - omegaRads = -omegaRads; + if (cosTheta2 < -1 || cosTheta2 > 1) { + throw new IllegalArgumentException("Position is unreachable."); } - setAngles(Rotation2d.fromRadians(alphaRads + omegaRads), Rotation2d.fromRadians(alphaRads - omegaRads)); + double theta2 = Math.acos(cosTheta2); + if (firstJointLeft) { + theta2 = -theta2; + } + + double theta1 = Math.atan2(y, x) + - Math + .atan2(SECOND_JOINT_LENGTH_METERS * Math.sin(theta2), FIRST_JOINT_LENGTH_METERS + SECOND_JOINT_LENGTH_METERS * Math.cos(theta2)); + + return new Pair<>(Rotation2d.fromRadians(theta1), Rotation2d.fromRadians(theta1 + theta2)); } public void applyTestBinds(SmartJoystick joystick) { @@ -83,10 +103,4 @@ public void applyTestBinds(SmartJoystick joystick) { .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); - } - } From efc035110ec294523135ac67dd9df51ba523328e Mon Sep 17 00:00:00 2001 From: YHerm <0549405422.yh@gmail.com> Date: Wed, 2 Apr 2025 11:32:02 +0300 Subject: [PATCH 3/3] Work --- .../robot/subsystems/DoubleJointedArm.java | 20 ++++++++++--------- 1 file changed, 11 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/DoubleJointedArm.java b/src/main/java/frc/robot/subsystems/DoubleJointedArm.java index 58397edbcb..351c375ceb 100644 --- a/src/main/java/frc/robot/subsystems/DoubleJointedArm.java +++ b/src/main/java/frc/robot/subsystems/DoubleJointedArm.java @@ -66,16 +66,8 @@ private static Pair toAngles(Translation2d positionMeter double x = positionMeters.getX(); double y = positionMeters.getY(); - double cosTheta2 = (x * x - + y * y - - FIRST_JOINT_LENGTH_METERS * FIRST_JOINT_LENGTH_METERS - - SECOND_JOINT_LENGTH_METERS * SECOND_JOINT_LENGTH_METERS) / (2 * FIRST_JOINT_LENGTH_METERS * SECOND_JOINT_LENGTH_METERS); + double theta2 = cosineLaw(Math.sqrt(x * x + y * y), FIRST_JOINT_LENGTH_METERS, SECOND_JOINT_LENGTH_METERS).getRadians(); - if (cosTheta2 < -1 || cosTheta2 > 1) { - throw new IllegalArgumentException("Position is unreachable."); - } - - double theta2 = Math.acos(cosTheta2); if (firstJointLeft) { theta2 = -theta2; } @@ -103,4 +95,14 @@ public void applyTestBinds(SmartJoystick joystick) { .onTrue(new InstantCommand(() -> setPosition(toPositionMeters(Rotation2d.fromDegrees(75), Rotation2d.fromDegrees(120)), false))); } + public static Rotation2d cosineLaw(double farSide, double nearSide1, double nearSide2) { + double cosTheta = (farSide * farSide - nearSide1 * nearSide1 - nearSide2 * nearSide2) / (2 * nearSide1 * nearSide2); + + if (cosTheta < -1 || cosTheta > 1) { + throw new IllegalArgumentException("Position is unreachable."); + } + + return Rotation2d.fromRadians(Math.acos(cosTheta)); + } + }