diff --git a/src/main/java/frc/JoysticksBindings.java b/src/main/java/frc/JoysticksBindings.java index 77408e0d7d..7959d22098 100644 --- a/src/main/java/frc/JoysticksBindings.java +++ b/src/main/java/frc/JoysticksBindings.java @@ -5,7 +5,7 @@ import frc.joysticks.JoystickPorts; import frc.joysticks.SmartJoystick; import frc.robot.Robot; -import frc.robot.visualizers.FollowPathCommand; +import frc.robot.structures.FollowTrajectoryDemo; public class JoysticksBindings { @@ -32,15 +32,15 @@ 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 FollowPathCommand(Robot.pathDown, p -> robot.getArm().setPosition(p, true))) + .andThen(new FollowTrajectoryDemo(Robot.pathDown, p -> robot.getArm().setPosition(p, true))) ); usedJoystick.B.onTrue( new InstantCommand(() -> robot.getArmVisualizer().showPath(Robot.pathLeft.getStates())).andThen(new WaitCommand(0.3)) - .andThen(new FollowPathCommand(Robot.pathLeft, p -> robot.getArm().setPosition(p, false))) + .andThen(new FollowTrajectoryDemo(Robot.pathLeft, p -> robot.getArm().setPosition(p, false))) ); usedJoystick.Y.onTrue( new InstantCommand(() -> robot.getArmVisualizer().showPath(Robot.pathUp.getStates())).andThen(new WaitCommand(0.3)) - .andThen(new FollowPathCommand(Robot.pathUp, p -> robot.getArm().setPosition(p, false))) + .andThen(new FollowTrajectoryDemo(Robot.pathUp, p -> robot.getArm().setPosition(p, false))) ); // List> flipArm = PathGenerator diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 4272051a71..9497d79695 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -15,8 +15,8 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import frc.RobotManager; import frc.robot.hardware.phoenix6.BusChain; -import frc.robot.subsystems.DoubleJointedArm; -import frc.robot.visualizers.DoubleJointedArmVisualizer; +import frc.robot.structures.doublejointedarm.DoubleJointedArm; +import frc.robot.structures.doublejointedarm.DoubleJointedArmVisualizer; import frc.utils.battery.BatteryUtil; import java.util.List; @@ -35,8 +35,8 @@ public class Robot { "", 2.5, 2.5, - DoubleJointedArm.FIRST_JOINT_LENGTH_METERS, - DoubleJointedArm.SECOND_JOINT_LENGTH_METERS + DoubleJointedArm.ELBOW_LENGTH_METERS, + DoubleJointedArm.WRIST_LENGTH_METERS ); public static Trajectory pathUp; @@ -71,7 +71,7 @@ public void periodic() { BatteryUtil.logStatus(); BusChain.logChainsStatuses(); - armVisualizer.setAngles(arm.getFirstJointAngle(), arm.getSecondJointAngle()); + armVisualizer.setAngles(arm.getElbowAngle(), arm.getWristAngle()); armVisualizer.setTargetPositionMeters(arm.getPositionMeters()); CommandScheduler.getInstance().run(); // Should be last diff --git a/src/main/java/frc/robot/visualizers/FollowPathCommand.java b/src/main/java/frc/robot/structures/FollowTrajectoryDemo.java similarity index 81% rename from src/main/java/frc/robot/visualizers/FollowPathCommand.java rename to src/main/java/frc/robot/structures/FollowTrajectoryDemo.java index c3085b73c2..4a50944e2f 100644 --- a/src/main/java/frc/robot/visualizers/FollowPathCommand.java +++ b/src/main/java/frc/robot/structures/FollowTrajectoryDemo.java @@ -1,4 +1,4 @@ -package frc.robot.visualizers; +package frc.robot.structures; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.trajectory.Trajectory; @@ -7,14 +7,14 @@ import java.util.function.Consumer; -public class FollowPathCommand extends Command { +public class FollowTrajectoryDemo extends Command { private final Consumer setPosition; private final Trajectory path; private double startTime = 0; - public FollowPathCommand(Trajectory trajectory, Consumer setPosition) { + public FollowTrajectoryDemo(Trajectory trajectory, Consumer setPosition) { this.path = trajectory; this.setPosition = setPosition; } diff --git a/src/main/java/frc/robot/structures/doublejointedarm/ArmAngles.java b/src/main/java/frc/robot/structures/doublejointedarm/ArmAngles.java new file mode 100644 index 0000000000..23dfe115af --- /dev/null +++ b/src/main/java/frc/robot/structures/doublejointedarm/ArmAngles.java @@ -0,0 +1,5 @@ +package frc.robot.structures.doublejointedarm; + +import edu.wpi.first.math.geometry.Rotation2d; + +public record ArmAngles(Rotation2d elbowAngle, Rotation2d wristAngle) {} diff --git a/src/main/java/frc/robot/structures/doublejointedarm/DoubleJointedArm.java b/src/main/java/frc/robot/structures/doublejointedarm/DoubleJointedArm.java new file mode 100644 index 0000000000..cf3c2e227a --- /dev/null +++ b/src/main/java/frc/robot/structures/doublejointedarm/DoubleJointedArm.java @@ -0,0 +1,92 @@ +package frc.robot.structures.doublejointedarm; + +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 frc.robot.subsystems.GBSubsystem; +import frc.utils.alerts.Alert; +import org.littletonrobotics.junction.Logger; + +import java.util.Optional; + +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; + + private Rotation2d elbowAngle = new Rotation2d(); + private Rotation2d wristAngle = new Rotation2d(); + + public DoubleJointedArm(String logPath) { + super(logPath); + } + + @Override + protected void subsystemPeriodic() { + Logger.recordOutput(getLogPath() + "/ElbowAngleDeg", elbowAngle.getDegrees()); + Logger.recordOutput(getLogPath() + "/WristAngleDeg", wristAngle.getDegrees()); + Logger.recordOutput(getLogPath() + "/PositionMeters", getPositionMeters()); + } + + public Rotation2d getElbowAngle() { + return elbowAngle; + } + + public Rotation2d getWristAngle() { + return wristAngle; + } + + public void setElbowAngle(Rotation2d elbowAngle) { + this.elbowAngle = elbowAngle; + } + + public void setWristAngle(Rotation2d wristAngle) { + this.wristAngle = wristAngle; + } + + public void setAngles(Rotation2d elbowAngle, Rotation2d wristAngle) { + setElbowAngle(elbowAngle); + setWristAngle(wristAngle); + } + + public Translation2d getPositionMeters() { + return toPositionMeters(elbowAngle, wristAngle); + } + + public void setPosition(Translation2d positionMeters, boolean isElbowLeft) { + Optional targetAngles = toAngles(positionMeters, isElbowLeft); + if (targetAngles.isEmpty()) { + new Alert(Alert.AlertType.ERROR, getLogPath() + "unreachable position").report(); + return; + } + setAngles(targetAngles.get().elbowAngle(), targetAngles.get().wristAngle()); + } + + private static Optional toAngles(Translation2d positionMeters, boolean isElbowLeft) { + return DoubleJointedArmKinematics.toAngles(positionMeters, ELBOW_LENGTH_METERS, WRIST_LENGTH_METERS, isElbowLeft); + } + + private static Translation2d toPositionMeters(Rotation2d elbowAngle, Rotation2d wristAngle) { + return DoubleJointedArmKinematics.toPositionMeters(new ArmAngles(elbowAngle, wristAngle), ELBOW_LENGTH_METERS, WRIST_LENGTH_METERS); + } + + 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)))); + joystick.X.onTrue(new InstantCommand(() -> setAngles(Rotation2d.fromDegrees(150), Rotation2d.fromDegrees(75)))); + 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))); + } + + +} diff --git a/src/main/java/frc/robot/structures/doublejointedarm/DoubleJointedArmKinematics.java b/src/main/java/frc/robot/structures/doublejointedarm/DoubleJointedArmKinematics.java new file mode 100644 index 0000000000..01f7aada80 --- /dev/null +++ b/src/main/java/frc/robot/structures/doublejointedarm/DoubleJointedArmKinematics.java @@ -0,0 +1,54 @@ +package frc.robot.structures.doublejointedarm; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; + +import java.util.Optional; + +public class DoubleJointedArmKinematics { + + public static Translation2d toPositionMeters(ArmAngles angles, double elbowLengthMeters, double wristLengthMeters) { + return new Translation2d( + elbowLengthMeters * angles.elbowAngle().getCos() + wristLengthMeters * angles.wristAngle().getCos(), + elbowLengthMeters * angles.elbowAngle().getSin() + wristLengthMeters * angles.wristAngle().getSin() + ); + } + + public static Optional toAngles( + Translation2d positionMeters, + double elbowLengthMeters, + double wristLengthMeters, + boolean isElbowLeft + ) { + double x = positionMeters.getX(); + double y = positionMeters.getY(); + + Optional wristAngleOptional = cosineLaw(Math.sqrt(x * x + y * y), elbowLengthMeters, wristLengthMeters); + if (wristAngleOptional.isEmpty()) { + return Optional.empty(); + } + + double wristAngleRads = wristAngleOptional.get().getRadians(); + + if (isElbowLeft) { + wristAngleRads = -wristAngleRads; + } + + double elbowAngleRads = Math.atan2(y, x) + - Math.atan2(wristLengthMeters * Math.sin(wristAngleRads), elbowLengthMeters + wristLengthMeters * Math.cos(wristAngleRads)); + + return Optional.of(new ArmAngles(Rotation2d.fromRadians(elbowAngleRads), Rotation2d.fromRadians(elbowAngleRads + wristAngleRads))); + } + + + public static Optional cosineLaw(double farSide, double nearSide1, double nearSide2) { + double cosTheta = (farSide * farSide - nearSide1 * nearSide1 - nearSide2 * nearSide2) / (2 * nearSide1 * nearSide2); + + if (cosTheta < -1 || cosTheta > 1) { + return Optional.empty(); + } + + return Optional.of(Rotation2d.fromRadians(Math.acos(cosTheta))); + } + +} diff --git a/src/main/java/frc/robot/visualizers/DoubleJointedArmVisualizer.java b/src/main/java/frc/robot/structures/doublejointedarm/DoubleJointedArmVisualizer.java similarity index 76% rename from src/main/java/frc/robot/visualizers/DoubleJointedArmVisualizer.java rename to src/main/java/frc/robot/structures/doublejointedarm/DoubleJointedArmVisualizer.java index fa040bab44..fbb00c92e3 100644 --- a/src/main/java/frc/robot/visualizers/DoubleJointedArmVisualizer.java +++ b/src/main/java/frc/robot/structures/doublejointedarm/DoubleJointedArmVisualizer.java @@ -1,4 +1,4 @@ -package frc.robot.visualizers; +package frc.robot.structures.doublejointedarm; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; @@ -22,27 +22,29 @@ public class DoubleJointedArmVisualizer { private final Mechanism2d mechanism; private final MechanismRoot2d root; - private final MechanismLigament2d firstJoint; - private final MechanismLigament2d secondJoint; + private final MechanismLigament2d elbow; + private final MechanismLigament2d wrist; private final MechanismRoot2d targetPosition; private final ArrayList> paths = new ArrayList<>(); + private int pathCounter = 0; + public DoubleJointedArmVisualizer( String name, double frameXMeters, double frameYMeters, - double firstJointLengthMeters, - double secondJointLengthMeters + double elbowLengthMeters, + double wristLengthMeters ) { 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, DEFAULT_LINE_WIDTH, new Color8Bit(Color.kPurple)); + this.elbow = new MechanismLigament2d("elbow", elbowLengthMeters, 0); + this.wrist = new MechanismLigament2d("wrist", wristLengthMeters, 0, DEFAULT_LINE_WIDTH, new Color8Bit(Color.kPurple)); this.targetPosition = mechanism.getRoot(name + " TargetPosition", 0, 0); - firstJoint.append(secondJoint); - root.append(firstJoint); + elbow.append(wrist); + root.append(elbow); targetPosition.append(TARGET_POSITION_MARK); showTargetPosition(false); @@ -51,8 +53,8 @@ public DoubleJointedArmVisualizer( } public void setAngles(Rotation2d firstJointAngle, Rotation2d secondJointAngle) { - firstJoint.setAngle(firstJointAngle); - secondJoint.setAngle(toFloorRelative(firstJointAngle, secondJointAngle)); + elbow.setAngle(firstJointAngle); + wrist.setAngle(toFloorRelative(firstJointAngle, secondJointAngle)); } public void setTargetPositionMeters(Translation2d positionMeters) { @@ -65,29 +67,33 @@ public void showTargetPosition(boolean show) { } public void showPath(List path) { + if (!paths.isEmpty()) { + for (int i = 0; i < paths.get(0).size(); i++) { + paths.get(0).get(i).setLineWeight(0); + paths.get(0).get(i).close(); + } + paths.remove(0); + } + ArrayList pathVisualize = new ArrayList<>(path.size()); for (int i = 0; i < path.size(); i++) { MechanismRoot2d root = mechanism.getRoot( - paths.size() + ", " + i, + pathCounter + ", " + 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, + pathCounter + ", " + i + " mark", 0, - 10.0F, + 0, + DEFAULT_LINE_WIDTH, 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); + pathCounter++; } private static Rotation2d toFloorRelative(Rotation2d floorRelativeAngle, Rotation2d jointRelativeAngle) { diff --git a/src/main/java/frc/robot/subsystems/DoubleJointedArm.java b/src/main/java/frc/robot/subsystems/DoubleJointedArm.java deleted file mode 100644 index 7c83ae2fa9..0000000000 --- a/src/main/java/frc/robot/subsystems/DoubleJointedArm.java +++ /dev/null @@ -1,109 +0,0 @@ -package frc.robot.subsystems; - -import edu.wpi.first.math.Pair; -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; - -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(); - - public DoubleJointedArm(String logPath) { - super(logPath); - } - - @Override - protected void subsystemPeriodic() { - Logger.recordOutput(getLogPath() + "/FirstJointAngleDeg", firstJointAngle.getDegrees()); - Logger.recordOutput(getLogPath() + "/SecondJointAngleDeg", secondJointAngle.getDegrees()); - Logger.recordOutput(getLogPath() + "/PositionMeters", getPositionMeters()); - } - - public Rotation2d getFirstJointAngle() { - return firstJointAngle; - } - - public Rotation2d getSecondJointAngle() { - return secondJointAngle; - } - - public void setFirstJointAngle(Rotation2d firstJointAngle) { - this.firstJointAngle = firstJointAngle; - } - - public void setSecondJointAngle(Rotation2d secondJointAngle) { - this.secondJointAngle = secondJointAngle; - } - - 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) { - Pair targetAngles = toAngles(positionMeters, firstJointLeft); - setAngles(targetAngles.getFirst(), targetAngles.getSecond()); - } - - 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); - } - - private static Pair toAngles(Translation2d positionMeters, boolean firstJointLeft) { - double x = positionMeters.getX(); - double y = positionMeters.getY(); - - double theta2 = cosineLaw(Math.sqrt(x * x + y * y), FIRST_JOINT_LENGTH_METERS, SECOND_JOINT_LENGTH_METERS).getRadians(); - - if (firstJointLeft) { - theta2 = -theta2; - } - - double theta1 = Math.atan2(y, x) - - Math - .atan2(SECOND_JOINT_LENGTH_METERS * Math.sin(theta2), FIRST_JOINT_LENGTH_METERS + SECOND_JOINT_LENGTH_METERS * Math.cos(theta2)); - - return new Pair<>(Rotation2d.fromRadians(theta1), Rotation2d.fromRadians(theta1 + theta2)); - } - - 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)))); - joystick.X.onTrue(new InstantCommand(() -> setAngles(Rotation2d.fromDegrees(150), Rotation2d.fromDegrees(75)))); - 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))); - } - - public static Rotation2d cosineLaw(double farSide, double nearSide1, double nearSide2) { - double cosTheta = (farSide * farSide - nearSide1 * nearSide1 - nearSide2 * nearSide2) / (2 * nearSide1 * nearSide2); - - if (cosTheta < -1 || cosTheta > 1) { - throw new IllegalArgumentException("Position is unreachable."); - } - - return Rotation2d.fromRadians(Math.acos(cosTheta)); - } - -} diff --git a/src/main/java/frc/robot/visualizers/FollowPathWithSideCommand.java b/src/main/java/frc/robot/visualizers/FollowPathWithSideCommand.java deleted file mode 100644 index a94aa6ea24..0000000000 --- a/src/main/java/frc/robot/visualizers/FollowPathWithSideCommand.java +++ /dev/null @@ -1,38 +0,0 @@ -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 deleted file mode 100644 index 63635b63eb..0000000000 --- a/src/main/java/frc/robot/visualizers/PathGenerator.java +++ /dev/null @@ -1,69 +0,0 @@ -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; - } - -}