From d6112d705ced218516f049114b2dd2f0118aa321 Mon Sep 17 00:00:00 2001 From: YHerm <0549405422.yh@gmail.com> Date: Thu, 3 Apr 2025 18:26:43 +0300 Subject: [PATCH 1/4] working i guess --- src/main/java/frc/JoysticksBindings.java | 10 ++++-- src/main/java/frc/robot/Robot.java | 2 +- .../doublejointedarm/DoubleJointedArm.java | 31 ++++++++++++++++++- 3 files changed, 38 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/JoysticksBindings.java b/src/main/java/frc/JoysticksBindings.java index 7959d22098..8b83a9979f 100644 --- a/src/main/java/frc/JoysticksBindings.java +++ b/src/main/java/frc/JoysticksBindings.java @@ -1,5 +1,6 @@ package frc; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.WaitCommand; import frc.joysticks.JoystickPorts; @@ -32,17 +33,20 @@ 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 FollowTrajectoryDemo(Robot.pathDown, p -> robot.getArm().setPosition(p, true))) + .andThen(new FollowTrajectoryDemo(Robot.pathDown, p -> robot.getArm().setPosition(p))) ); usedJoystick.B.onTrue( new InstantCommand(() -> robot.getArmVisualizer().showPath(Robot.pathLeft.getStates())).andThen(new WaitCommand(0.3)) - .andThen(new FollowTrajectoryDemo(Robot.pathLeft, p -> robot.getArm().setPosition(p, false))) + .andThen(new FollowTrajectoryDemo(Robot.pathLeft, p -> robot.getArm().setPosition(p))) ); usedJoystick.Y.onTrue( new InstantCommand(() -> robot.getArmVisualizer().showPath(Robot.pathUp.getStates())).andThen(new WaitCommand(0.3)) - .andThen(new FollowTrajectoryDemo(Robot.pathUp, p -> robot.getArm().setPosition(p, false))) + .andThen(new FollowTrajectoryDemo(Robot.pathUp, p -> robot.getArm().setPosition(p))) ); + usedJoystick.START.onTrue(new InstantCommand(() -> robot.getArm().setPosition(new Translation2d(0, 1), false))); + usedJoystick.BACK.onTrue(new InstantCommand(() -> robot.getArm().setPosition(new Translation2d(0, 1), true))); + // List> flipArm = PathGenerator // .flipArm(Robot.pathLeft.get(Robot.pathLeft.size() - 1), false, DoubleJointedArm.TOTAL_LENGTH_METERS); // usedJoystick.X.onTrue( diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 9497d79695..4ead2b0ac2 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -55,7 +55,7 @@ public Robot() { pathLeft = TrajectoryGenerator.generateTrajectory( new Pose2d(new Translation2d(-0.4, 1), Rotation2d.fromDegrees(0)), - List.of(), + List.of(new Translation2d(0.1, 1.2)), new Pose2d(new Translation2d(0.4, 1), Rotation2d.fromDegrees(0)), new TrajectoryConfig(2, 2).setStartVelocity(0).setEndVelocity(0) ); diff --git a/src/main/java/frc/robot/structures/doublejointedarm/DoubleJointedArm.java b/src/main/java/frc/robot/structures/doublejointedarm/DoubleJointedArm.java index cf3c2e227a..f47c3af004 100644 --- a/src/main/java/frc/robot/structures/doublejointedarm/DoubleJointedArm.java +++ b/src/main/java/frc/robot/structures/doublejointedarm/DoubleJointedArm.java @@ -1,5 +1,6 @@ package frc.robot.structures.doublejointedarm; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj2.command.InstantCommand; @@ -64,7 +65,31 @@ public void setPosition(Translation2d positionMeters, boolean isElbowLeft) { setAngles(targetAngles.get().elbowAngle(), targetAngles.get().wristAngle()); } - private static Optional toAngles(Translation2d positionMeters, boolean isElbowLeft) { + public void setPosition(Translation2d positionMeters) { + Optional targetAngles = toAngles(positionMeters); + if (targetAngles.isEmpty()) { + new Alert(Alert.AlertType.ERROR, getLogPath() + "unreachable position").report(); + return; + } + setAngles(targetAngles.get().elbowAngle(), targetAngles.get().wristAngle()); + } + + private Optional toAngles(Translation2d positionMeters) { + return DoubleJointedArmKinematics.toAngles( + positionMeters, + ELBOW_LENGTH_METERS, + WRIST_LENGTH_METERS, + isGoingTowardsElbow(positionMeters) + ); + } + + private boolean isGoingTowardsElbow(Translation2d positionMeters) { + boolean isLettingAutoPick = + Math.abs(MathUtil.angleModulus(getElbowAngle().getRadians() - positionMeters.getAngle().getRadians())) < Rotation2d.fromDegrees(3).getRadians(); + return isLettingAutoPick ? positionMeters.getX() > 0 : getElbowAngle().getRadians() > positionMeters.getAngle().getRadians(); + } + + private Optional toAngles(Translation2d positionMeters, boolean isElbowLeft) { return DoubleJointedArmKinematics.toAngles(positionMeters, ELBOW_LENGTH_METERS, WRIST_LENGTH_METERS, isElbowLeft); } @@ -72,6 +97,10 @@ private static Translation2d toPositionMeters(Rotation2d elbowAngle, Rotation2d return DoubleJointedArmKinematics.toPositionMeters(new ArmAngles(elbowAngle, wristAngle), ELBOW_LENGTH_METERS, WRIST_LENGTH_METERS); } + private static Rotation2d toElbowRelative(Rotation2d elbowAngle, Rotation2d wristAngle) { + return elbowAngle.minus(wristAngle); + } + 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)))); From c838d35f26b8682d0d4d8d790bfec0a8c5924cfe Mon Sep 17 00:00:00 2001 From: YHerm <0549405422.yh@gmail.com> Date: Thu, 3 Apr 2025 18:34:34 +0300 Subject: [PATCH 2/4] Clean --- .../doublejointedarm/DoubleJointedArm.java | 29 ++++++++----------- 1 file changed, 12 insertions(+), 17 deletions(-) diff --git a/src/main/java/frc/robot/structures/doublejointedarm/DoubleJointedArm.java b/src/main/java/frc/robot/structures/doublejointedarm/DoubleJointedArm.java index f47c3af004..0abdf4ba1e 100644 --- a/src/main/java/frc/robot/structures/doublejointedarm/DoubleJointedArm.java +++ b/src/main/java/frc/robot/structures/doublejointedarm/DoubleJointedArm.java @@ -16,6 +16,7 @@ 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 Rotation2d SWITCH_DIRECTION_TOLERANCE = Rotation2d.fromDegrees(3); private Rotation2d elbowAngle = new Rotation2d(); private Rotation2d wristAngle = new Rotation2d(); @@ -65,6 +66,10 @@ public void setPosition(Translation2d positionMeters, boolean isElbowLeft) { setAngles(targetAngles.get().elbowAngle(), targetAngles.get().wristAngle()); } + private Optional toAngles(Translation2d positionMeters, boolean isElbowLeft) { + return DoubleJointedArmKinematics.toAngles(positionMeters, ELBOW_LENGTH_METERS, WRIST_LENGTH_METERS, isElbowLeft); + } + public void setPosition(Translation2d positionMeters) { Optional targetAngles = toAngles(positionMeters); if (targetAngles.isEmpty()) { @@ -75,32 +80,22 @@ public void setPosition(Translation2d positionMeters) { } private Optional toAngles(Translation2d positionMeters) { - return DoubleJointedArmKinematics.toAngles( - positionMeters, - ELBOW_LENGTH_METERS, - WRIST_LENGTH_METERS, - isGoingTowardsElbow(positionMeters) - ); + return DoubleJointedArmKinematics.toAngles(positionMeters, ELBOW_LENGTH_METERS, WRIST_LENGTH_METERS, isElbowLeft(positionMeters)); } - private boolean isGoingTowardsElbow(Translation2d positionMeters) { - boolean isLettingAutoPick = - Math.abs(MathUtil.angleModulus(getElbowAngle().getRadians() - positionMeters.getAngle().getRadians())) < Rotation2d.fromDegrees(3).getRadians(); - return isLettingAutoPick ? positionMeters.getX() > 0 : getElbowAngle().getRadians() > positionMeters.getAngle().getRadians(); - } + private boolean isElbowLeft(Translation2d positionMeters) { + double elbowRads = getElbowAngle().getRadians(); + double positionRads = positionMeters.getAngle().getRadians(); + double distanceFromStraightLineRads = Math.abs(MathUtil.angleModulus(elbowRads - positionRads)); - private Optional toAngles(Translation2d positionMeters, boolean isElbowLeft) { - return DoubleJointedArmKinematics.toAngles(positionMeters, ELBOW_LENGTH_METERS, WRIST_LENGTH_METERS, isElbowLeft); + boolean isLettingAutoPick = distanceFromStraightLineRads < SWITCH_DIRECTION_TOLERANCE.getRadians(); + return isLettingAutoPick ? positionMeters.getX() > 0 : elbowRads > positionRads; } private static Translation2d toPositionMeters(Rotation2d elbowAngle, Rotation2d wristAngle) { return DoubleJointedArmKinematics.toPositionMeters(new ArmAngles(elbowAngle, wristAngle), ELBOW_LENGTH_METERS, WRIST_LENGTH_METERS); } - private static Rotation2d toElbowRelative(Rotation2d elbowAngle, Rotation2d wristAngle) { - return elbowAngle.minus(wristAngle); - } - 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)))); From fb37f81dd90bf341ad7f0a78a6116ab3af455e40 Mon Sep 17 00:00:00 2001 From: YHerm <0549405422.yh@gmail.com> Date: Thu, 3 Apr 2025 18:38:12 +0300 Subject: [PATCH 3/4] Clean --- .../doublejointedarm/DoubleJointedArm.java | 21 ++++++------------- 1 file changed, 6 insertions(+), 15 deletions(-) diff --git a/src/main/java/frc/robot/structures/doublejointedarm/DoubleJointedArm.java b/src/main/java/frc/robot/structures/doublejointedarm/DoubleJointedArm.java index 0abdf4ba1e..ebf946b3c0 100644 --- a/src/main/java/frc/robot/structures/doublejointedarm/DoubleJointedArm.java +++ b/src/main/java/frc/robot/structures/doublejointedarm/DoubleJointedArm.java @@ -66,21 +66,8 @@ public void setPosition(Translation2d positionMeters, boolean isElbowLeft) { setAngles(targetAngles.get().elbowAngle(), targetAngles.get().wristAngle()); } - private Optional toAngles(Translation2d positionMeters, boolean isElbowLeft) { - return DoubleJointedArmKinematics.toAngles(positionMeters, ELBOW_LENGTH_METERS, WRIST_LENGTH_METERS, isElbowLeft); - } - public void setPosition(Translation2d positionMeters) { - Optional targetAngles = toAngles(positionMeters); - if (targetAngles.isEmpty()) { - new Alert(Alert.AlertType.ERROR, getLogPath() + "unreachable position").report(); - return; - } - setAngles(targetAngles.get().elbowAngle(), targetAngles.get().wristAngle()); - } - - private Optional toAngles(Translation2d positionMeters) { - return DoubleJointedArmKinematics.toAngles(positionMeters, ELBOW_LENGTH_METERS, WRIST_LENGTH_METERS, isElbowLeft(positionMeters)); + setPosition(positionMeters, isElbowLeft(positionMeters)); } private boolean isElbowLeft(Translation2d positionMeters) { @@ -92,7 +79,11 @@ private boolean isElbowLeft(Translation2d positionMeters) { return isLettingAutoPick ? positionMeters.getX() > 0 : elbowRads > positionRads; } - private static Translation2d toPositionMeters(Rotation2d elbowAngle, Rotation2d wristAngle) { + private Optional toAngles(Translation2d positionMeters, boolean isElbowLeft) { + return DoubleJointedArmKinematics.toAngles(positionMeters, ELBOW_LENGTH_METERS, WRIST_LENGTH_METERS, isElbowLeft); + } + + private Translation2d toPositionMeters(Rotation2d elbowAngle, Rotation2d wristAngle) { return DoubleJointedArmKinematics.toPositionMeters(new ArmAngles(elbowAngle, wristAngle), ELBOW_LENGTH_METERS, WRIST_LENGTH_METERS); } From 7c86cfe1cc300e9e16595fb26e9db00954487f4f Mon Sep 17 00:00:00 2001 From: YHerm <0549405422.yh@gmail.com> Date: Thu, 3 Apr 2025 18:46:18 +0300 Subject: [PATCH 4/4] save trajectory in arm --- src/main/java/frc/JoysticksBindings.java | 29 +------------- src/main/java/frc/robot/Robot.java | 3 ++ .../doublejointedarm/DoubleJointedArm.java | 39 +++++++++++++++++++ 3 files changed, 43 insertions(+), 28 deletions(-) diff --git a/src/main/java/frc/JoysticksBindings.java b/src/main/java/frc/JoysticksBindings.java index 8b83a9979f..90d19baecf 100644 --- a/src/main/java/frc/JoysticksBindings.java +++ b/src/main/java/frc/JoysticksBindings.java @@ -1,12 +1,8 @@ package frc; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.WaitCommand; import frc.joysticks.JoystickPorts; import frc.joysticks.SmartJoystick; import frc.robot.Robot; -import frc.robot.structures.FollowTrajectoryDemo; public class JoysticksBindings { @@ -30,30 +26,7 @@ public static void configureBindings(Robot robot) { private static void mainJoystickButtons(Robot robot) { SmartJoystick usedJoystick = MAIN_JOYSTICK; // bindings... -// robot.getArm().applyTestBinds(usedJoystick); - usedJoystick.A.onTrue( - new InstantCommand(() -> robot.getArmVisualizer().showPath(Robot.pathDown.getStates())).andThen(new WaitCommand(0.3)) - .andThen(new FollowTrajectoryDemo(Robot.pathDown, p -> robot.getArm().setPosition(p))) - ); - usedJoystick.B.onTrue( - new InstantCommand(() -> robot.getArmVisualizer().showPath(Robot.pathLeft.getStates())).andThen(new WaitCommand(0.3)) - .andThen(new FollowTrajectoryDemo(Robot.pathLeft, p -> robot.getArm().setPosition(p))) - ); - usedJoystick.Y.onTrue( - new InstantCommand(() -> robot.getArmVisualizer().showPath(Robot.pathUp.getStates())).andThen(new WaitCommand(0.3)) - .andThen(new FollowTrajectoryDemo(Robot.pathUp, p -> robot.getArm().setPosition(p))) - ); - - usedJoystick.START.onTrue(new InstantCommand(() -> robot.getArm().setPosition(new Translation2d(0, 1), false))); - usedJoystick.BACK.onTrue(new InstantCommand(() -> robot.getArm().setPosition(new Translation2d(0, 1), true))); - -// List> flipArm = PathGenerator -// .flipArm(Robot.pathLeft.get(Robot.pathLeft.size() - 1), false, DoubleJointedArm.TOTAL_LENGTH_METERS); -// usedJoystick.X.onTrue( -// new InstantCommand(() -> robot.getArmVisualizer().showPath(PathGenerator.createTranslation2dList(flipArm))) -// .andThen(new WaitCommand(0.3)) -// .andThen(new FollowPathWithSideCommand(flipArm, p -> robot.getArm().setPosition(p.getFirst(), p.getSecond()))) -// ); + robot.getArm().applyTestBinds(usedJoystick); } private static void secondJoystickButtons(Robot robot) { diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 4ead2b0ac2..6cf642624f 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -73,6 +73,9 @@ public void periodic() { armVisualizer.setAngles(arm.getElbowAngle(), arm.getWristAngle()); armVisualizer.setTargetPositionMeters(arm.getPositionMeters()); + if (arm.hasTrajectoryChanged()) { + armVisualizer.showPath(arm.getCurrentTrajectory().getStates()); + } CommandScheduler.getInstance().run(); // Should be last } diff --git a/src/main/java/frc/robot/structures/doublejointedarm/DoubleJointedArm.java b/src/main/java/frc/robot/structures/doublejointedarm/DoubleJointedArm.java index ebf946b3c0..ae80dc92ca 100644 --- a/src/main/java/frc/robot/structures/doublejointedarm/DoubleJointedArm.java +++ b/src/main/java/frc/robot/structures/doublejointedarm/DoubleJointedArm.java @@ -3,8 +3,13 @@ import edu.wpi.first.math.MathUtil; 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.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import frc.joysticks.Axis; import frc.joysticks.SmartJoystick; +import frc.robot.Robot; +import frc.robot.structures.FollowTrajectoryDemo; import frc.robot.subsystems.GBSubsystem; import frc.utils.alerts.Alert; import org.littletonrobotics.junction.Logger; @@ -20,11 +25,28 @@ public class DoubleJointedArm extends GBSubsystem { private Rotation2d elbowAngle = new Rotation2d(); private Rotation2d wristAngle = new Rotation2d(); + private Trajectory currentTrajectory = null; + private boolean hasTrajectoryChanged = false; public DoubleJointedArm(String logPath) { super(logPath); } + public boolean hasTrajectoryChanged() { + boolean check = hasTrajectoryChanged; + hasTrajectoryChanged = false; + return check; + } + + public Trajectory getCurrentTrajectory() { + return currentTrajectory; + } + + public void setCurrentTrajectory(Trajectory currentTrajectory) { + hasTrajectoryChanged = true; + this.currentTrajectory = currentTrajectory; + } + @Override protected void subsystemPeriodic() { Logger.recordOutput(getLogPath() + "/ElbowAngleDeg", elbowAngle.getDegrees()); @@ -101,6 +123,23 @@ public void applyTestBinds(SmartJoystick joystick) { .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))); + + joystick.R1.onTrue( + new InstantCommand(() -> setCurrentTrajectory(Robot.pathDown)).andThen(new WaitCommand(0.3)) + .andThen(new FollowTrajectoryDemo(Robot.pathDown, this::setPosition)) + ); + joystick.getAxisAsButton(Axis.LEFT_TRIGGER) + .onTrue( + new InstantCommand(() -> setCurrentTrajectory(Robot.pathLeft)).andThen(new WaitCommand(0.3)) + .andThen(new FollowTrajectoryDemo(Robot.pathLeft, this::setPosition)) + ); + joystick.L1.onTrue( + new InstantCommand(() -> setCurrentTrajectory(Robot.pathUp)).andThen(new WaitCommand(0.3)) + .andThen(new FollowTrajectoryDemo(Robot.pathUp, this::setPosition)) + ); + + joystick.START.onTrue(new InstantCommand(() -> setPosition(new Translation2d(0, 1), false))); + joystick.BACK.onTrue(new InstantCommand(() -> setPosition(new Translation2d(0, 1), true))); }