mirror of
https://github.com/Team4388/2022NoWayHome.git
synced 2026-06-09 00:38:05 -06:00
Testing PathPlanner read/write
This commit is contained in:
nathanrsxtn
@@ -5,6 +5,13 @@
|
||||
package frc4388.robot;
|
||||
|
||||
|
||||
import java.util.logging.Level;
|
||||
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;
|
||||
|
||||
@@ -24,6 +31,7 @@ public final class Main {
|
||||
*/
|
||||
public static void main(String... args) {
|
||||
AnsiLogging.systemInstall();
|
||||
DurianPlugins.register(Errors.Plugins.Log.class, e -> Logger.getLogger(e.getStackTrace()[0].getClassName()).log(Level.SEVERE, e, e::getLocalizedMessage));
|
||||
RobotBase.startRobot(Robot::new);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -10,10 +10,12 @@ import java.util.logging.Level;
|
||||
import java.util.logging.Logger;
|
||||
|
||||
import edu.wpi.first.wpilibj.DriverStation;
|
||||
import edu.wpi.first.wpilibj.Filesystem;
|
||||
import edu.wpi.first.wpilibj.TimedRobot;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import edu.wpi.first.wpilibj2.command.CommandScheduler;
|
||||
import frc4388.utility.PathPlannerUtil;
|
||||
import frc4388.utility.RobotLogger;
|
||||
import frc4388.utility.RobotTime;
|
||||
|
||||
@@ -37,7 +39,7 @@ public class Robot extends TimedRobot {
|
||||
*/
|
||||
@Override
|
||||
public void robotInit() {
|
||||
if (org.fusesource.jansi.Ansi.isEnabled()) {
|
||||
// if (org.fusesource.jansi.Ansi.isEnabled()) {
|
||||
LOGGER.log(Level.ALL, "Logging Test 1/8");
|
||||
LOGGER.log(Level.SEVERE, "Logging Test 2/8");
|
||||
LOGGER.log(Level.WARNING, "Logging Test 3/8");
|
||||
@@ -46,7 +48,10 @@ public class Robot extends TimedRobot {
|
||||
LOGGER.log(Level.FINE, "Logging Test 6/8");
|
||||
LOGGER.log(Level.FINER, "Logging Test 7/8");
|
||||
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);
|
||||
LOGGER.fine("robotInit()");
|
||||
// Instantiate our RobotContainer. This will perform all our button bindings, and put our
|
||||
// autonomous chooser on the dashboard.
|
||||
@@ -84,6 +89,22 @@ public class Robot extends TimedRobot {
|
||||
LOGGER.fine("disabledInit()");
|
||||
m_robotTime.endMatchTime();
|
||||
RobotLogger.getInstance().setEnabled(false);
|
||||
if (isTest()) {
|
||||
try {
|
||||
String p = RobotLogger.getInstance().exportPath();
|
||||
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, "----------------------------------------------------------------------");
|
||||
LOGGER.log(Level.WARNING, "----------------------------------------------------------------------");
|
||||
LOGGER.log(Level.WARNING, "----------------------------------------------------------------------");
|
||||
LOGGER.log(Level.WARNING, "----------------------------------------------------------------------");
|
||||
} catch (IOException e) {
|
||||
e.printStackTrace();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@Override
|
||||
@@ -99,7 +120,12 @@ public class Robot extends TimedRobot {
|
||||
@Override
|
||||
public void autonomousInit() {
|
||||
LOGGER.fine("autonomousInit()");
|
||||
m_autonomousCommand = m_robotContainer.getAutonomousCommand();
|
||||
try {
|
||||
m_autonomousCommand = m_robotContainer.getAutonomousCommand();
|
||||
} catch (IOException e) {
|
||||
// TODO Auto-generated catch block
|
||||
e.printStackTrace();
|
||||
}
|
||||
|
||||
/*String autoSelected = SmartDashboard.getString("Auto Selector", "Default");
|
||||
switch (autoSelected) {
|
||||
@@ -154,11 +180,6 @@ public class Robot extends TimedRobot {
|
||||
@Override
|
||||
public void testInit() {
|
||||
RobotLogger.getInstance().setEnabled(false);
|
||||
try {
|
||||
RobotLogger.getInstance().exportPath();
|
||||
} catch (IOException e) {
|
||||
e.printStackTrace();
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
|
||||
@@ -4,36 +4,33 @@
|
||||
|
||||
package frc4388.robot;
|
||||
|
||||
import java.io.File;
|
||||
import java.io.IOException;
|
||||
import java.util.Arrays;
|
||||
import java.util.Comparator;
|
||||
|
||||
import java.util.List;
|
||||
|
||||
import com.pathplanner.lib.PathPlanner;
|
||||
import com.pathplanner.lib.PathPlannerTrajectory;
|
||||
import com.pathplanner.lib.commands.PPSwerveControllerCommand;
|
||||
|
||||
import edu.wpi.first.math.controller.HolonomicDriveController;
|
||||
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.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.math.trajectory.TrapezoidProfile;
|
||||
import edu.wpi.first.math.trajectory.constraint.TrajectoryConstraint;
|
||||
import edu.wpi.first.wpilibj.Filesystem;
|
||||
import edu.wpi.first.wpilibj.GenericHID;
|
||||
import edu.wpi.first.wpilibj.XboxController;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import edu.wpi.first.wpilibj2.command.InstantCommand;
|
||||
import edu.wpi.first.wpilibj2.command.RunCommand;
|
||||
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
|
||||
import edu.wpi.first.wpilibj2.command.SwerveControllerCommand;
|
||||
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
|
||||
|
||||
import frc4388.robot.Constants.*;
|
||||
import frc4388.robot.Constants.LEDConstants;
|
||||
import frc4388.robot.Constants.OIConstants;
|
||||
import frc4388.robot.subsystems.LED;
|
||||
import frc4388.robot.subsystems.SwerveDrive;
|
||||
import frc4388.utility.LEDPatterns;
|
||||
import frc4388.utility.PathPlannerTrajectoryUtil;
|
||||
import frc4388.utility.controller.DeadbandedXboxController;
|
||||
|
||||
/**
|
||||
@@ -49,7 +46,7 @@ public class RobotContainer {
|
||||
|
||||
/* Subsystems */
|
||||
private final SwerveDrive m_robotSwerveDrive = new SwerveDrive(
|
||||
m_robotMap.leftFront, m_robotMap.leftBack, m_robotMap.rightFront, m_robotMap.rightBack, m_robotMap.gyro);
|
||||
m_robotMap.leftFront, m_robotMap.leftBack, m_robotMap.rightFront, m_robotMap.rightBack, m_robotMap.gyro);
|
||||
|
||||
private final LED m_robotLED = new LED(m_robotMap.LEDController);
|
||||
|
||||
@@ -66,12 +63,12 @@ public class RobotContainer {
|
||||
// drives the swerve drive with a two-axis input from the driver controller
|
||||
m_robotSwerveDrive.setDefaultCommand(
|
||||
new RunCommand(() -> m_robotSwerveDrive.driveWithInput(
|
||||
getDriverController().getLeftX(),
|
||||
getDriverController().getLeftY(),
|
||||
getDriverController().getRightX(),
|
||||
getDriverController().getRightY(),
|
||||
true),
|
||||
m_robotSwerveDrive));
|
||||
getDriverController().getLeftX(),
|
||||
getDriverController().getLeftY(),
|
||||
getDriverController().getRightX(),
|
||||
getDriverController().getRightY(),
|
||||
true),
|
||||
m_robotSwerveDrive));
|
||||
|
||||
// continually sends updates to the Blinkin LED controller to keep the lights on
|
||||
m_robotLED.setDefaultCommand(new RunCommand(m_robotLED::updateLED, m_robotLED));
|
||||
@@ -86,107 +83,69 @@ public class RobotContainer {
|
||||
private void configureButtonBindings() {
|
||||
/* Driver Buttons */
|
||||
new JoystickButton(getDriverController(), XboxController.Button.kY.value)
|
||||
// new XboxControllerRawButton(m_driverXbox, XboxControllerRaw.Y_BUTTON)
|
||||
.whenPressed(m_robotSwerveDrive.m_gyro::reset);
|
||||
// new XboxControllerRawButton(m_driverXbox, XboxControllerRaw.Y_BUTTON)
|
||||
.whenPressed(m_robotSwerveDrive.m_gyro::reset);
|
||||
|
||||
new JoystickButton(getDriverController(), XboxController.Button.kLeftBumper.value)
|
||||
// new XboxControllerRawButton(m_driverXbox, XboxControllerRaw.LEFT_BUMPER_BUTTON)
|
||||
.whenPressed(() -> m_robotSwerveDrive.highSpeed(false));
|
||||
|
||||
// new XboxControllerRawButton(m_driverXbox,
|
||||
// XboxControllerRaw.LEFT_BUMPER_BUTTON)
|
||||
.whenPressed(() -> m_robotSwerveDrive.highSpeed(false));
|
||||
|
||||
new JoystickButton(getDriverController(), XboxController.Button.kRightBumper.value)
|
||||
// new XboxControllerRawButton(m_driverXbox, XboxControllerRaw.RIGHT_BUMPER_BUTTON)
|
||||
.whenPressed(() -> m_robotSwerveDrive.highSpeed(true));
|
||||
|
||||
// new XboxControllerRawButton(m_driverXbox,
|
||||
// XboxControllerRaw.RIGHT_BUMPER_BUTTON)
|
||||
.whenPressed(() -> m_robotSwerveDrive.highSpeed(true));
|
||||
|
||||
new JoystickButton(getDriverController(), XboxController.Button.kA.value)
|
||||
|
||||
.whenPressed(() -> zeroOdometry(new Pose2d(0, 0, new Rotation2d(0))));
|
||||
//.whenPressed(this::resetOdometry);
|
||||
.whenPressed(() -> zeroOdometry(new Pose2d(0, 0, new Rotation2d(0))));
|
||||
// .whenPressed(this::resetOdometry);
|
||||
|
||||
/* Operator Buttons */
|
||||
// activates "Lit Mode"
|
||||
new JoystickButton(getOperatorController(), XboxController.Button.kA.value)
|
||||
// new XboxControllerRawButton(m_driverXbox, XboxControllerRaw.A_BUTTON)
|
||||
// new XboxControllerRawButton(m_driverXbox, XboxControllerRaw.A_BUTTON)
|
||||
.whenPressed(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW))
|
||||
.whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN));
|
||||
}
|
||||
|
||||
public void configAuto(boolean pathplanner) {
|
||||
|
||||
}
|
||||
|
||||
public void configAuto() {
|
||||
configAuto(true);
|
||||
}
|
||||
|
||||
/**
|
||||
* Use this to pass the autonomous command to the main {@link Robot} class.
|
||||
*
|
||||
* @return the command to run in autonomous
|
||||
* @throws IOException
|
||||
*/
|
||||
public Command getAutonomousCommand() {
|
||||
public Command getAutonomousCommand() throws IOException {
|
||||
// https://github.com/mjansen4857/pathplanner/wiki <-- Pathplanner Wiki
|
||||
|
||||
TrajectoryConfig config = new TrajectoryConfig(1.0, 1.0)
|
||||
.setKinematics(m_robotSwerveDrive.m_kinematics);
|
||||
Trajectory exTraj = TrajectoryGenerator.generateTrajectory(
|
||||
new Pose2d(0, 0, new Rotation2d(0)),
|
||||
List.of(new Translation2d(0, 0)),
|
||||
new Pose2d(1, 0, new Rotation2d(0)),
|
||||
config);
|
||||
try (
|
||||
PIDController xController = new PIDController(10.0, 0.0, 0.0);
|
||||
PIDController yController = new PIDController(1.3, 0.0, 0.0)) {
|
||||
ProfiledPIDController thetaController = new ProfiledPIDController(
|
||||
10.0, 0.0, 0.0, new TrapezoidProfile.Constraints(2 * Math.PI, Math.PI));
|
||||
thetaController.enableContinuousInput(-Math.PI, Math.PI);
|
||||
PathPlannerTrajectory ppRecorded = PathPlannerTrajectoryUtil
|
||||
.fromPathweaverJson(Arrays.stream(Filesystem.getDeployDirectory().listFiles())
|
||||
.max(Comparator.comparingLong(File::lastModified)).orElseThrow().toPath(), 1.0, 1.0);
|
||||
|
||||
Trajectory firstTestPath = PathPlanner.loadPath("First Test Path", 1.0, 1.0);
|
||||
Trajectory moveForward = PathPlanner.loadPath("Move Forward", 1.0, 1.0);
|
||||
Trajectory rotate = PathPlanner.loadPath("Rotate", 1.0, 1.0);
|
||||
PathPlannerTrajectory ppCurrentPath = ppRecorded; // change this to change auto
|
||||
|
||||
Trajectory currentPath = moveForward; // change this to change auto
|
||||
PPSwerveControllerCommand ppSwerveControllerCommand = new PPSwerveControllerCommand(
|
||||
ppCurrentPath,
|
||||
m_robotSwerveDrive::getOdometry,
|
||||
m_robotSwerveDrive.m_kinematics,
|
||||
xController,
|
||||
yController,
|
||||
thetaController,
|
||||
m_robotSwerveDrive::setModuleStates,
|
||||
m_robotSwerveDrive);
|
||||
|
||||
PIDController xController = new PIDController(10.0, 0.0, 0.0);
|
||||
PIDController yController = new PIDController(1.3, 0.0, 0.0);
|
||||
ProfiledPIDController thetaController = new ProfiledPIDController(
|
||||
10.0, 0.0, 0.0, new TrapezoidProfile.Constraints(2 * Math.PI, Math.PI));
|
||||
thetaController.enableContinuousInput(-Math.PI, Math.PI);
|
||||
|
||||
SwerveControllerCommand swerveControllerCommand = new SwerveControllerCommand(
|
||||
currentPath,
|
||||
m_robotSwerveDrive::getOdometry,
|
||||
m_robotSwerveDrive.m_kinematics,
|
||||
xController,
|
||||
yController,
|
||||
thetaController,
|
||||
m_robotSwerveDrive::setModuleStates,
|
||||
m_robotSwerveDrive
|
||||
);
|
||||
|
||||
PathPlannerTrajectory ppfirstTestPath = PathPlanner.loadPath("First Test Path", 4.0, 4.0);
|
||||
PathPlannerTrajectory ppMoveForward = PathPlanner.loadPath("Move Forward", 1.0, 1.0);
|
||||
PathPlannerTrajectory ppRotate = PathPlanner.loadPath("Rotate", 1.0, 1.0);
|
||||
|
||||
PathPlannerTrajectory ppCurrentPath = ppfirstTestPath; // change this to change auto
|
||||
|
||||
PPSwerveControllerCommand ppSwerveControllerCommand = new PPSwerveControllerCommand(
|
||||
ppCurrentPath,
|
||||
m_robotSwerveDrive::getOdometry,
|
||||
m_robotSwerveDrive.m_kinematics,
|
||||
xController,
|
||||
yController,
|
||||
thetaController,
|
||||
m_robotSwerveDrive::setModuleStates,
|
||||
m_robotSwerveDrive
|
||||
);
|
||||
|
||||
// return new SequentialCommandGroup(
|
||||
// new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(currentPath.getInitialPose())),
|
||||
// swerveControllerCommand,
|
||||
// new InstantCommand(() -> m_robotSwerveDrive.stopModules())
|
||||
// );
|
||||
|
||||
return new SequentialCommandGroup(
|
||||
new InstantCommand(() -> m_robotSwerveDrive.m_gyro.reset()),
|
||||
new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(ppCurrentPath.getInitialPose())),
|
||||
ppSwerveControllerCommand,
|
||||
new InstantCommand(() -> m_robotSwerveDrive.stopModules())
|
||||
);
|
||||
return new SequentialCommandGroup(
|
||||
new InstantCommand(m_robotSwerveDrive.m_gyro::reset),
|
||||
new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(ppCurrentPath.getInitialPose())),
|
||||
ppSwerveControllerCommand,
|
||||
new InstantCommand(m_robotSwerveDrive::stopModules));
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -203,6 +162,7 @@ public class RobotContainer {
|
||||
public void zeroOdometry(Pose2d pose) {
|
||||
m_robotSwerveDrive.resetOdometry(pose);
|
||||
}
|
||||
|
||||
/**
|
||||
* Add your docs here.
|
||||
*/
|
||||
|
||||
@@ -122,6 +122,7 @@ public class SwerveDrive extends SubsystemBase {
|
||||
setModuleStates(states);
|
||||
}
|
||||
private Rotation2d rotTarget = new Rotation2d();
|
||||
private ChassisSpeeds chassisSpeeds = new ChassisSpeeds();
|
||||
public void driveWithInput(double leftX, double leftY, double rightX, double rightY, boolean fieldRelative)
|
||||
{
|
||||
ignoreAngles = leftX == 0 && leftY == 0 && rightX == 0 && rightY == 0;
|
||||
@@ -132,11 +133,12 @@ public class SwerveDrive extends SubsystemBase {
|
||||
double rot = rotTarget.minus(m_gyro.getRotation2d()).getRadians();
|
||||
double xSpeedMetersPerSecond = -speed.getX();
|
||||
double ySpeedMetersPerSecond = speed.getY();
|
||||
chassisSpeeds = fieldRelative
|
||||
? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot * SwerveDriveConstants.ROTATION_SPEED, m_gyro.getRotation2d())
|
||||
: new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rightX * SwerveDriveConstants.ROTATION_SPEED);
|
||||
SwerveModuleState[] states =
|
||||
m_kinematics.toSwerveModuleStates(
|
||||
fieldRelative
|
||||
? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot * SwerveDriveConstants.ROTATION_SPEED, m_gyro.getRotation2d())
|
||||
: new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rightX * SwerveDriveConstants.ROTATION_SPEED));
|
||||
chassisSpeeds);
|
||||
setModuleStates(states);
|
||||
}
|
||||
|
||||
@@ -155,7 +157,21 @@ public class SwerveDrive extends SubsystemBase {
|
||||
updateOdometry();
|
||||
// m_gyro.setFusedHeadingToCompass();
|
||||
// m_gyro.setYawToCompass();
|
||||
RobotLogger.getInstance().put("poseMeters", m_poseEstimator.getEstimatedPosition());
|
||||
{
|
||||
|
||||
|
||||
|
||||
|
||||
// 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());
|
||||
|
||||
|
||||
|
||||
|
||||
}
|
||||
SmartDashboard.putNumber("Pigeon Fused Heading", m_gyro.getFusedHeading(fstatus));
|
||||
SmartDashboard.putNumber("Pigeon Yaw", m_gyro.getYaw());
|
||||
SmartDashboard.putNumber("Pigeon Get Angle", m_gyro.getAngle());
|
||||
|
||||
@@ -0,0 +1,185 @@
|
||||
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();
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,49 @@
|
||||
package frc4388.utility;
|
||||
|
||||
import java.io.File;
|
||||
import java.util.Optional;
|
||||
import com.diffplug.common.base.Errors;
|
||||
import com.fasterxml.jackson.databind.ObjectMapper;
|
||||
import com.fasterxml.jackson.datatype.jdk8.Jdk8Module;
|
||||
|
||||
public class PathPlannerUtil {
|
||||
public static final class Path {
|
||||
public Optional<Waypoint>[] waypoints;
|
||||
|
||||
public static final class Waypoint {
|
||||
public Optional<Point> anchorPoint;
|
||||
public Optional<Point> prevControl;
|
||||
public Optional<Point> nextControl;
|
||||
|
||||
public static final class Point {
|
||||
public Optional<Double> x;
|
||||
public Optional<Double> y;
|
||||
}
|
||||
|
||||
public Optional<Double> holonomicAngle;
|
||||
public Optional<Boolean> isReversal;
|
||||
public Optional<Boolean> velOverride;
|
||||
public Optional<Boolean> isLocked;
|
||||
}
|
||||
|
||||
public Optional<Double> maxVelocity;
|
||||
public Optional<Double> maxAcceleration;
|
||||
public Optional<Boolean> isReversed;
|
||||
|
||||
private static final ObjectMapper objectMapper = new ObjectMapper();
|
||||
static { objectMapper.registerModule(new Jdk8Module()); }
|
||||
|
||||
public static Path read(File src) {
|
||||
return Errors.log().getWithDefault(() -> objectMapper.readValue(src, Path.class), null);
|
||||
}
|
||||
|
||||
public void write(File resultFile) {
|
||||
Errors.log().run(() -> objectMapper.writeValue(resultFile, this));
|
||||
}
|
||||
|
||||
@Override
|
||||
public String toString() {
|
||||
return Errors.log().getWithDefault(() -> objectMapper.writeValueAsString(this), super.toString());
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -2,19 +2,18 @@ package frc4388.utility;
|
||||
|
||||
import java.io.IOException;
|
||||
import java.nio.file.Path;
|
||||
import java.util.HashMap;
|
||||
import java.util.Hashtable;
|
||||
import java.util.ArrayList;
|
||||
import java.util.List;
|
||||
import java.util.Map;
|
||||
import java.util.Objects;
|
||||
import java.util.Map.Entry;
|
||||
import java.util.stream.Collectors;
|
||||
|
||||
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.math.trajectory.TrajectoryUtil;
|
||||
import edu.wpi.first.wpilibj.DriverStation;
|
||||
import edu.wpi.first.wpilibj.Filesystem;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
|
||||
public final class RobotLogger {
|
||||
private static final int SAMPLE_BASE = 15_000; // ms of sampling
|
||||
@@ -26,42 +25,54 @@ public final class RobotLogger {
|
||||
return Objects.requireNonNullElseGet(instance, () -> instance = new RobotLogger());
|
||||
}
|
||||
|
||||
private static long getTime() {
|
||||
return RobotTime.getInstance().m_robotTime;
|
||||
public static double getTime() {
|
||||
return DriverStation.getMatchTime();
|
||||
}
|
||||
|
||||
private RobotLogger() {
|
||||
data = new HashMap<>();
|
||||
data = new ArrayList<>();
|
||||
}
|
||||
|
||||
private final Map<String, Map<Long, Object>> data;
|
||||
private final List<PathPlannerTrajectory.PathPlannerState> data;
|
||||
private boolean enabled = false;
|
||||
|
||||
public void setEnabled(boolean value) {
|
||||
enabled = value;
|
||||
}
|
||||
|
||||
public void put(String key, Object value) {
|
||||
if (enabled)
|
||||
data.compute(key, (k, v) -> {
|
||||
v = v == null ? new Hashtable<>(SAMPLE_BASE / SAMPLE_RATE, 1) : v;
|
||||
v.put(getTime(), value);
|
||||
return v;
|
||||
});
|
||||
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 exportPath() throws IOException {
|
||||
List<Trajectory.State> states = data.get("poseMeters").entrySet().stream().map(entry -> {
|
||||
double timeSeconds = entry.getKey();
|
||||
double velocityMetersPerSecond = 0;
|
||||
double accelerationMetersPerSecondSq = 0;
|
||||
Pose2d poseMeters = (Pose2d) entry.getValue();
|
||||
double curvatureRadPerMeter = 0;
|
||||
return new Trajectory.State(timeSeconds, velocityMetersPerSecond, accelerationMetersPerSecondSq, poseMeters, curvatureRadPerMeter);
|
||||
}).collect(Collectors.toUnmodifiableList());
|
||||
String pathPath = "paths/" + System.nanoTime() + ".wpilib.json";
|
||||
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();
|
||||
TrajectoryUtil.toPathweaverJson(new Trajectory(states), outputPath);
|
||||
PathPlannerTrajectoryUtil.toPathweaverJson(states, outputPath);
|
||||
return pathPath;
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user