From 199c0b8c59b378d6b68f6ab4dc9517cfc0ab5790 Mon Sep 17 00:00:00 2001 From: YHerm <0549405422.yh@gmail.com> Date: Thu, 3 Apr 2025 21:36:17 +0300 Subject: [PATCH 1/4] color of tarjectory --- .../structures/doublejointedarm/DoubleJointedArmVisualizer.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/structures/doublejointedarm/DoubleJointedArmVisualizer.java b/src/main/java/frc/robot/structures/doublejointedarm/DoubleJointedArmVisualizer.java index fbb00c92e3..28e2f1afcc 100644 --- a/src/main/java/frc/robot/structures/doublejointedarm/DoubleJointedArmVisualizer.java +++ b/src/main/java/frc/robot/structures/doublejointedarm/DoubleJointedArmVisualizer.java @@ -87,7 +87,7 @@ public void showPath(List path) { 0, 0, DEFAULT_LINE_WIDTH, - new Color8Bit(Color.kBlueViolet) + new Color8Bit(Color.kCyan) ); pathVisualize.add(mark); root.append(mark); From 1a81be73c122b01bb95f3a761da090854540f1a6 Mon Sep 17 00:00:00 2001 From: YHerm <0549405422.yh@gmail.com> Date: Thu, 3 Apr 2025 22:23:17 +0300 Subject: [PATCH 2/4] code --- .../structures/doublejointedarm/DoubleJointedArm.java | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/structures/doublejointedarm/DoubleJointedArm.java b/src/main/java/frc/robot/structures/doublejointedarm/DoubleJointedArm.java index ae80dc92ca..b006aa8790 100644 --- a/src/main/java/frc/robot/structures/doublejointedarm/DoubleJointedArm.java +++ b/src/main/java/frc/robot/structures/doublejointedarm/DoubleJointedArm.java @@ -89,16 +89,20 @@ public void setPosition(Translation2d positionMeters, boolean isElbowLeft) { } public void setPosition(Translation2d positionMeters) { - setPosition(positionMeters, isElbowLeft(positionMeters)); + setPosition(positionMeters, isElbowLeft(positionMeters, positionMeters.getX() > 0)); } - private boolean isElbowLeft(Translation2d positionMeters) { + public void setPosition(Translation2d positionMeters, Translation2d targetPositionMeters) { + setPosition(positionMeters, isElbowLeft(positionMeters, targetPositionMeters.getX() > 0)); + } + + private boolean isElbowLeft(Translation2d positionMeters, boolean autoPick) { double elbowRads = getElbowAngle().getRadians(); double positionRads = positionMeters.getAngle().getRadians(); double distanceFromStraightLineRads = Math.abs(MathUtil.angleModulus(elbowRads - positionRads)); boolean isLettingAutoPick = distanceFromStraightLineRads < SWITCH_DIRECTION_TOLERANCE.getRadians(); - return isLettingAutoPick ? positionMeters.getX() > 0 : elbowRads > positionRads; + return isLettingAutoPick ? autoPick : elbowRads > positionRads; } private Optional toAngles(Translation2d positionMeters, boolean isElbowLeft) { From 067b31317f019306c7dda430cd1b6df690fb3bfb Mon Sep 17 00:00:00 2001 From: YHerm <0549405422.yh@gmail.com> Date: Thu, 3 Apr 2025 23:17:13 +0300 Subject: [PATCH 3/4] poses --- .../doublejointedarm/DoubleJointedArm.java | 45 ++++++++++++++++--- 1 file changed, 40 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/structures/doublejointedarm/DoubleJointedArm.java b/src/main/java/frc/robot/structures/doublejointedarm/DoubleJointedArm.java index b006aa8790..919aca9a15 100644 --- a/src/main/java/frc/robot/structures/doublejointedarm/DoubleJointedArm.java +++ b/src/main/java/frc/robot/structures/doublejointedarm/DoubleJointedArm.java @@ -1,9 +1,14 @@ package frc.robot.structures.doublejointedarm; import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.trajectory.Trajectory; +import edu.wpi.first.math.trajectory.TrajectoryConfig; +import edu.wpi.first.math.trajectory.TrajectoryGenerator; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.DeferredCommand; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.WaitCommand; import frc.joysticks.Axis; @@ -14,13 +19,16 @@ import frc.utils.alerts.Alert; import org.littletonrobotics.junction.Logger; +import java.util.List; import java.util.Optional; +import java.util.Set; 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; + public static final double MAX_LENGTH_METERS = ELBOW_LENGTH_METERS + WRIST_LENGTH_METERS; + public static final double MIN_LENGTH_METERS = ELBOW_LENGTH_METERS - WRIST_LENGTH_METERS; public static final Rotation2d SWITCH_DIRECTION_TOLERANCE = Rotation2d.fromDegrees(3); private Rotation2d elbowAngle = new Rotation2d(); @@ -114,10 +122,10 @@ private Translation2d toPositionMeters(Rotation2d elbowAngle, Rotation2d wristAn } 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.A.onTrue(moveToPosition(new Translation2d(-0.5, 0.5))); + joystick.B.onTrue(moveToPosition(new Translation2d(-0.08, 1.2))); + joystick.X.onTrue(moveToPosition(new Translation2d(0.4, 0.1))); + joystick.Y.onTrue(moveToPosition(new Translation2d(0.8, 0.3))); joystick.POV_DOWN .onTrue(new InstantCommand(() -> setPosition(toPositionMeters(Rotation2d.fromDegrees(140), Rotation2d.fromDegrees(195)), false))); @@ -146,5 +154,32 @@ public void applyTestBinds(SmartJoystick joystick) { joystick.BACK.onTrue(new InstantCommand(() -> setPosition(new Translation2d(0, 1), true))); } + public Command moveToPosition(Translation2d targetPosition) { + return new DeferredCommand(() -> { + Translation2d currentPose = getPositionMeters(); + Rotation2d angle = targetPosition.minus(currentPose).getAngle(); + List midPoints = List.of(); + if (currentPose.getX() * targetPosition.getX() < 0) { + double x = (targetPosition.getX() + currentPose.getX()) / 2; + double y = (targetPosition.getY() + currentPose.getY()) / 2; + Rotation2d midAngle = Rotation2d.fromRadians(Math.atan2(y, x)); + double magnitude = Math.sqrt(x*x + y*y) > DoubleJointedArm.ELBOW_LENGTH_METERS ? MAX_LENGTH_METERS : MIN_LENGTH_METERS; + midPoints = List.of(new Translation2d(magnitude * midAngle.getCos(), magnitude * midAngle.getSin())); + } + if (currentPose.minus(targetPosition).getNorm() < 0.01) { + return new InstantCommand(); + } + Trajectory trajectory = TrajectoryGenerator.generateTrajectory( + new Pose2d(currentPose, angle), + midPoints, + new Pose2d(targetPosition, angle), + DEFAULT_TRAJECTORY_CONFIG + ); + setCurrentTrajectory(trajectory); + return new FollowTrajectoryDemo(trajectory, (position) -> setPosition(position, targetPosition)); + }, Set.of(this)); + } + + public static final TrajectoryConfig DEFAULT_TRAJECTORY_CONFIG = new TrajectoryConfig(2, 2); } From 6cdd10e75ff581c519d2a10229452c2365ede0e1 Mon Sep 17 00:00:00 2001 From: YHerm <0549405422.yh@gmail.com> Date: Mon, 7 Apr 2025 15:00:03 +0300 Subject: [PATCH 4/4] format --- .../structures/doublejointedarm/DoubleJointedArm.java | 10 +++------- 1 file changed, 3 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/structures/doublejointedarm/DoubleJointedArm.java b/src/main/java/frc/robot/structures/doublejointedarm/DoubleJointedArm.java index 919aca9a15..82cd6c6ec1 100644 --- a/src/main/java/frc/robot/structures/doublejointedarm/DoubleJointedArm.java +++ b/src/main/java/frc/robot/structures/doublejointedarm/DoubleJointedArm.java @@ -163,18 +163,14 @@ public Command moveToPosition(Translation2d targetPosition) { double x = (targetPosition.getX() + currentPose.getX()) / 2; double y = (targetPosition.getY() + currentPose.getY()) / 2; Rotation2d midAngle = Rotation2d.fromRadians(Math.atan2(y, x)); - double magnitude = Math.sqrt(x*x + y*y) > DoubleJointedArm.ELBOW_LENGTH_METERS ? MAX_LENGTH_METERS : MIN_LENGTH_METERS; + double magnitude = Math.sqrt(x * x + y * y) > DoubleJointedArm.ELBOW_LENGTH_METERS ? MAX_LENGTH_METERS : MIN_LENGTH_METERS; midPoints = List.of(new Translation2d(magnitude * midAngle.getCos(), magnitude * midAngle.getSin())); } if (currentPose.minus(targetPosition).getNorm() < 0.01) { return new InstantCommand(); } - Trajectory trajectory = TrajectoryGenerator.generateTrajectory( - new Pose2d(currentPose, angle), - midPoints, - new Pose2d(targetPosition, angle), - DEFAULT_TRAJECTORY_CONFIG - ); + Trajectory trajectory = TrajectoryGenerator + .generateTrajectory(new Pose2d(currentPose, angle), midPoints, new Pose2d(targetPosition, angle), DEFAULT_TRAJECTORY_CONFIG); setCurrentTrajectory(trajectory); return new FollowTrajectoryDemo(trajectory, (position) -> setPosition(position, targetPosition)); }, Set.of(this));