diff --git a/src/main/java/frc/JoysticksBindings.java b/src/main/java/frc/JoysticksBindings.java index 1c2f5ceeee..9fbf98459c 100644 --- a/src/main/java/frc/JoysticksBindings.java +++ b/src/main/java/frc/JoysticksBindings.java @@ -1,8 +1,18 @@ package frc; +import edu.wpi.first.math.Pair; +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.subsystems.DoubleJointedArm; +import frc.robot.visualizers.FollowPathCommand; +import frc.robot.visualizers.FollowPathWithSideCommand; +import frc.robot.visualizers.PathGenerator; + +import java.util.List; public class JoysticksBindings { @@ -25,7 +35,27 @@ public static void configureBindings(Robot robot) { private static void mainJoystickButtons(Robot robot) { SmartJoystick usedJoystick = MAIN_JOYSTICK; // bindings... - robot.getArm().applyTestBinds(usedJoystick); +// robot.getArm().applyTestBinds(usedJoystick); + usedJoystick.A.onTrue( + new InstantCommand(() -> robot.getArmVisualizer().showPath(Robot.pathDown)).andThen(new WaitCommand(0.3)) + .andThen(new FollowPathCommand(Robot.pathDown, p -> robot.getArm().setPosition(p, true))) + ); + usedJoystick.B.onTrue( + new InstantCommand(() -> robot.getArmVisualizer().showPath(Robot.pathLeft)).andThen(new WaitCommand(0.3)) + .andThen(new FollowPathCommand(Robot.pathLeft, p -> robot.getArm().setPosition(p, false))) + ); + usedJoystick.Y.onTrue( + new InstantCommand(() -> robot.getArmVisualizer().showPath(Robot.pathUp)).andThen(new WaitCommand(0.3)) + .andThen(new FollowPathCommand(Robot.pathUp, p -> robot.getArm().setPosition(p, false))) + ); + + 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()))) + ); } 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 c7c909758e..ba7bb6da02 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -4,6 +4,7 @@ package frc.robot; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.InstantCommand; @@ -11,8 +12,11 @@ import frc.robot.hardware.phoenix6.BusChain; import frc.robot.subsystems.DoubleJointedArm; import frc.robot.visualizers.DoubleJointedArmVisualizer; +import frc.robot.visualizers.PathGenerator; import frc.utils.battery.BatteryUtil; +import java.util.List; + /** * This class is where the bulk of the robot should be declared. Since Command-based is a "declarative" paradigm, very little robot logic should * actually be handled in the {@link RobotManager} periodic methods (other than the scheduler calls). Instead, the structure of the robot @@ -31,8 +35,16 @@ public class Robot { DoubleJointedArm.SECOND_JOINT_LENGTH_METERS ); + public static List pathUp; + public static List pathLeft; + public static List pathDown; + public Robot() { BatteryUtil.scheduleLimiter(); + + pathUp = PathGenerator.straightLine(new Translation2d(-0.4, 0.05), new Translation2d(-0.4, 1)); + pathLeft = PathGenerator.straightLine(new Translation2d(-0.4, 1), new Translation2d(0.4, 1)); + pathDown = PathGenerator.straightLine(new Translation2d(0.4, 1), new Translation2d(0.4, 0.05)); } public void periodic() { @@ -53,4 +65,8 @@ public DoubleJointedArm getArm() { return arm; } + public DoubleJointedArmVisualizer getArmVisualizer() { + return armVisualizer; + } + } diff --git a/src/main/java/frc/robot/subsystems/DoubleJointedArm.java b/src/main/java/frc/robot/subsystems/DoubleJointedArm.java index 351c375ceb..7c83ae2fa9 100644 --- a/src/main/java/frc/robot/subsystems/DoubleJointedArm.java +++ b/src/main/java/frc/robot/subsystems/DoubleJointedArm.java @@ -11,6 +11,7 @@ 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; private Rotation2d firstJointAngle = new Rotation2d(); private Rotation2d secondJointAngle = new Rotation2d(); diff --git a/src/main/java/frc/robot/visualizers/DoubleJointedArmVisualizer.java b/src/main/java/frc/robot/visualizers/DoubleJointedArmVisualizer.java index f594cf8c93..d9f05d22ff 100644 --- a/src/main/java/frc/robot/visualizers/DoubleJointedArmVisualizer.java +++ b/src/main/java/frc/robot/visualizers/DoubleJointedArmVisualizer.java @@ -9,6 +9,9 @@ import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj.util.Color8Bit; +import java.util.ArrayList; +import java.util.List; + public class DoubleJointedArmVisualizer { private static final MechanismLigament2d TARGET_POSITION_MARK = new MechanismLigament2d("mark", 0.01, 0, 10.0F, new Color8Bit(Color.kGreen)); @@ -21,6 +24,7 @@ public class DoubleJointedArmVisualizer { private final MechanismLigament2d firstJoint; private final MechanismLigament2d secondJoint; private final MechanismRoot2d targetPosition; + private final ArrayList> paths = new ArrayList<>(); public DoubleJointedArmVisualizer( String name, @@ -56,7 +60,30 @@ public void setTargetPositionMeters(Translation2d positionMeters) { } public void showTargetPosition(boolean show) { - TARGET_POSITION_MARK.setLength(show ? 0.01 : 0); + TARGET_POSITION_MARK.setLineWeight(show ? DEFAULT_LINE_WIDTH : 0); + } + + public void showPath(List path) { + ArrayList pathVisualize = new ArrayList<>(path.size()); + for (int i = 0; i < path.size(); i++) { + MechanismRoot2d root = mechanism + .getRoot(paths.size() + ", " + i, path.get(i).getX() + armRootPosition.getX(), path.get(i).getY() + armRootPosition.getY()); + MechanismLigament2d mark = new MechanismLigament2d( + paths.size() + ", " + i + " mark", + 0.01, + 0, + 10.0F, + new Color8Bit(Color.kBlueViolet) + ); + 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); } 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/visualizers/FollowPathCommand.java new file mode 100644 index 0000000000..f323ed589d --- /dev/null +++ b/src/main/java/frc/robot/visualizers/FollowPathCommand.java @@ -0,0 +1,37 @@ +package frc.robot.visualizers; + +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 FollowPathCommand extends Command { + + private final Consumer setPosition; + private final List path; + + private int currentPoint = 0; + + public FollowPathCommand(List path, Consumer setPosition) { + this.path = path; + this.setPosition = setPosition; + } + + @Override + public void initialize() { + currentPoint = 0; + } + + @Override + public void execute() { + setPosition.accept(path.get(currentPoint)); + currentPoint++; + } + + @Override + public boolean isFinished() { + return currentPoint >= path.size(); + } + +} diff --git a/src/main/java/frc/robot/visualizers/FollowPathWithSideCommand.java b/src/main/java/frc/robot/visualizers/FollowPathWithSideCommand.java new file mode 100644 index 0000000000..a94aa6ea24 --- /dev/null +++ b/src/main/java/frc/robot/visualizers/FollowPathWithSideCommand.java @@ -0,0 +1,38 @@ +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 new file mode 100644 index 0000000000..63635b63eb --- /dev/null +++ b/src/main/java/frc/robot/visualizers/PathGenerator.java @@ -0,0 +1,69 @@ +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; + } + +}