From dbea1e08bb100c5c22aebfa9256ff1df05672081 Mon Sep 17 00:00:00 2001 From: YHerm <0549405422.yh@gmail.com> Date: Thu, 3 Apr 2025 10:01:11 +0300 Subject: [PATCH 1/3] lcean & optimize --- src/main/java/frc/JoysticksBindings.java | 8 +-- src/main/java/frc/robot/Robot.java | 4 +- .../doublejointedarm}/DoubleJointedArm.java | 3 +- .../DoubleJointedArmVisualizer.java | 22 +++--- .../FollowTrajectoryDemo.java} | 6 +- .../FollowPathWithSideCommand.java | 38 ---------- .../frc/robot/visualizers/PathGenerator.java | 69 ------------------- 7 files changed, 25 insertions(+), 125 deletions(-) rename src/main/java/frc/robot/{subsystems => structures/doublejointedarm}/DoubleJointedArm.java (98%) rename src/main/java/frc/robot/{visualizers => structures/doublejointedarm}/DoubleJointedArmVisualizer.java (91%) rename src/main/java/frc/robot/{visualizers/FollowPathCommand.java => structures/doublejointedarm/FollowTrajectoryDemo.java} (80%) delete mode 100644 src/main/java/frc/robot/visualizers/FollowPathWithSideCommand.java delete 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 77408e0d7d..8769065f9f 100644 --- a/src/main/java/frc/JoysticksBindings.java +++ b/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.doublejointedarm.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> flipArm = PathGenerator diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 4272051a71..9d14ac4eec 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/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; diff --git a/src/main/java/frc/robot/subsystems/DoubleJointedArm.java b/src/main/java/frc/robot/structures/doublejointedarm/DoubleJointedArm.java similarity index 98% rename from src/main/java/frc/robot/subsystems/DoubleJointedArm.java rename to src/main/java/frc/robot/structures/doublejointedarm/DoubleJointedArm.java index 7c83ae2fa9..d4afcd4a3c 100644 --- a/src/main/java/frc/robot/subsystems/DoubleJointedArm.java +++ b/src/main/java/frc/robot/structures/doublejointedarm/DoubleJointedArm.java @@ -1,10 +1,11 @@ -package frc.robot.subsystems; +package frc.robot.structures.doublejointedarm; 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; import frc.joysticks.SmartJoystick; +import frc.robot.subsystems.GBSubsystem; import org.littletonrobotics.junction.Logger; public class DoubleJointedArm extends GBSubsystem { diff --git a/src/main/java/frc/robot/visualizers/DoubleJointedArmVisualizer.java b/src/main/java/frc/robot/structures/doublejointedarm/DoubleJointedArmVisualizer.java similarity index 91% rename from src/main/java/frc/robot/visualizers/DoubleJointedArmVisualizer.java rename to src/main/java/frc/robot/structures/doublejointedarm/DoubleJointedArmVisualizer.java index fa040bab44..20fe9d24df 100644 --- a/src/main/java/frc/robot/visualizers/DoubleJointedArmVisualizer.java +++ b/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; @@ -27,6 +27,8 @@ public class DoubleJointedArmVisualizer { private final MechanismRoot2d targetPosition; private final ArrayList> paths = new ArrayList<>(); + private int pathCounter = 0; + public DoubleJointedArmVisualizer( String name, double frameXMeters, @@ -65,15 +67,23 @@ public void showTargetPosition(boolean show) { } public void showPath(List 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 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", + pathCounter + ", " + i + " mark", 0.01, 0, 10.0F, @@ -82,12 +92,8 @@ public void showPath(List path) { 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) { diff --git a/src/main/java/frc/robot/visualizers/FollowPathCommand.java b/src/main/java/frc/robot/structures/doublejointedarm/FollowTrajectoryDemo.java similarity index 80% rename from src/main/java/frc/robot/visualizers/FollowPathCommand.java rename to src/main/java/frc/robot/structures/doublejointedarm/FollowTrajectoryDemo.java index c3085b73c2..3008372264 100644 --- a/src/main/java/frc/robot/visualizers/FollowPathCommand.java +++ b/src/main/java/frc/robot/structures/doublejointedarm/FollowTrajectoryDemo.java @@ -1,4 +1,4 @@ -package frc.robot.visualizers; +package frc.robot.structures.doublejointedarm; 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 setPosition; private final Trajectory path; private double startTime = 0; - public FollowPathCommand(Trajectory trajectory, Consumer setPosition) { + public FollowTrajectoryDemo(Trajectory trajectory, Consumer setPosition) { this.path = trajectory; this.setPosition = setPosition; } diff --git a/src/main/java/frc/robot/visualizers/FollowPathWithSideCommand.java b/src/main/java/frc/robot/visualizers/FollowPathWithSideCommand.java deleted file mode 100644 index a94aa6ea24..0000000000 --- a/src/main/java/frc/robot/visualizers/FollowPathWithSideCommand.java +++ /dev/null @@ -1,38 +0,0 @@ -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 deleted file mode 100644 index 63635b63eb..0000000000 --- a/src/main/java/frc/robot/visualizers/PathGenerator.java +++ /dev/null @@ -1,69 +0,0 @@ -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 straightLine(Translation2d start, Translation2d end) { - 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 Translation2d(x, y)); - } - - 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 275e97b25649c907467500b2f3a176abb28a163a Mon Sep 17 00:00:00 2001 From: YHerm <0549405422.yh@gmail.com> Date: Thu, 3 Apr 2025 10:20:55 +0300 Subject: [PATCH 2/3] need test --- .../doublejointedarm/ArmAngles.java | 5 ++ .../doublejointedarm/DoubleJointedArm.java | 30 +---------- .../DoubleJointedArmKinematics.java | 51 +++++++++++++++++++ 3 files changed, 57 insertions(+), 29 deletions(-) create mode 100644 src/main/java/frc/robot/structures/doublejointedarm/ArmAngles.java create mode 100644 src/main/java/frc/robot/structures/doublejointedarm/DoubleJointedArmKinematics.java diff --git a/src/main/java/frc/robot/structures/doublejointedarm/ArmAngles.java b/src/main/java/frc/robot/structures/doublejointedarm/ArmAngles.java new file mode 100644 index 0000000000..9df3494cf5 --- /dev/null +++ b/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 firstJoint, Rotation2d secondJoint) {} diff --git a/src/main/java/frc/robot/structures/doublejointedarm/DoubleJointedArm.java b/src/main/java/frc/robot/structures/doublejointedarm/DoubleJointedArm.java index d4afcd4a3c..e9597dbd32 100644 --- a/src/main/java/frc/robot/structures/doublejointedarm/DoubleJointedArm.java +++ b/src/main/java/frc/robot/structures/doublejointedarm/DoubleJointedArm.java @@ -59,26 +59,7 @@ public void setPosition(Translation2d positionMeters, boolean firstJointLeft) { } 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 theta2 = cosineLaw(Math.sqrt(x * x + y * y), FIRST_JOINT_LENGTH_METERS, SECOND_JOINT_LENGTH_METERS).getRadians(); - - 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)); + return DoubleJointedArmKinematics.toPositionMeters(new ArmAngles(firstJointAngle, secondJointAngle), FIRST_JOINT_LENGTH_METERS, SECOND_JOINT_LENGTH_METERS); } public void applyTestBinds(SmartJoystick joystick) { @@ -97,14 +78,5 @@ 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)); - } } diff --git a/src/main/java/frc/robot/structures/doublejointedarm/DoubleJointedArmKinematics.java b/src/main/java/frc/robot/structures/doublejointedarm/DoubleJointedArmKinematics.java new file mode 100644 index 0000000000..e2e6a249c4 --- /dev/null +++ b/src/main/java/frc/robot/structures/doublejointedarm/DoubleJointedArmKinematics.java @@ -0,0 +1,51 @@ +package frc.robot.structures.doublejointedarm; + +import edu.wpi.first.math.Pair; +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 firstJointLengthMeters, double secondJointLengthMeters) { + return new Translation2d( + firstJointLengthMeters * angles.firstJoint().getCos() + secondJointLengthMeters * angles.secondJoint().getCos(), + firstJointLengthMeters * angles.firstJoint().getSin() + secondJointLengthMeters * angles.secondJoint().getSin() + ); + } + + public static Optional toAngles(Translation2d positionMeters, double firstJointLengthMeters, double secondJointLengthMeters, boolean elbowLeft) { + double x = positionMeters.getX(); + double y = positionMeters.getY(); + + Optional theta2Optional = cosineLaw(Math.sqrt(x * x + y * y), firstJointLengthMeters, secondJointLengthMeters); + if (theta2Optional.isEmpty()) { + return Optional.empty(); + } + + double wristAngleRads = theta2Optional.get().getRadians(); + + if (elbowLeft) { + wristAngleRads = -wristAngleRads; + } + + double elbowAngleRads = Math.atan2(y, x) + - Math + .atan2(secondJointLengthMeters * Math.sin(wristAngleRads), firstJointLengthMeters + secondJointLengthMeters * Math.cos(wristAngleRads)); + + return Optional.of(new ArmAngles(Rotation2d.fromRadians(elbowAngleRads), Rotation2d.fromRadians(elbowAngleRads + wristAngleRads))); + } + + + public static Optional 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))); + } + +} From 46dc9b9ec2dd8de77732b582054f33805813958a Mon Sep 17 00:00:00 2001 From: YHerm <0549405422.yh@gmail.com> Date: Thu, 3 Apr 2025 16:51:45 +0300 Subject: [PATCH 3/3] Cleaned --- src/main/java/frc/JoysticksBindings.java | 2 +- src/main/java/frc/robot/Robot.java | 6 +- .../FollowTrajectoryDemo.java | 2 +- .../doublejointedarm/ArmAngles.java | 2 +- .../doublejointedarm/DoubleJointedArm.java | 60 +++++++++++-------- .../DoubleJointedArmKinematics.java | 29 +++++---- .../DoubleJointedArmVisualizer.java | 24 ++++---- 7 files changed, 69 insertions(+), 56 deletions(-) rename src/main/java/frc/robot/structures/{doublejointedarm => }/FollowTrajectoryDemo.java (95%) diff --git a/src/main/java/frc/JoysticksBindings.java b/src/main/java/frc/JoysticksBindings.java index 8769065f9f..7959d22098 100644 --- a/src/main/java/frc/JoysticksBindings.java +++ b/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.structures.doublejointedarm.FollowTrajectoryDemo; +import frc.robot.structures.FollowTrajectoryDemo; public class JoysticksBindings { diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 9d14ac4eec..9497d79695 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -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 diff --git a/src/main/java/frc/robot/structures/doublejointedarm/FollowTrajectoryDemo.java b/src/main/java/frc/robot/structures/FollowTrajectoryDemo.java similarity index 95% rename from src/main/java/frc/robot/structures/doublejointedarm/FollowTrajectoryDemo.java rename to src/main/java/frc/robot/structures/FollowTrajectoryDemo.java index 3008372264..4a50944e2f 100644 --- a/src/main/java/frc/robot/structures/doublejointedarm/FollowTrajectoryDemo.java +++ b/src/main/java/frc/robot/structures/FollowTrajectoryDemo.java @@ -1,4 +1,4 @@ -package frc.robot.structures.doublejointedarm; +package frc.robot.structures; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.trajectory.Trajectory; diff --git a/src/main/java/frc/robot/structures/doublejointedarm/ArmAngles.java b/src/main/java/frc/robot/structures/doublejointedarm/ArmAngles.java index 9df3494cf5..23dfe115af 100644 --- a/src/main/java/frc/robot/structures/doublejointedarm/ArmAngles.java +++ b/src/main/java/frc/robot/structures/doublejointedarm/ArmAngles.java @@ -2,4 +2,4 @@ import edu.wpi.first.math.geometry.Rotation2d; -public record ArmAngles(Rotation2d firstJoint, Rotation2d secondJoint) {} +public record ArmAngles(Rotation2d elbowAngle, Rotation2d wristAngle) {} diff --git a/src/main/java/frc/robot/structures/doublejointedarm/DoubleJointedArm.java b/src/main/java/frc/robot/structures/doublejointedarm/DoubleJointedArm.java index e9597dbd32..cf3c2e227a 100644 --- a/src/main/java/frc/robot/structures/doublejointedarm/DoubleJointedArm.java +++ b/src/main/java/frc/robot/structures/doublejointedarm/DoubleJointedArm.java @@ -1,21 +1,23 @@ package frc.robot.structures.doublejointedarm; -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; 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 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; + 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 firstJointAngle = new Rotation2d(); - private Rotation2d secondJointAngle = new Rotation2d(); + private Rotation2d elbowAngle = new Rotation2d(); + private Rotation2d wristAngle = new Rotation2d(); public DoubleJointedArm(String logPath) { super(logPath); @@ -23,43 +25,51 @@ public DoubleJointedArm(String logPath) { @Override protected void subsystemPeriodic() { - Logger.recordOutput(getLogPath() + "/FirstJointAngleDeg", firstJointAngle.getDegrees()); - Logger.recordOutput(getLogPath() + "/SecondJointAngleDeg", secondJointAngle.getDegrees()); + Logger.recordOutput(getLogPath() + "/ElbowAngleDeg", elbowAngle.getDegrees()); + Logger.recordOutput(getLogPath() + "/WristAngleDeg", wristAngle.getDegrees()); Logger.recordOutput(getLogPath() + "/PositionMeters", getPositionMeters()); } - public Rotation2d getFirstJointAngle() { - return firstJointAngle; + public Rotation2d getElbowAngle() { + return elbowAngle; } - public Rotation2d getSecondJointAngle() { - return secondJointAngle; + public Rotation2d getWristAngle() { + return wristAngle; } - public void setFirstJointAngle(Rotation2d firstJointAngle) { - this.firstJointAngle = firstJointAngle; + public void setElbowAngle(Rotation2d elbowAngle) { + this.elbowAngle = elbowAngle; } - public void setSecondJointAngle(Rotation2d secondJointAngle) { - this.secondJointAngle = secondJointAngle; + public void setWristAngle(Rotation2d wristAngle) { + this.wristAngle = wristAngle; } - public void setAngles(Rotation2d firstJointAngle, Rotation2d secondJointAngle) { - setFirstJointAngle(firstJointAngle); - setSecondJointAngle(secondJointAngle); + public void setAngles(Rotation2d elbowAngle, Rotation2d wristAngle) { + setElbowAngle(elbowAngle); + setWristAngle(wristAngle); } public Translation2d getPositionMeters() { - return toPositionMeters(firstJointAngle, secondJointAngle); + return toPositionMeters(elbowAngle, wristAngle); + } + + public void setPosition(Translation2d positionMeters, boolean isElbowLeft) { + Optional targetAngles = toAngles(positionMeters, isElbowLeft); + if (targetAngles.isEmpty()) { + new Alert(Alert.AlertType.ERROR, getLogPath() + "unreachable position").report(); + return; + } + setAngles(targetAngles.get().elbowAngle(), targetAngles.get().wristAngle()); } - public void setPosition(Translation2d positionMeters, boolean firstJointLeft) { - Pair targetAngles = toAngles(positionMeters, firstJointLeft); - setAngles(targetAngles.getFirst(), targetAngles.getSecond()); + private static Optional toAngles(Translation2d positionMeters, boolean isElbowLeft) { + return DoubleJointedArmKinematics.toAngles(positionMeters, ELBOW_LENGTH_METERS, WRIST_LENGTH_METERS, isElbowLeft); } - private static Translation2d toPositionMeters(Rotation2d firstJointAngle, Rotation2d secondJointAngle) { - return DoubleJointedArmKinematics.toPositionMeters(new ArmAngles(firstJointAngle, secondJointAngle), FIRST_JOINT_LENGTH_METERS, SECOND_JOINT_LENGTH_METERS); + 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) { diff --git a/src/main/java/frc/robot/structures/doublejointedarm/DoubleJointedArmKinematics.java b/src/main/java/frc/robot/structures/doublejointedarm/DoubleJointedArmKinematics.java index e2e6a249c4..01f7aada80 100644 --- a/src/main/java/frc/robot/structures/doublejointedarm/DoubleJointedArmKinematics.java +++ b/src/main/java/frc/robot/structures/doublejointedarm/DoubleJointedArmKinematics.java @@ -1,6 +1,5 @@ package frc.robot.structures.doublejointedarm; -import edu.wpi.first.math.Pair; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; @@ -8,31 +7,35 @@ public class DoubleJointedArmKinematics { - public static Translation2d toPositionMeters(ArmAngles angles, double firstJointLengthMeters, double secondJointLengthMeters) { + public static Translation2d toPositionMeters(ArmAngles angles, double elbowLengthMeters, double wristLengthMeters) { return new Translation2d( - firstJointLengthMeters * angles.firstJoint().getCos() + secondJointLengthMeters * angles.secondJoint().getCos(), - firstJointLengthMeters * angles.firstJoint().getSin() + secondJointLengthMeters * angles.secondJoint().getSin() + elbowLengthMeters * angles.elbowAngle().getCos() + wristLengthMeters * angles.wristAngle().getCos(), + elbowLengthMeters * angles.elbowAngle().getSin() + wristLengthMeters * angles.wristAngle().getSin() ); } - - public static Optional toAngles(Translation2d positionMeters, double firstJointLengthMeters, double secondJointLengthMeters, boolean elbowLeft) { + + public static Optional toAngles( + Translation2d positionMeters, + double elbowLengthMeters, + double wristLengthMeters, + boolean isElbowLeft + ) { double x = positionMeters.getX(); double y = positionMeters.getY(); - Optional theta2Optional = cosineLaw(Math.sqrt(x * x + y * y), firstJointLengthMeters, secondJointLengthMeters); - if (theta2Optional.isEmpty()) { + Optional wristAngleOptional = cosineLaw(Math.sqrt(x * x + y * y), elbowLengthMeters, wristLengthMeters); + if (wristAngleOptional.isEmpty()) { return Optional.empty(); } - - double wristAngleRads = theta2Optional.get().getRadians(); - if (elbowLeft) { + double wristAngleRads = wristAngleOptional.get().getRadians(); + + if (isElbowLeft) { wristAngleRads = -wristAngleRads; } double elbowAngleRads = Math.atan2(y, x) - - Math - .atan2(secondJointLengthMeters * Math.sin(wristAngleRads), firstJointLengthMeters + secondJointLengthMeters * Math.cos(wristAngleRads)); + - Math.atan2(wristLengthMeters * Math.sin(wristAngleRads), elbowLengthMeters + wristLengthMeters * Math.cos(wristAngleRads)); return Optional.of(new ArmAngles(Rotation2d.fromRadians(elbowAngleRads), Rotation2d.fromRadians(elbowAngleRads + wristAngleRads))); } diff --git a/src/main/java/frc/robot/structures/doublejointedarm/DoubleJointedArmVisualizer.java b/src/main/java/frc/robot/structures/doublejointedarm/DoubleJointedArmVisualizer.java index 20fe9d24df..fbb00c92e3 100644 --- a/src/main/java/frc/robot/structures/doublejointedarm/DoubleJointedArmVisualizer.java +++ b/src/main/java/frc/robot/structures/doublejointedarm/DoubleJointedArmVisualizer.java @@ -22,8 +22,8 @@ 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> paths = new ArrayList<>(); @@ -33,18 +33,18 @@ 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); @@ -53,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) { @@ -84,9 +84,9 @@ public void showPath(List path) { ); MechanismLigament2d mark = new MechanismLigament2d( pathCounter + ", " + i + " mark", - 0.01, 0, - 10.0F, + 0, + DEFAULT_LINE_WIDTH, new Color8Bit(Color.kBlueViolet) ); pathVisualize.add(mark);