diff --git a/src/main/java/frc/JoysticksBindings.java b/src/main/java/frc/JoysticksBindings.java index 9fbf98459c..77408e0d7d 100644 --- a/src/main/java/frc/JoysticksBindings.java +++ b/src/main/java/frc/JoysticksBindings.java @@ -1,18 +1,12 @@ 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 { @@ -37,25 +31,25 @@ private static void mainJoystickButtons(Robot robot) { // bindings... // robot.getArm().applyTestBinds(usedJoystick); usedJoystick.A.onTrue( - new InstantCommand(() -> robot.getArmVisualizer().showPath(Robot.pathDown)).andThen(new WaitCommand(0.3)) + new InstantCommand(() -> robot.getArmVisualizer().showPath(Robot.pathDown.getStates())).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)) + new InstantCommand(() -> robot.getArmVisualizer().showPath(Robot.pathLeft.getStates())).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)) + new InstantCommand(() -> robot.getArmVisualizer().showPath(Robot.pathUp.getStates())).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()))) - ); +// 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 ba7bb6da02..4272051a71 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -4,7 +4,12 @@ package frc.robot; +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.CommandScheduler; import edu.wpi.first.wpilibj2.command.InstantCommand; @@ -12,7 +17,6 @@ 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; @@ -35,16 +39,32 @@ public class Robot { DoubleJointedArm.SECOND_JOINT_LENGTH_METERS ); - public static List pathUp; - public static List pathLeft; - public static List pathDown; + public static Trajectory pathUp; + public static Trajectory pathLeft; + public static Trajectory 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)); + pathUp = TrajectoryGenerator.generateTrajectory( + new Pose2d(new Translation2d(-0.4, 0.05), Rotation2d.fromDegrees(90)), + List.of(), + new Pose2d(new Translation2d(-0.4, 1), Rotation2d.fromDegrees(90)), + new TrajectoryConfig(2, 2).setStartVelocity(0).setEndVelocity(0) + ); + + pathLeft = TrajectoryGenerator.generateTrajectory( + new Pose2d(new Translation2d(-0.4, 1), Rotation2d.fromDegrees(0)), + List.of(), + new Pose2d(new Translation2d(0.4, 1), Rotation2d.fromDegrees(0)), + new TrajectoryConfig(2, 2).setStartVelocity(0).setEndVelocity(0) + ); + pathDown = TrajectoryGenerator.generateTrajectory( + new Pose2d(new Translation2d(0.4, 1), Rotation2d.fromDegrees(-90)), + List.of(), + new Pose2d(new Translation2d(0.4, 0.05), Rotation2d.fromDegrees(-90)), + new TrajectoryConfig(2, 2).setStartVelocity(0).setEndVelocity(0) + ); } public void periodic() { diff --git a/src/main/java/frc/robot/visualizers/DoubleJointedArmVisualizer.java b/src/main/java/frc/robot/visualizers/DoubleJointedArmVisualizer.java index d9f05d22ff..fa040bab44 100644 --- a/src/main/java/frc/robot/visualizers/DoubleJointedArmVisualizer.java +++ b/src/main/java/frc/robot/visualizers/DoubleJointedArmVisualizer.java @@ -2,6 +2,7 @@ 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.wpilibj.smartdashboard.Mechanism2d; import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d; @@ -63,11 +64,14 @@ public void showTargetPosition(boolean show) { TARGET_POSITION_MARK.setLineWeight(show ? DEFAULT_LINE_WIDTH : 0); } - public void showPath(List path) { + 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()); + MechanismRoot2d root = mechanism.getRoot( + paths.size() + ", " + i, + path.get(i).poseMeters.getX() + armRootPosition.getX(), + path.get(i).poseMeters.getY() + armRootPosition.getY() + ); MechanismLigament2d mark = new MechanismLigament2d( paths.size() + ", " + i + " mark", 0.01, diff --git a/src/main/java/frc/robot/visualizers/FollowPathCommand.java b/src/main/java/frc/robot/visualizers/FollowPathCommand.java index f323ed589d..c3085b73c2 100644 --- a/src/main/java/frc/robot/visualizers/FollowPathCommand.java +++ b/src/main/java/frc/robot/visualizers/FollowPathCommand.java @@ -1,37 +1,37 @@ package frc.robot.visualizers; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.trajectory.Trajectory; import edu.wpi.first.wpilibj2.command.Command; +import frc.utils.time.TimeUtil; -import java.util.List; import java.util.function.Consumer; public class FollowPathCommand extends Command { private final Consumer setPosition; - private final List path; + private final Trajectory path; - private int currentPoint = 0; + private double startTime = 0; - public FollowPathCommand(List path, Consumer setPosition) { - this.path = path; + public FollowPathCommand(Trajectory trajectory, Consumer setPosition) { + this.path = trajectory; this.setPosition = setPosition; } @Override public void initialize() { - currentPoint = 0; + startTime = TimeUtil.getCurrentTimeSeconds(); } @Override public void execute() { - setPosition.accept(path.get(currentPoint)); - currentPoint++; + setPosition.accept(path.sample(TimeUtil.getCurrentTimeSeconds() - startTime).poseMeters.getTranslation()); } @Override public boolean isFinished() { - return currentPoint >= path.size(); + return TimeUtil.getCurrentTimeSeconds() - startTime >= path.getTotalTimeSeconds(); } }