diff --git a/src/main/deploy/NewPath.json b/src/main/deploy/trajectories/NewPath.json similarity index 100% rename from src/main/deploy/NewPath.json rename to src/main/deploy/trajectories/NewPath.json diff --git a/src/main/java/org/littletonrobotics/frc2023/util/trajectory/ChoreoTrajectoryFactory.java b/src/main/java/org/littletonrobotics/frc2023/util/trajectory/ChoreoTrajectoryFactory.java deleted file mode 100644 index 97fa3ec..0000000 --- a/src/main/java/org/littletonrobotics/frc2023/util/trajectory/ChoreoTrajectoryFactory.java +++ /dev/null @@ -1,148 +0,0 @@ -// Copyright (c) 2023 FRC 6328 -// http://github.com/Mechanical-Advantage -// -// Use of this source code is governed by an MIT-style -// license that can be found in the LICENSE file at -// the root directory of this project. - -package org.littletonrobotics.frc2023.util.trajectory; - -import static org.littletonrobotics.frc2023.util.trajectory.CustomSwerveDriveController.DriveDynamicState; - -import com.fasterxml.jackson.core.type.TypeReference; -import com.fasterxml.jackson.databind.ObjectMapper; -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.Filesystem; -import java.io.File; -import java.io.IOException; -import java.util.Collections; -import java.util.HashMap; -import java.util.List; -import java.util.Optional; - -public class ChoreoTrajectoryFactory { - private static final ObjectMapper mapper = new ObjectMapper(); - private static final HashMap FILE_HASH_MAP = new HashMap<>(); - // load trajectory files into map - static { - File trajectory_dir = Filesystem.getDeployDirectory(); - if (trajectory_dir.isDirectory() && trajectory_dir.listFiles() != null) { - for (File file : trajectory_dir.listFiles()) { - String fileName = file.getName(); - // check if json - int pointIndex = fileName.lastIndexOf("."); - String type = fileName.substring(pointIndex); - if (!type.equals(".json")) continue; - // use name of file without ".json" - String trimmedName = fileName.substring(0, pointIndex - 1); - FILE_HASH_MAP.put(trimmedName, file); - } - } - } - - // TODO: does this add another unnecessary purpose to this file? - - /** - * @param trajectoryName Name of trajectory file without ".json" - * @return Optional of trajectory file - */ - public static Optional getTrajectoryFile(String trajectoryName) { - if (!FILE_HASH_MAP.containsKey(trajectoryName)) { - DriverStation.reportWarning("No trajectory with name: " + trajectoryName, false); - return Optional.empty(); - } - return Optional.of(FILE_HASH_MAP.get(trajectoryName)); - } - - /** - * @param trajectoryFile File to make trajectory from - * @return - */ - public static TrajectoryImplementation createTrajectoryFromFile(File trajectoryFile) { - try { - return createTrajectoryImplementation( - mapper.readValue(trajectoryFile, new TypeReference<>() {})); - } catch (IOException e) { - e.printStackTrace(); - return createTrajectoryImplementation(Collections.emptyList()); - } - } - - private static TrajectoryImplementation createTrajectoryImplementation( - List states) { - return new TrajectoryImplementation() { - @Override - public double getDuration() { - return states.get(states.size() - 1).timestamp(); - } - - @Override - public Pose2d[] getTrajectoryPoses() { - return states.stream() - .map(state -> new Pose2d(state.x(), state.y(), new Rotation2d(state.heading()))) - .toArray(Pose2d[]::new); - } - - @Override - public DriveDynamicState sample(double time) { - ChoreoTrajectoryState before = null; - ChoreoTrajectoryState after = null; - - for (ChoreoTrajectoryState state : states) { - if (time == state.timestamp()) { - return ChoreoTrajectoryState.toDynamicState(state); - } - - if (state.timestamp() < time) { - before = state; - } else { - after = state; - break; - } - } - - if (before == null) return ChoreoTrajectoryState.toDynamicState(states.get(0)); - - if (after == null) - return ChoreoTrajectoryState.toDynamicState(states.get(states.size() - 1)); - - double s = 1 - ((after.timestamp() - time) / (after.timestamp() - before.timestamp())); - - DriveDynamicState beforeState = ChoreoTrajectoryState.toDynamicState(before); - DriveDynamicState afterState = ChoreoTrajectoryState.toDynamicState(after); - - Pose2d interpolatedPose = beforeState.pose().interpolate(afterState.pose(), s); - double interpolatedVelocityX = - MathUtil.interpolate(beforeState.velocityX(), afterState.velocityX(), s); - double interpolatedVelocityY = - MathUtil.interpolate(beforeState.velocityY(), afterState.velocityY(), s); - double interpolatedAngularVelocity = - MathUtil.interpolate(beforeState.angularVelocity(), afterState.angularVelocity(), s); - - return new DriveDynamicState( - interpolatedPose, - interpolatedVelocityX, - interpolatedVelocityY, - interpolatedAngularVelocity); - } - }; - } - - private record ChoreoTrajectoryState( - double timestamp, - double x, - double y, - double heading, - double velocityX, - double velocityY, - double angularVelocity) { - public static DriveDynamicState toDynamicState(ChoreoTrajectoryState state) { - Pose2d pose = new Pose2d(state.x(), state.y(), new Rotation2d(state.heading())); - return new DriveDynamicState( - pose, state.velocityX(), state.velocityY(), state.angularVelocity()); - } - } -} diff --git a/src/main/java/org/littletonrobotics/frc2023/util/trajectory/CustomSwerveDriveController.java b/src/main/java/org/littletonrobotics/frc2023/util/trajectory/CustomSwerveDriveController.java deleted file mode 100644 index 9da51db..0000000 --- a/src/main/java/org/littletonrobotics/frc2023/util/trajectory/CustomSwerveDriveController.java +++ /dev/null @@ -1,68 +0,0 @@ -// Copyright (c) 2023 FRC 6328 -// http://github.com/Mechanical-Advantage -// -// Use of this source code is governed by an MIT-style -// license that can be found in the LICENSE file at -// the root directory of this project. - -package org.littletonrobotics.frc2023.util.trajectory; - -import static org.littletonrobotics.frc2023.util.AllianceFlipUtil.*; - -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; - -public class CustomSwerveDriveController { - private PIDController xController, yController, thetaController; - - private Pose2d m_poseError; - private Rotation2d m_rotationError; - - public CustomSwerveDriveController( - PIDController xController, PIDController yController, PIDController thetaController) { - this.xController = xController; - this.yController = yController; - this.thetaController = thetaController; - - this.thetaController.enableContinuousInput(-Math.PI, Math.PI); - } - - public ChassisSpeeds calculate(Pose2d currentPose, DriveDynamicState state) { - // Calculate feedforward velocities (field-relative). - double xFF = state.velocityX(); - double yFF = state.velocityY(); - double thetaFF = state.angularVelocity(); - - Pose2d poseRef = state.pose(); - - m_poseError = poseRef.relativeTo(currentPose); - m_rotationError = poseRef.getRotation().minus(currentPose.getRotation()); - - // Calculate feedback velocities (based on position error). - double xFeedback = xController.calculate(currentPose.getX(), poseRef.getX()); - double yFeedback = yController.calculate(currentPose.getY(), poseRef.getY()); - double thetaFeedback = - thetaController.calculate( - currentPose.getRotation().getRadians(), poseRef.getRotation().getRadians()); - - // Return next output. - return ChassisSpeeds.fromFieldRelativeSpeeds( - xFF + xFeedback, yFF + yFeedback, thetaFF + thetaFeedback, currentPose.getRotation()); - } - - public record DriveDynamicState( - Pose2d pose, double velocityX, double velocityY, double angularVelocity) { - public DriveDynamicState maybeFlip() { - double x = apply(pose.getX()); - double y = pose.getY(); - Rotation2d heading = apply(pose.getRotation()); - return new DriveDynamicState( - new Pose2d(x, y, heading), - shouldFlip() ? -velocityX : velocityX, - velocityY, - shouldFlip() ? -angularVelocity : angularVelocity); - } - } -} diff --git a/src/main/java/org/littletonrobotics/frc2023/util/trajectory/CustomTrajectoryFactory.java b/src/main/java/org/littletonrobotics/frc2023/util/trajectory/CustomTrajectoryFactory.java deleted file mode 100644 index d2084a7..0000000 --- a/src/main/java/org/littletonrobotics/frc2023/util/trajectory/CustomTrajectoryFactory.java +++ /dev/null @@ -1,57 +0,0 @@ -// Copyright (c) 2023 FRC 6328 -// http://github.com/Mechanical-Advantage -// -// Use of this source code is governed by an MIT-style -// license that can be found in the LICENSE file at -// the root directory of this project. - -package org.littletonrobotics.frc2023.util.trajectory; - -import static org.littletonrobotics.frc2023.util.trajectory.CustomSwerveDriveController.DriveDynamicState; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.trajectory.Trajectory; -import edu.wpi.first.math.trajectory.TrajectoryConfig; -import java.util.List; -import org.littletonrobotics.frc2023.util.AllianceFlipUtil; - -public class CustomTrajectoryFactory { - public static TrajectoryImplementation createFromWaypoints( - TrajectoryConfig config, List waypoints) { - CustomTrajectoryGenerator trajectoryGenerator = new CustomTrajectoryGenerator(); - trajectoryGenerator.generate(config, waypoints); - - return new TrajectoryImplementation() { - @Override - public double getDuration() { - return trajectoryGenerator.getDriveTrajectory().getTotalTimeSeconds(); - } - - @Override - public Pose2d[] getTrajectoryPoses() { - return trajectoryGenerator.getDriveTrajectory().getStates().stream() - .map(state -> AllianceFlipUtil.apply(state.poseMeters)) - .toArray(Pose2d[]::new); - } - - @Override - public DriveDynamicState sample(double time) { - Trajectory.State driveState = trajectoryGenerator.getDriveTrajectory().sample(time); - RotationSequence.State rotationState = - trajectoryGenerator.getHolonomicRotationSequence().sample(time); - - // TODO: Check if this is actually what I need to do - double velocityX = - driveState.poseMeters.getRotation().getCos() * driveState.velocityMetersPerSecond; - double velocityY = - driveState.poseMeters.getRotation().getSin() * driveState.velocityMetersPerSecond; - - return new DriveDynamicState( - new Pose2d(driveState.poseMeters.getTranslation(), rotationState.position), - velocityX, - velocityY, - rotationState.velocityRadiansPerSec); - } - }; - } -} diff --git a/src/main/java/org/littletonrobotics/frc2023/util/trajectory/TrajectoryImplementation.java b/src/main/java/org/littletonrobotics/frc2023/util/trajectory/TrajectoryImplementation.java deleted file mode 100644 index 589c622..0000000 --- a/src/main/java/org/littletonrobotics/frc2023/util/trajectory/TrajectoryImplementation.java +++ /dev/null @@ -1,18 +0,0 @@ -// Copyright (c) 2023 FRC 6328 -// http://github.com/Mechanical-Advantage -// -// Use of this source code is governed by an MIT-style -// license that can be found in the LICENSE file at -// the root directory of this project. - -package org.littletonrobotics.frc2023.util.trajectory; - -import edu.wpi.first.math.geometry.Pose2d; - -public interface TrajectoryImplementation { - double getDuration(); - - Pose2d[] getTrajectoryPoses(); - - CustomSwerveDriveController.DriveDynamicState sample(double time); -} diff --git a/src/main/java/org/littletonrobotics/frc2023/util/trajectory/updated/ChoreoTrajectoryFactory.java b/src/main/java/org/littletonrobotics/frc2023/util/trajectory/updated/ChoreoTrajectoryFactory.java index 56772ba..27a1be8 100644 --- a/src/main/java/org/littletonrobotics/frc2023/util/trajectory/updated/ChoreoTrajectoryFactory.java +++ b/src/main/java/org/littletonrobotics/frc2023/util/trajectory/updated/ChoreoTrajectoryFactory.java @@ -28,16 +28,16 @@ public class ChoreoTrajectoryFactory { private static final HashMap FILE_HASH_MAP = new HashMap<>(); // load trajectory files into map static { - File trajectory_dir = Filesystem.getDeployDirectory(); + File trajectory_dir = new File(Filesystem.getDeployDirectory(), "trajectories"); if (trajectory_dir.isDirectory() && trajectory_dir.listFiles() != null) { for (File file : trajectory_dir.listFiles()) { String fileName = file.getName(); // check if json - int pointIndex = fileName.lastIndexOf("."); - String type = fileName.substring(pointIndex); + int pointIndex = fileName.lastIndexOf('.'); + String type = (pointIndex == -1) ? "not json" : fileName.substring(pointIndex); if (!type.equals(".json")) continue; // use name of file without ".json" - String trimmedName = fileName.substring(0, pointIndex - 1); + String trimmedName = fileName.substring(0, pointIndex); FILE_HASH_MAP.put(trimmedName, file); } }