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..205668bff9 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; @@ -10,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(); @@ -21,6 +26,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,16 +45,48 @@ 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() { + return toPositionMeters(firstJointAngle, secondJointAngle); + } + + 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(140), Rotation2d.fromDegrees(195)), false))); + joystick.POV_RIGHT + .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)), true))); + joystick.POV_UP + .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); } } diff --git a/src/main/java/frc/robot/visualizers/DoubleJointedArmVisualizer.java b/src/main/java/frc/robot/visualizers/DoubleJointedArmVisualizer.java index c73bf35a28..2dd9a1d24d 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,16 @@ 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; + private final Mechanism2d mechanism; private final MechanismRoot2d root; private final MechanismLigament2d firstJoint; private final MechanismLigament2d secondJoint; + private final MechanismRoot2d targetPosition; public DoubleJointedArmVisualizer( String name, @@ -23,12 +30,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.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); root.append(firstJoint); + targetPosition.append(TARGET_POSITION_MARK); + + showTargetPosition(false); + SmartDashboard.putData(name + " DoubleJointedArmMech2d", mechanism); } @@ -37,6 +50,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) { + TARGET_POSITION_MARK.setLength(show ? 0.01 : 0); + } + private static Rotation2d toFloorRelative(Rotation2d floorRelativeAngle, Rotation2d jointRelativeAngle) { return jointRelativeAngle.minus(floorRelativeAngle); }