From 3618c7cecafb77cd6d8c04a1aac372008465e5d3 Mon Sep 17 00:00:00 2001 From: YHerm <0549405422.yh@gmail.com> Date: Tue, 1 Apr 2025 16:53:15 +0300 Subject: [PATCH 1/4] added target position, angles to position --- src/main/java/frc/JoysticksBindings.java | 1 + src/main/java/frc/robot/Robot.java | 1 + .../robot/subsystems/DoubleJointedArm.java | 14 ++++++++++--- .../DoubleJointedArmVisualizer.java | 20 +++++++++++++++++++ 4 files changed, 33 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/JoysticksBindings.java b/src/main/java/frc/JoysticksBindings.java index 0fe5ec2b70..1c2f5ceeee 100644 --- a/src/main/java/frc/JoysticksBindings.java +++ b/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) { diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 20dc0c7f11..c7c909758e 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/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 } diff --git a/src/main/java/frc/robot/subsystems/DoubleJointedArm.java b/src/main/java/frc/robot/subsystems/DoubleJointedArm.java index 2abd1bddd2..e022822abd 100644 --- a/src/main/java/frc/robot/subsystems/DoubleJointedArm.java +++ b/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; @@ -21,6 +22,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,9 +41,15 @@ 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() { + 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); } public void applyTestBinds(SmartJoystick joystick) { diff --git a/src/main/java/frc/robot/visualizers/DoubleJointedArmVisualizer.java b/src/main/java/frc/robot/visualizers/DoubleJointedArmVisualizer.java index c73bf35a28..fbae91a824 100644 --- a/src/main/java/frc/robot/visualizers/DoubleJointedArmVisualizer.java +++ b/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,14 @@ public class DoubleJointedArmVisualizer { + private final Translation2d armRootPosition; + private final Mechanism2d mechanism; private final MechanismRoot2d root; private final MechanismLigament2d firstJoint; private final MechanismLigament2d secondJoint; + private final MechanismRoot2d targetPosition; + private MechanismLigament2d targetPositionMark = new MechanismLigament2d("mark", 0.03, 0, 10.0F, new Color8Bit(Color.kGreen)); public DoubleJointedArmVisualizer( String name, @@ -23,12 +28,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.targetPosition = mechanism.getRoot(name + " TargetPosition", 0, 0); firstJoint.append(secondJoint); root.append(firstJoint); + targetPosition.append(targetPositionMark); + + showTargetPosition(false); + SmartDashboard.putData(name + " DoubleJointedArmMech2d", mechanism); } @@ -37,6 +48,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) { + targetPositionMark.setLength(show ? 0.01 : 0); + } + private static Rotation2d toFloorRelative(Rotation2d floorRelativeAngle, Rotation2d jointRelativeAngle) { return jointRelativeAngle.minus(floorRelativeAngle); } From 65f1f93cd6312cdfc14052057e6a3d96d1668de7 Mon Sep 17 00:00:00 2001 From: YHerm <0549405422.yh@gmail.com> Date: Tue, 1 Apr 2025 17:09:55 +0300 Subject: [PATCH 2/4] working kinda --- .../robot/subsystems/DoubleJointedArm.java | 31 +++++++++++++++++-- .../DoubleJointedArmVisualizer.java | 7 +++-- 2 files changed, 32 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/DoubleJointedArm.java b/src/main/java/frc/robot/subsystems/DoubleJointedArm.java index e022822abd..081998dfbb 100644 --- a/src/main/java/frc/robot/subsystems/DoubleJointedArm.java +++ b/src/main/java/frc/robot/subsystems/DoubleJointedArm.java @@ -11,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(); @@ -47,9 +51,16 @@ public void setAngles(Rotation2d firstJointAngle, Rotation2d secondJointAngle) { } public Translation2d getPositionMeters() { - 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); + return toPositionMeters(firstJointAngle, secondJointAngle); + } + + public void setPosition(Translation2d positionMeters) { + 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); + + setAngles(Rotation2d.fromRadians(alphaRads + omegaRads), Rotation2d.fromRadians(alphaRads - omegaRads)); } public void applyTestBinds(SmartJoystick joystick) { @@ -57,6 +68,20 @@ public void applyTestBinds(SmartJoystick joystick) { joystick.B.onTrue(new InstantCommand(() -> setAngles(Rotation2d.fromDegrees(60), Rotation2d.fromDegrees(80)))); 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.POV_DOWN + .onTrue(new InstantCommand(() -> setPosition(toPositionMeters(Rotation2d.fromDegrees(100), Rotation2d.fromDegrees(210))))); + joystick.POV_RIGHT + .onTrue(new InstantCommand(() -> setPosition(toPositionMeters(Rotation2d.fromDegrees(60), Rotation2d.fromDegrees(80))))); + joystick.POV_LEFT + .onTrue(new InstantCommand(() -> setPosition(toPositionMeters(Rotation2d.fromDegrees(150), Rotation2d.fromDegrees(75))))); + joystick.POV_UP.onTrue(new InstantCommand(() -> setPosition(toPositionMeters(Rotation2d.fromDegrees(95), Rotation2d.fromDegrees(85))))); + } + + 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); } } diff --git a/src/main/java/frc/robot/visualizers/DoubleJointedArmVisualizer.java b/src/main/java/frc/robot/visualizers/DoubleJointedArmVisualizer.java index fbae91a824..1c3bcb3455 100644 --- a/src/main/java/frc/robot/visualizers/DoubleJointedArmVisualizer.java +++ b/src/main/java/frc/robot/visualizers/DoubleJointedArmVisualizer.java @@ -11,6 +11,8 @@ public class DoubleJointedArmVisualizer { + private static final MechanismLigament2d TARGET_POSITION_MARK = new MechanismLigament2d("mark", 0.03, 0, 10.0F, new Color8Bit(Color.kGreen)); + private final Translation2d armRootPosition; private final Mechanism2d mechanism; @@ -18,7 +20,6 @@ public class DoubleJointedArmVisualizer { private final MechanismLigament2d firstJoint; private final MechanismLigament2d secondJoint; private final MechanismRoot2d targetPosition; - private MechanismLigament2d targetPositionMark = new MechanismLigament2d("mark", 0.03, 0, 10.0F, new Color8Bit(Color.kGreen)); public DoubleJointedArmVisualizer( String name, @@ -36,7 +37,7 @@ public DoubleJointedArmVisualizer( firstJoint.append(secondJoint); root.append(firstJoint); - targetPosition.append(targetPositionMark); + targetPosition.append(TARGET_POSITION_MARK); showTargetPosition(false); @@ -54,7 +55,7 @@ public void setTargetPositionMeters(Translation2d positionMeters) { } public void showTargetPosition(boolean show) { - targetPositionMark.setLength(show ? 0.01 : 0); + TARGET_POSITION_MARK.setLength(show ? 0.01 : 0); } private static Rotation2d toFloorRelative(Rotation2d floorRelativeAngle, Rotation2d jointRelativeAngle) { From 91e358fc5cc462f03f00c991e1a1507dbdd24fcd Mon Sep 17 00:00:00 2001 From: YHerm <0549405422.yh@gmail.com> Date: Tue, 1 Apr 2025 17:30:07 +0300 Subject: [PATCH 3/4] added angles option pick --- .../robot/subsystems/DoubleJointedArm.java | 21 ++++++++++++------- 1 file changed, 13 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/DoubleJointedArm.java b/src/main/java/frc/robot/subsystems/DoubleJointedArm.java index 081998dfbb..205668bff9 100644 --- a/src/main/java/frc/robot/subsystems/DoubleJointedArm.java +++ b/src/main/java/frc/robot/subsystems/DoubleJointedArm.java @@ -54,28 +54,33 @@ public Translation2d getPositionMeters() { return toPositionMeters(firstJointAngle, secondJointAngle); } - public void setPosition(Translation2d positionMeters) { + 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(100), Rotation2d.fromDegrees(210))))); + .onTrue(new InstantCommand(() -> setPosition(toPositionMeters(Rotation2d.fromDegrees(140), Rotation2d.fromDegrees(195)), false))); joystick.POV_RIGHT - .onTrue(new InstantCommand(() -> setPosition(toPositionMeters(Rotation2d.fromDegrees(60), Rotation2d.fromDegrees(80))))); + .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))))); - joystick.POV_UP.onTrue(new InstantCommand(() -> setPosition(toPositionMeters(Rotation2d.fromDegrees(95), Rotation2d.fromDegrees(85))))); + .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) { From 72f32c23bd89c7121755c5e94f8b92d1c375ce38 Mon Sep 17 00:00:00 2001 From: YHerm <0549405422.yh@gmail.com> Date: Tue, 1 Apr 2025 17:35:50 +0300 Subject: [PATCH 4/4] clean --- .../java/frc/robot/visualizers/DoubleJointedArmVisualizer.java | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/visualizers/DoubleJointedArmVisualizer.java b/src/main/java/frc/robot/visualizers/DoubleJointedArmVisualizer.java index 1c3bcb3455..2dd9a1d24d 100644 --- a/src/main/java/frc/robot/visualizers/DoubleJointedArmVisualizer.java +++ b/src/main/java/frc/robot/visualizers/DoubleJointedArmVisualizer.java @@ -12,6 +12,7 @@ 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; @@ -32,7 +33,7 @@ public DoubleJointedArmVisualizer( 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);