diff --git a/build.gradle b/build.gradle index 996bdec..ab45437 100644 --- a/build.gradle +++ b/build.gradle @@ -55,6 +55,8 @@ def includeDesktopSupport = true dependencies { implementation wpi.java.deps.wpilib() implementation wpi.java.vendor.java() + implementation 'com.diffplug.durian:durian:3.4.0' + implementation 'com.fasterxml.jackson.datatype:jackson-datatype-jdk8:2.13.1' implementation 'org.fusesource.jansi:jansi:2.4.0' roborioDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.roborio) diff --git a/simgui-ds.json b/simgui-ds.json index c4b7efd..69b1a3c 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -91,8 +91,7 @@ ], "robotJoysticks": [ { - "guid": "78696e70757401000000000000000000", - "useGamepad": true + "guid": "Keyboard0" } ] } diff --git a/src/main/java/frc4388/robot/Main.java b/src/main/java/frc4388/robot/Main.java index 21f4236..64dac2f 100644 --- a/src/main/java/frc4388/robot/Main.java +++ b/src/main/java/frc4388/robot/Main.java @@ -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); } } diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 3b8eaae..c564b4f 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -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(); - } } /** diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index a26b704..dd42b7e 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -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. */ diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index ddb30f1..4322d65 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -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()); diff --git a/src/main/java/frc4388/utility/PathPlannerTrajectoryUtil.java b/src/main/java/frc4388/utility/PathPlannerTrajectoryUtil.java new file mode 100644 index 0000000..0f00f92 --- /dev/null +++ b/src/main/java/frc4388/utility/PathPlannerTrajectoryUtil.java @@ -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 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 new file mode 100644 index 0000000..e2495aa --- /dev/null +++ b/src/main/java/frc4388/utility/PathPlannerUtil.java @@ -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[] waypoints; + + public static final class Waypoint { + public Optional anchorPoint; + public Optional prevControl; + public Optional nextControl; + + public static final class Point { + public Optional x; + public Optional y; + } + + public Optional holonomicAngle; + public Optional isReversal; + public Optional velOverride; + public Optional isLocked; + } + + public Optional maxVelocity; + public Optional maxAcceleration; + public Optional 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()); + } + } +} diff --git a/src/main/java/frc4388/utility/RobotLogger.java b/src/main/java/frc4388/utility/RobotLogger.java index fed25f0..c352730 100644 --- a/src/main/java/frc4388/utility/RobotLogger.java +++ b/src/main/java/frc4388/utility/RobotLogger.java @@ -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> data; + private final List 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 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 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; } }