Team4388/2022NoWayHome
1
0
Fork 0
mirror of https://github.com/Team4388/2022NoWayHome.git synced 2026-06-09 08:48:07 -06:00
Code Issues Packages Projects Releases Wiki Activity

Implement working PathPlanner I/O

Browse Source
This commit is contained in:
nathanrsxtn
2022-02-15 11:05:59 -07:00
parent 2dba3604e9
commit 3cb4a13bb1
9 changed files with 94 additions and 297 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/deploy/example.txt
-3
View File
@@ -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.
src/main/java/frc4388/robot/Main.java
-1
View File
@@ -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;
src/main/java/frc4388/robot/Robot.java
+15 -8
View File
@@ -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);
}
/**
src/main/java/frc4388/robot/RobotContainer.java
+25 -23
View File
@@ -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<Command> commands = new ArrayList<Command>();
ArrayList<Command> 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<inputs.length; i++) {
for (int i = 0; i < inputs.length; i++) {
if (inputs[i] instanceof String) {
PathPlannerTrajectory traj = PathPlanner.loadPath(inputs[i].toString(), maxVel, maxAccel);
PathPlannerState initState = (PathPlannerState) traj.sample(0);
commands.add(new InstantCommand(() -> 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<File> 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.");
}
/**
src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+15 -16
View File
@@ -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());
src/main/java/frc4388/utility/DataLogging.java
-12
View File
@@ -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() {
}
}
src/main/java/frc4388/utility/PathPlannerTrajectoryUtil.java
-185
View File
@@ -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> T getProxy(Class<T> 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<PathPlannerState> states = (List<PathPlannerState>) 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<PathPlannerState> 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();
}
}
}
src/main/java/frc4388/utility/PathPlannerUtil.java
+18 -1
View File
@@ -11,18 +11,35 @@ public class PathPlannerUtil {
public Optional<Waypoint>[] 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<Point> anchorPoint;
public Optional<Point> prevControl;
public Optional<Point> 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<Double> x;
public Optional<Double> y;
}
public Optional<Double> holonomicAngle;
public Optional<Boolean> isReversal;
public Optional<Boolean> velOverride;
public Optional<Double> velOverride;
public Optional<Boolean> isLocked;
}
src/main/java/frc4388/utility/RobotLogger.java
+21 -48
View File
@@ -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<PathPlannerTrajectory.PathPlannerState> data;
private boolean enabled = false;
public void setEnabled(boolean value) {
enabled = value;
}
private final List<Waypoint> data = new ArrayList<>();
private final SendableChooser<Boolean> 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<PathPlannerTrajectory.PathPlannerState> 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;
}
}