diff --git a/src/main/deploy/example.txt b/src/main/deploy/example.txt deleted file mode 100644 index 70c79b6..0000000 --- a/src/main/deploy/example.txt +++ /dev/null @@ -1,3 +0,0 @@ -Files placed in this directory will be deployed to the RoboRIO into the -'deploy' directory in the home folder. Use the 'FileUtilities.getFilePath' wpilib function -to get a proper path relative to the deploy directory. \ No newline at end of file diff --git a/src/main/java/frc4388/robot/Main.java b/src/main/java/frc4388/robot/Main.java index 64dac2f..a718f7e 100644 --- a/src/main/java/frc4388/robot/Main.java +++ b/src/main/java/frc4388/robot/Main.java @@ -11,7 +11,6 @@ import java.util.logging.Logger; import com.diffplug.common.base.DurianPlugins; import com.diffplug.common.base.Errors; -import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.RobotBase; import frc4388.utility.AnsiLogging; diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index aa4c6fd..3a33a29 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -4,11 +4,13 @@ package frc4388.robot; +import java.io.File; import java.io.IOException; import java.util.logging.Level; import java.util.logging.Logger; +import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj.TimedRobot; @@ -18,6 +20,7 @@ import edu.wpi.first.wpilibj2.command.CommandScheduler; import frc4388.utility.PathPlannerUtil; import frc4388.utility.RobotLogger; import frc4388.utility.RobotTime; +import frc4388.utility.PathPlannerUtil.Path; /** * The VM is configured to automatically run this class, and to call the @@ -50,9 +53,11 @@ public class Robot extends TimedRobot { LOGGER.log(Level.FINEST, "Logging Test 8/8"); // } - var path = PathPlannerUtil.Path.read(Filesystem.getDeployDirectory().toPath().resolve("pathplanner").resolve("Move Forward.path").toFile()); - LOGGER.finest(path::toString); + // var path = PathPlannerUtil.Path.read(Filesystem.getDeployDirectory().toPath().resolve("pathplanner").resolve("Move Forward.path").toFile()); + // LOGGER.finest(path::toString); LOGGER.fine("robotInit()"); + LOGGER.fine("Sssssssssh."); + DriverStation.silenceJoystickConnectionWarning(true); // Instantiate our RobotContainer. This will perform all our button bindings, and put our // autonomous chooser on the dashboard. m_robotContainer = new RobotContainer(); @@ -88,19 +93,24 @@ public class Robot extends TimedRobot { public void disabledInit() { LOGGER.fine("disabledInit()"); m_robotTime.endMatchTime(); - RobotLogger.getInstance().setEnabled(false); if (isTest()) { try { - String p = RobotLogger.getInstance().exportPath(); + // IMPORTANT: Had to chown the pathplanner folder in order to save autos. + Path path = RobotLogger.getInstance().createPath(0.1, 0.1, false); + String pathPath = "recording." + System.currentTimeMillis() + ".path"; + File outputFile = Filesystem.getDeployDirectory().toPath().resolve("pathplanner").resolve(pathPath).toFile(); + outputFile.createNewFile(); + path.write(outputFile); LOGGER.log(Level.WARNING, "----------------------------------------------------------------------"); LOGGER.log(Level.WARNING, "----------------------------------------------------------------------"); LOGGER.log(Level.WARNING, "----------------------------------------------------------------------"); LOGGER.log(Level.WARNING, "----------------------------------------------------------------------"); - LOGGER.log(Level.WARNING, "Recorded path to {0} in the deploy directory on the RoboRIO", p); + LOGGER.log(Level.WARNING, "Recorded path to {0} in the deploy directory on the RoboRIO", pathPath); LOGGER.log(Level.WARNING, "----------------------------------------------------------------------"); LOGGER.log(Level.WARNING, "----------------------------------------------------------------------"); LOGGER.log(Level.WARNING, "----------------------------------------------------------------------"); LOGGER.log(Level.WARNING, "----------------------------------------------------------------------"); + // LOGGER.finest(path::toString); } catch (IOException e) { e.printStackTrace(); } @@ -132,7 +142,6 @@ public class Robot extends TimedRobot { m_autonomousCommand.schedule(); } m_robotTime.startMatchTime(); - RobotLogger.getInstance().setEnabled(false); } /** @@ -153,7 +162,6 @@ public class Robot extends TimedRobot { m_autonomousCommand.cancel(); } m_robotTime.startMatchTime(); - RobotLogger.getInstance().setEnabled(true); DriverStation.silenceJoystickConnectionWarning(true); } @@ -168,7 +176,6 @@ public class Robot extends TimedRobot { @Override public void testInit() { - RobotLogger.getInstance().setEnabled(false); } /** diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 79d4825..e5cc5a6 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -4,8 +4,13 @@ package frc4388.robot; +import java.io.File; import java.io.IOException; import java.util.ArrayList; +import java.util.Arrays; +import java.util.Comparator; +import java.util.Optional; +import java.util.logging.Logger; import com.ctre.phoenix.motorcontrol.TalonFXControlMode; import com.ctre.phoenix.motorcontrol.can.TalonFX; @@ -18,11 +23,12 @@ import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj.command.PrintCommand; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.button.JoystickButton; @@ -113,8 +119,7 @@ public class RobotContainer { } public Command[] buildAuto(double maxVel, double maxAccel, Object... inputs) { - - ArrayList commands = new ArrayList(); + ArrayList commands = new ArrayList<>(); commands.add(new InstantCommand(() -> m_robotSwerveDrive.m_gyro.reset())); PIDController xController = SwerveDriveConstants.X_CONTROLLER; @@ -123,30 +128,18 @@ public class RobotContainer { thetaController.enableContinuousInput(-Math.PI, Math.PI); // parse input - for (int i=0; i m_robotSwerveDrive.resetOdometry(new Pose2d(initState.poseMeters.getTranslation(), initState.holonomicRotation)))); - commands.add(new PPSwerveControllerCommand( - traj, - m_robotSwerveDrive::getOdometry, - m_robotSwerveDrive.m_kinematics, - xController, - yController, - thetaController, - m_robotSwerveDrive::setModuleStates, - m_robotSwerveDrive)); - } - - if (inputs[i] instanceof Command) { + commands.add(new PPSwerveControllerCommand(traj, m_robotSwerveDrive::getOdometry, m_robotSwerveDrive.m_kinematics, xController, yController, thetaController, m_robotSwerveDrive::setModuleStates, m_robotSwerveDrive)); + } else if (inputs[i] instanceof Command) commands.add((Command) inputs[i]); - } } - - commands.add(new InstantCommand(() -> m_robotSwerveDrive.stopModules())); + + commands.add(new InstantCommand(m_robotSwerveDrive::stopModules)); Command[] ret = new Command[commands.size()]; ret = commands.toArray(ret); return ret; @@ -161,9 +154,18 @@ public class RobotContainer { public Command getAutonomousCommand() throws IOException { // https://github.com/mjansen4857/pathplanner/wiki <-- Pathplanner Wiki - Command[] trajCommands = buildAuto(0.5, 0.5, "Move Forward", "Move Down"); - SequentialCommandGroup ret = new SequentialCommandGroup(trajCommands); - return (new ParallelCommandGroup(buildAuto(0.1, 0.1, ret, new InstantCommand(() -> m_testMotor.set(TalonFXControlMode.PercentOutput, 0.2))))); + // Command[] trajCommands = buildAuto(0.5, 0.5, "Move Forward", "Move Down"); + // SequentialCommandGroup ret = new SequentialCommandGroup(trajCommands); + // return (new ParallelCommandGroup(buildAuto(0.1, 0.1, ret, new InstantCommand(() -> m_testMotor.set(TalonFXControlMode.PercentOutput, 0.2))))); + Optional recordedAuto = Arrays.stream(Filesystem.getDeployDirectory().toPath().resolve("pathplanner").toFile().listFiles()).filter(f -> f.getName().startsWith("recording")).max(Comparator.comparingLong(File::lastModified)); + if (recordedAuto.isPresent()) { + String name = recordedAuto.orElseThrow().getName().replace(".path", ""); + Logger.getLogger(this.getClass().getName()).fine(name); + return new SequentialCommandGroup(buildAuto(1.0, 1.0, name)); + } + Logger.getLogger(this.getClass().getName()).severe("No recorded autos found."); + return new InstantCommand(); + // return new PrintCommand("No recorded autos found."); } /** diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 11a9dd5..f8ed27d 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -24,6 +24,7 @@ import frc4388.robot.Constants.OIConstants; import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.utility.Gains; import frc4388.utility.RobotLogger; +import frc4388.utility.PathPlannerUtil.Path.Waypoint.Point; public class SwerveDrive extends SubsystemBase { @@ -156,28 +157,26 @@ public class SwerveDrive extends SubsystemBase { module.setDesiredState(state, false); } } - + @Override public void periodic() { // System.err.println(m_gyro.getFusedHeading() +" aaa"); updateOdometry(); // m_gyro.setFusedHeadingToCompass(); // m_gyro.setYawToCompass(); - { - - - - - // double velocityMetersPerSecond = new Translation2d(chassisSpeeds.vxMetersPerSecond, chassisSpeeds.vyMetersPerSecond).getNorm(); - double velocityMetersPerSecond = chassisSpeeds.vxMetersPerSecond; - Pose2d poseMeters = m_poseEstimator.getEstimatedPosition(); - double curvatureRadPerMeter = 0; - RobotLogger.getInstance().put(velocityMetersPerSecond, poseMeters, curvatureRadPerMeter, 0, new Rotation2d(), new Rotation2d(), new Rotation2d()); - - - - - } + Pose2d pos = m_poseEstimator.getEstimatedPosition(); + RobotLogger.getInstance().put( + /* anchorPointX */ pos.getX(), + /* anchorPointY */ pos.getY(), + /* prevControlX */ pos.getX(), + /* prevControlY */ pos.getY(), + /* nextControlX */ pos.getX(), + /* nextControlY */ pos.getY(), + /* holonomicAngle */ m_gyro.getRotation2d().getDegrees(), + /* isReversal */ false, + /* velOverride */ null, + /* isLocked */ false + ); SmartDashboard.putNumber("Pigeon Fused Heading", m_gyro.getFusedHeading(fstatus)); SmartDashboard.putNumber("Pigeon Yaw", m_gyro.getYaw()); SmartDashboard.putNumber("Pigeon Get Angle", m_gyro.getAngle()); diff --git a/src/main/java/frc4388/utility/DataLogging.java b/src/main/java/frc4388/utility/DataLogging.java deleted file mode 100644 index 730da4c..0000000 --- a/src/main/java/frc4388/utility/DataLogging.java +++ /dev/null @@ -1,12 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc4388.utility; - -/** Add your docs here. */ -public class DataLogging { - public DataLogging() { - - } -} diff --git a/src/main/java/frc4388/utility/PathPlannerTrajectoryUtil.java b/src/main/java/frc4388/utility/PathPlannerTrajectoryUtil.java deleted file mode 100644 index 0f00f92..0000000 --- a/src/main/java/frc4388/utility/PathPlannerTrajectoryUtil.java +++ /dev/null @@ -1,185 +0,0 @@ -package frc4388.utility; - -import java.io.BufferedWriter; -import java.io.FileWriter; -import java.io.IOException; -import java.lang.reflect.InvocationHandler; -import java.lang.reflect.InvocationTargetException; -import java.lang.reflect.Method; -import java.lang.reflect.Proxy; -import java.nio.file.Files; -import java.nio.file.Path; -import java.util.Collection; -import java.util.List; -import java.util.Objects; -import java.util.logging.Logger; -import java.util.stream.Collectors; - -import com.fasterxml.jackson.core.JsonProcessingException; -import com.fasterxml.jackson.databind.ObjectMapper; -import com.pathplanner.lib.PathPlanner; -import com.pathplanner.lib.PathPlannerTrajectory; -import com.pathplanner.lib.PathPlannerTrajectory.PathPlannerState; - -import org.json.simple.JSONArray; -import org.json.simple.JSONObject; -import org.json.simple.JSONValue; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; - -public final class PathPlannerTrajectoryUtil { - - // public static final class BetterUtil { - - // public static interface IPlanet { - // String getName(); - // } - - // static class Planet implements IPlanet { - // private String name; - - // public String getName() { - // return name; - // } - - // public void setName(String iName) { - // name = iName; - // } - // } - - // public static void test() throws JsonProcessingException { - // IPlanet ip = getProxy(IPlanet.class, new Planet()); - // ObjectMapper mapper = new ObjectMapper(); - // mapper.writeValueAsString(ip); - // } - - // public static T getProxy(Class type, Object obj) { - // class ProxyUtil implements InvocationHandler { - // Object obj; - - // public ProxyUtil(Object o) { - // obj = o; - // } - - // @Override - // public Object invoke(Object proxy, Method m, Object[] args) throws Throwable { - // Object result = null; - // result = m.invoke(obj, args); - // return result; - // } - // } - // @SuppressWarnings("unchecked") - // T proxy = (T) Proxy.newProxyInstance(type.getClassLoader(), new Class[] { type }, new ProxyUtil(obj)); - // return proxy; - // } - // } - - public static PathPlannerTrajectory fromPathPlannerJson(Path path, double maxVel, double maxAccel, boolean reversed) - throws IOException { - if (path.endsWith(".path")) - return PathPlanner.loadPath(path.toFile().getName(), maxVel, maxAccel, reversed); - throw new IOException("Invalid path"); - } - - public static PathPlannerTrajectory fromPathPlannerJson(Path path, double maxVel, double maxAccel) - throws IOException { - return fromPathPlannerJson(path, maxVel, maxAccel, false); - } - - @SuppressWarnings("unchecked") - public static PathPlannerTrajectory fromPathweaverJson(Path path, double maxVel, double maxAccel) throws IOException { - Object obj = JSONValue.parse(Files.newBufferedReader(path)); - JSONArray array = (JSONArray) obj; - List states = (List) array.stream().map(o -> { - JSONObject jo = (JSONObject) o; - PathPlannerState pathPlannerState = new PathPlannerState(); - pathPlannerState.timeSeconds = Double.parseDouble(Objects.toString(jo.get("time"))); - pathPlannerState.velocityMetersPerSecond = Double.parseDouble(Objects.toString(jo.get("velocity"))); - pathPlannerState.accelerationMetersPerSecondSq = Double.parseDouble(Objects.toString(jo.get("acceleration"))); - JSONObject pose = (JSONObject) jo.get("pose"); - JSONObject rotation = (JSONObject) pose.get("rotation"); - JSONObject translation = (JSONObject) pose.get("translation"); - double radians = Double.parseDouble(Objects.toString(rotation.get("radians"))); - double x = Double.parseDouble(Objects.toString(translation.get("x"))); - double y = Double.parseDouble(Objects.toString(translation.get("y"))); - pathPlannerState.poseMeters = new Pose2d(x, y, new Rotation2d(radians)); - pathPlannerState.curvatureRadPerMeter = Double.parseDouble(Objects.toString(jo.get("curvature"))); - pathPlannerState.positionMeters = Double.parseDouble(Objects.toString(jo.get("position"))); - pathPlannerState.angularVelocity = new Rotation2d( - Double.parseDouble(Objects.toString(jo.get("angularVelocity")))); - pathPlannerState.angularAcceleration = new Rotation2d( - Double.parseDouble(Objects.toString(jo.get("angularAcceleration")))); - pathPlannerState.holonomicRotation = new Rotation2d( - Double.parseDouble(Objects.toString(jo.get("holonomicRotation")))); - Logger.getAnonymousLogger().finest(pathPlannerState.toString()); - return pathPlannerState; - }).collect(Collectors.toList()); - try { - var constructor = PathPlannerTrajectory.class.getDeclaredConstructor(List.class); - constructor.setAccessible(true); - var a = constructor.newInstance(states); - Logger.getAnonymousLogger() - .severe(() -> a.getStates().stream().map(PathPlannerState.class::cast).map(o -> String.format( - "State(Sec: %.2f, Vel m/s: %.2f, Accel m/s/s: %.2f, Pose: %s, Curvature: %.2f, positionMeters: %.2f, angularVelocity: %s, angularAcceleration: %s, holonomicRotation: %s)", - o.timeSeconds, - o.velocityMetersPerSecond, - o.accelerationMetersPerSecondSq, - o.poseMeters, - o.curvatureRadPerMeter, - o.positionMeters, - o.angularVelocity, - o.angularAcceleration, - o.holonomicRotation)).collect(Collectors.joining("\n"))); - return a; - } catch (InstantiationException | IllegalAccessException | IllegalArgumentException | InvocationTargetException - | NoSuchMethodException | SecurityException e) { - throw new RuntimeException(e); - } - } - - public static void toPathweaverJson(Collection trajectoryStates, Path path) throws IOException { - try (BufferedWriter out = new BufferedWriter(new FileWriter(path.toFile()))) { - out.write('['); - for (var state : trajectoryStates) { - double acceleration = state.accelerationMetersPerSecondSq; - double curvature = state.curvatureRadPerMeter; - // pose - // rotation - double poseRotationRadians = state.poseMeters.getRotation().getRadians(); - // translation - double poseTranslationX = state.poseMeters.getX(); - double poseTranslationY = state.poseMeters.getY(); - double time = 0.0; - double velocity = state.velocityMetersPerSecond; - double position = state.positionMeters; - double holonomicRotation = state.holonomicRotation.getRadians(); - double angularVelocity = state.angularVelocity.getRadians(); - double angularAcceleration = state.angularAcceleration.getRadians(); - double curveRadius = 0; - out.write('{'); - out.write("\"acceleration\":" + acceleration + ","); - out.write("\"curvature\":" + curvature + ","); - out.write("\"pose\": {"); - out.write("\"rotation\": {"); - out.write("\"radians\":" + poseRotationRadians); - out.write("},"); - out.write("\"translation\": {"); - out.write("\"x\":" + poseTranslationX + ","); - out.write("\"y\":" + poseTranslationY); - out.write('}'); - out.write("},"); - out.write("\"time\":" + time + ","); - out.write("\"velocity\":" + velocity + ","); - out.write("\"position\":" + position + ","); - out.write("\"holonomicRotation\":" + holonomicRotation + ","); - out.write("\"angularVelocity\":" + angularVelocity + ","); - out.write("\"angularAcceleration\":" + angularAcceleration + ","); - out.write("\"curveRadius\":" + curveRadius); - out.write("},"); - } - out.write(']'); - out.flush(); - } - } -} diff --git a/src/main/java/frc4388/utility/PathPlannerUtil.java b/src/main/java/frc4388/utility/PathPlannerUtil.java index e2495aa..4c2c7b2 100644 --- a/src/main/java/frc4388/utility/PathPlannerUtil.java +++ b/src/main/java/frc4388/utility/PathPlannerUtil.java @@ -11,18 +11,35 @@ public class PathPlannerUtil { public Optional[] waypoints; public static final class Waypoint { + public Waypoint() {} + public Waypoint(Double anchorPointX, Double anchorPointY, Double prevControlX, Double prevControlY, Double nextControlX, Double nextControlY, Double holonomicAngle, Boolean isReversal, Double velOverride, Boolean isLocked) { + this.anchorPoint = anchorPointX == null && anchorPointY == null ? Optional.empty() : Optional.of(Point.of(anchorPointX, anchorPointY)); + this.prevControl = prevControlX == null && prevControlY == null ? Optional.empty() : Optional.of(Point.of(prevControlX, prevControlY)); + this.nextControl = nextControlX == null && nextControlY == null ? Optional.empty() : Optional.of(Point.of(nextControlX, nextControlY)); + this.holonomicAngle = Optional.ofNullable(holonomicAngle); + this.isReversal = Optional.ofNullable(isReversal); + this.velOverride = Optional.ofNullable(velOverride); + this.isLocked = Optional.ofNullable(isLocked); + } public Optional anchorPoint; public Optional prevControl; public Optional nextControl; public static final class Point { + public Point() {} + public static Point of(Double x, Double y) { + Point point = new Point(); + point.x = Optional.ofNullable(x); + point.y = Optional.ofNullable(y); + return point; + } public Optional x; public Optional y; } public Optional holonomicAngle; public Optional isReversal; - public Optional velOverride; + public Optional velOverride; public Optional isLocked; } diff --git a/src/main/java/frc4388/utility/RobotLogger.java b/src/main/java/frc4388/utility/RobotLogger.java index c352730..16c1a33 100644 --- a/src/main/java/frc4388/utility/RobotLogger.java +++ b/src/main/java/frc4388/utility/RobotLogger.java @@ -1,24 +1,16 @@ package frc4388.utility; -import java.io.IOException; -import java.nio.file.Path; import java.util.ArrayList; import java.util.List; import java.util.Objects; +import java.util.Optional; -import com.pathplanner.lib.PathPlannerTrajectory; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.trajectory.Trajectory; import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.Filesystem; +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc4388.utility.PathPlannerUtil.Path.Waypoint; public final class RobotLogger { - private static final int SAMPLE_BASE = 15_000; // ms of sampling - private static final int SAMPLE_RATE = 20; // ms between samples - private static RobotLogger instance = null; public static RobotLogger getInstance() { @@ -30,49 +22,30 @@ public final class RobotLogger { } private RobotLogger() { - data = new ArrayList<>(); + sendableChooser.setDefaultOption("Disable", false); + sendableChooser.addOption("Enable", true); + SmartDashboard.putData("Recording", sendableChooser); } - private final List data; - private boolean enabled = false; - - public void setEnabled(boolean value) { - enabled = value; - } + private final List data = new ArrayList<>(); + private final SendableChooser sendableChooser = new SendableChooser<>(); double lastVelocityMetersPerSecond = 0; - public void put( - double velocityMetersPerSecond, - Pose2d poseMeters, - double curvatureRadPerMeter, - double positionMeters, - Rotation2d angularVelocity, - Rotation2d angularAcceleration, - Rotation2d holonomicRotation) { - if (enabled) { - double accelerationMetersPerSecondSq = lastVelocityMetersPerSecond == 0 ? velocityMetersPerSecond : - (velocityMetersPerSecond - lastVelocityMetersPerSecond) / 2.0; - PathPlannerTrajectory.PathPlannerState pathPlannerState = new PathPlannerTrajectory.PathPlannerState(); - pathPlannerState.timeSeconds = getTime(); - pathPlannerState.velocityMetersPerSecond = velocityMetersPerSecond; - pathPlannerState.accelerationMetersPerSecondSq = accelerationMetersPerSecondSq; - pathPlannerState.poseMeters = poseMeters; - pathPlannerState.curvatureRadPerMeter = curvatureRadPerMeter; - pathPlannerState.positionMeters = positionMeters; - pathPlannerState.angularVelocity = angularVelocity; - pathPlannerState.angularAcceleration = angularAcceleration; - pathPlannerState.holonomicRotation = holonomicRotation; - data.add(pathPlannerState); - lastVelocityMetersPerSecond = velocityMetersPerSecond; + public void put(Double anchorPointX, Double anchorPointY, Double prevControlX, Double prevControlY, Double nextControlX, Double nextControlY, Double holonomicAngle, Boolean isReversal, Double velOverride, Boolean isLocked) { + if (Boolean.TRUE.equals(sendableChooser.getSelected())) { + Waypoint waypoint = new Waypoint(anchorPointX, anchorPointY, prevControlX, prevControlY, nextControlX, nextControlY, holonomicAngle, isReversal, velOverride, isLocked); + data.add(waypoint); } } - public String exportPath() throws IOException { - List states = data; - String pathPath = "recording." + System.currentTimeMillis() + ".json"; - Path outputPath = Filesystem.getDeployDirectory().toPath().resolve(pathPath); - outputPath.toFile().createNewFile(); - PathPlannerTrajectoryUtil.toPathweaverJson(states, outputPath); - return pathPath; + @SuppressWarnings("unchecked") + public PathPlannerUtil.Path createPath(Double maxVelocity, Double maxAcceleration, Boolean isReversed) { + PathPlannerUtil.Path path = new PathPlannerUtil.Path(); + path.waypoints = data.stream().map(Optional::ofNullable).toArray(Optional[]::new); + path.maxVelocity = Optional.ofNullable(maxVelocity); + path.maxAcceleration = Optional.ofNullable(maxAcceleration); + path.isReversed = Optional.ofNullable(isReversed); + data.clear(); + return path; } }