diff --git a/src/main/deploy/pathplanner/autos/Example Auto.auto b/src/main/deploy/pathplanner/autos/Example Auto.auto index 77a8433f..0de36e00 100644 --- a/src/main/deploy/pathplanner/autos/Example Auto.auto +++ b/src/main/deploy/pathplanner/autos/Example Auto.auto @@ -20,7 +20,7 @@ { "type": "path", "data": { - "pathName": "Example Path" + "pathName": "ShootAndTaxi1" } } ] diff --git a/src/main/deploy/pathplanner/paths/Example Path.path b/src/main/deploy/pathplanner/paths/Example Path.path deleted file mode 100644 index 8f3609bb..00000000 --- a/src/main/deploy/pathplanner/paths/Example Path.path +++ /dev/null @@ -1,65 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 2.0, - "y": 7.0 - }, - "prevControl": null, - "nextControl": { - "x": 3.013282910175676, - "y": 6.5274984191074985 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 5.166769560390973, - "y": 5.0964860911203305 - }, - "prevControl": { - "x": 4.166769560390973, - "y": 6.0964860911203305 - }, - "nextControl": { - "x": 6.166769560390973, - "y": 4.0964860911203305 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 7.0, - "y": 1.0 - }, - "prevControl": { - "x": 6.75, - "y": 2.5 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": 0, - "rotateFast": false - }, - "reversed": false, - "folder": null, - "previewStartingState": null, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/java/frc/robot/FieldConstants.java b/src/main/java/frc/robot/FieldConstants.java new file mode 100644 index 00000000..631ec0e5 --- /dev/null +++ b/src/main/java/frc/robot/FieldConstants.java @@ -0,0 +1,51 @@ +package frc.robot; + +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.util.Units; + +/** + * Contains various field dimensions and useful reference points. Dimensions are in meters, and sets + * of corners start in the lower left moving clockwise. All units in Meters
+ *
+ * + *

All translations and poses are stored with the origin at the rightmost point on the BLUE + * ALLIANCE wall.
+ *
+ * Length refers to the x direction (as described by wpilib)
+ * Width refers to the y direction (as described by wpilib) + */ +public class FieldConstants { + public static double fieldLength = Units.inchesToMeters(653.22); + public static double fieldWidth = Units.inchesToMeters(323.28); + public static double wingX = Units.inchesToMeters(229.201); + + public static double podiumX = Units.inchesToMeters(121); + public static double startingLineX = Units.inchesToMeters(74.111); + + public static final class StagingLocations { + public static double centerlineX = Units.inchesToMeters(fieldLength / 2); + // need to update + public static double centerlineFirstY = Units.inchesToMeters(-1); + public static double centerlineSeparationY = Units.inchesToMeters(66); + public static double spikeX = Units.inchesToMeters(114); + // need + public static double spikeFirstY = Units.inchesToMeters(-1); + public static double spikeSeparationY = Units.inchesToMeters(57); + + public static Translation2d[] centerlineTranslations = new Translation2d[5]; + public static Translation2d[] spikeTranslations = new Translation2d[3]; + + static { + for (int i = 0; i < centerlineTranslations.length; i++) { + centerlineTranslations[i] = + new Translation2d(centerlineX, centerlineFirstY + (i * centerlineSeparationY)); + } + } + + static { + for (int i = 0; i < spikeTranslations.length; i++) { + spikeTranslations[i] = new Translation2d(spikeX, spikeFirstY + (i * spikeSeparationY)); + } + } + } +} diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index d72b2e1e..8c9fc807 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -13,7 +13,6 @@ package frc.robot; -import com.pathplanner.lib.auto.AutoBuilder; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.GenericHID; @@ -98,14 +97,7 @@ public RobotContainer() { break; } - // Set up auto routines - // NamedCommands.registerCommand( - // "Run Flywheel", - // Commands.startEnd( - // () -> flywheel.runVelocity(flywheelSpeedInput.get()), flywheel::stop, - // flywheel) - // .withTimeout(5.0)); - autoChooser = new LoggedDashboardChooser<>("Auto Choices", AutoBuilder.buildAutoChooser()); + autoChooser = new LoggedDashboardChooser<>("Auto Choices"); // Set up feedforward characterization autoChooser.addOption( @@ -148,6 +140,11 @@ private void configureButtonBindings() { new Pose2d(drive.getPose().getTranslation(), new Rotation2d())), drive) .ignoringDisable(true)); + controller + .a() + .whileTrue( + Commands.run(() -> shooter.runFlywheelVelocity(flywheelSpeedInput.get()), shooter)) + .whileFalse(Commands.run(() -> shooter.runFlywheelVelocity(0.0), shooter)); } /** diff --git a/src/main/java/frc/robot/commands/FeedForwardCharacterization.java b/src/main/java/frc/robot/commands/FeedForwardCharacterization.java index d7ac7a7a..391a3d31 100644 --- a/src/main/java/frc/robot/commands/FeedForwardCharacterization.java +++ b/src/main/java/frc/robot/commands/FeedForwardCharacterization.java @@ -24,7 +24,7 @@ public class FeedForwardCharacterization extends Command { private static final double START_DELAY_SECS = 2.0; - private static final double RAMP_VOLTS_PER_SEC = 0.1; + private static final double RAMP_VOLTS_PER_SEC = 0.4; private FeedForwardCharacterizationData data; private final Consumer voltageConsumer; diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index f36eb6ef..ac2f489c 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -13,11 +13,6 @@ package frc.robot.subsystems.drive; -import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.pathfinding.Pathfinding; -import com.pathplanner.lib.util.HolonomicPathFollowerConfig; -import com.pathplanner.lib.util.PathPlannerLogging; -import com.pathplanner.lib.util.ReplanningConfig; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; @@ -28,18 +23,16 @@ import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.util.LocalADStarAK; import java.util.concurrent.locks.Lock; import java.util.concurrent.locks.ReentrantLock; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; public class Drive extends SubsystemBase { - private static final double MAX_LINEAR_SPEED = Units.feetToMeters(14.5); - private static final double TRACK_WIDTH_X = Units.inchesToMeters(25.0); - private static final double TRACK_WIDTH_Y = Units.inchesToMeters(25.0); + private static final double MAX_LINEAR_SPEED = Units.inchesToMeters(140.0); + private static final double TRACK_WIDTH_X = Units.inchesToMeters(26.0); + private static final double TRACK_WIDTH_Y = Units.inchesToMeters(26.0); private static final double DRIVE_BASE_RADIUS = Math.hypot(TRACK_WIDTH_X / 2.0, TRACK_WIDTH_Y / 2.0); private static final double MAX_ANGULAR_SPEED = MAX_LINEAR_SPEED / DRIVE_BASE_RADIUS; @@ -64,29 +57,6 @@ public Drive( modules[1] = new Module(frModuleIO, 1); modules[2] = new Module(blModuleIO, 2); modules[3] = new Module(brModuleIO, 3); - - // Configure AutoBuilder for PathPlanner - AutoBuilder.configureHolonomic( - this::getPose, - this::setPose, - () -> kinematics.toChassisSpeeds(getModuleStates()), - this::runVelocity, - new HolonomicPathFollowerConfig( - MAX_LINEAR_SPEED, DRIVE_BASE_RADIUS, new ReplanningConfig()), - () -> - DriverStation.getAlliance().isPresent() - && DriverStation.getAlliance().get() == Alliance.Red, - this); - Pathfinding.setPathfinder(new LocalADStarAK()); - PathPlannerLogging.setLogActivePathCallback( - (activePath) -> { - Logger.recordOutput( - "Odometry/Trajectory", activePath.toArray(new Pose2d[activePath.size()])); - }); - PathPlannerLogging.setLogTargetPoseCallback( - (targetPose) -> { - Logger.recordOutput("Odometry/TrajectorySetpoint", targetPose); - }); } public void periodic() { diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java new file mode 100644 index 00000000..70e571ef --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java @@ -0,0 +1,3 @@ +package frc.robot.subsystems.drive; + +public class DriveConstants {} diff --git a/src/main/java/frc/robot/subsystems/kitbotshooter/KitbotFeederIO.java b/src/main/java/frc/robot/subsystems/kitbotshooter/KitbotFeederIO.java index 9f672e13..bb261025 100644 --- a/src/main/java/frc/robot/subsystems/kitbotshooter/KitbotFeederIO.java +++ b/src/main/java/frc/robot/subsystems/kitbotshooter/KitbotFeederIO.java @@ -4,14 +4,14 @@ public interface KitbotFeederIO { @AutoLog - class FeederIOInputs { + class KitbotFeederIOInputs { public double feederPositionRads = 0.0; public double feederVelocityRadPerSec = 0.0; public double feederAppliedVolts = 0.0; public double feederCurrentAmps = 0.0; } - default void updateInputs(FeederIOInputs inputs) {} + default void updateInputs(KitbotFeederIOInputs inputs) {} ; /** run at voltage */ diff --git a/src/main/java/frc/robot/subsystems/kitbotshooter/KitbotFeederIOSim.java b/src/main/java/frc/robot/subsystems/kitbotshooter/KitbotFeederIOSim.java index 27daadf5..d2448920 100644 --- a/src/main/java/frc/robot/subsystems/kitbotshooter/KitbotFeederIOSim.java +++ b/src/main/java/frc/robot/subsystems/kitbotshooter/KitbotFeederIOSim.java @@ -12,7 +12,7 @@ public class KitbotFeederIOSim implements KitbotFeederIO { private double inputVolts = 0.0; @Override - public void updateInputs(FeederIOInputs inputs) { + public void updateInputs(KitbotFeederIOInputs inputs) { positionRads += sim.getAngularVelocityRadPerSec() * (Constants.loopPeriodMs / 1000.0); inputs.feederPositionRads = positionRads; inputs.feederVelocityRadPerSec = sim.getAngularVelocityRadPerSec(); diff --git a/src/main/java/frc/robot/subsystems/kitbotshooter/KitbotFeederIOSparkMax.java b/src/main/java/frc/robot/subsystems/kitbotshooter/KitbotFeederIOSparkMax.java index f284ef2e..b04304ab 100644 --- a/src/main/java/frc/robot/subsystems/kitbotshooter/KitbotFeederIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/kitbotshooter/KitbotFeederIOSparkMax.java @@ -30,7 +30,7 @@ public KitbotFeederIOSparkMax() { } @Override - public void updateInputs(FeederIOInputs inputs) { + public void updateInputs(KitbotFeederIOInputs inputs) { inputs.feederPositionRads = Units.rotationsToRadians(encoder.getPosition()) / GEARING; inputs.feederVelocityRadPerSec = Units.rotationsToRadians(encoder.getVelocity()) / GEARING; inputs.feederAppliedVolts = motor.getAppliedOutput(); diff --git a/src/main/java/frc/robot/subsystems/kitbotshooter/KitbotFlywheelIO.java b/src/main/java/frc/robot/subsystems/kitbotshooter/KitbotFlywheelIO.java index d35b17d6..4b630ab4 100644 --- a/src/main/java/frc/robot/subsystems/kitbotshooter/KitbotFlywheelIO.java +++ b/src/main/java/frc/robot/subsystems/kitbotshooter/KitbotFlywheelIO.java @@ -4,14 +4,14 @@ public interface KitbotFlywheelIO { @AutoLog - class FlywheelIOInputs { + class KitbotFlywheelIOInputs { public double[] flywheelPositionRads = new double[] {}; public double[] flywheelVelocityRadPerSec = new double[] {}; public double[] flywheelAppliedVolts = new double[] {}; public double[] flywheelCurrentAmps = new double[] {}; } - default void updateInputs(FlywheelIOInputs inputs) {} + default void updateInputs(KitbotFlywheelIOInputs inputs) {} ; /** run to velocity */ diff --git a/src/main/java/frc/robot/subsystems/kitbotshooter/KitbotFlywheelIOSim.java b/src/main/java/frc/robot/subsystems/kitbotshooter/KitbotFlywheelIOSim.java index 6aa87c73..eeb4213a 100644 --- a/src/main/java/frc/robot/subsystems/kitbotshooter/KitbotFlywheelIOSim.java +++ b/src/main/java/frc/robot/subsystems/kitbotshooter/KitbotFlywheelIOSim.java @@ -19,7 +19,7 @@ public class KitbotFlywheelIOSim implements KitbotFlywheelIO { private double inputVolts = 0.0; @Override - public void updateInputs(FlywheelIOInputs inputs) { + public void updateInputs(KitbotFlywheelIOInputs inputs) { positionRads += sim.getAngularVelocityRadPerSec() * (Constants.loopPeriodMs / 1000.0); inputs.flywheelPositionRads = new double[] {positionRads, positionRads}; inputs.flywheelVelocityRadPerSec = new double[2]; diff --git a/src/main/java/frc/robot/subsystems/kitbotshooter/KitbotFlywheelIOSparkMax.java b/src/main/java/frc/robot/subsystems/kitbotshooter/KitbotFlywheelIOSparkMax.java index bf771915..45c76da1 100644 --- a/src/main/java/frc/robot/subsystems/kitbotshooter/KitbotFlywheelIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/kitbotshooter/KitbotFlywheelIOSparkMax.java @@ -54,7 +54,7 @@ public KitbotFlywheelIOSparkMax() { } @Override - public void updateInputs(FlywheelIOInputs inputs) { + public void updateInputs(KitbotFlywheelIOInputs inputs) { inputs.flywheelPositionRads = getPosition(); inputs.flywheelVelocityRadPerSec = getVelocity(); inputs.flywheelAppliedVolts = diff --git a/src/main/java/frc/robot/subsystems/kitbotshooter/KitbotShooter.java b/src/main/java/frc/robot/subsystems/kitbotshooter/KitbotShooter.java index bd1f03b1..3a7b4bc0 100644 --- a/src/main/java/frc/robot/subsystems/kitbotshooter/KitbotShooter.java +++ b/src/main/java/frc/robot/subsystems/kitbotshooter/KitbotShooter.java @@ -16,8 +16,8 @@ public class KitbotShooter extends SubsystemBase { private KitbotFlywheelIO kitbotFlywheelIO; private KitbotFeederIO kitbotFeederIO; - private FlywheelIOInputsAutoLogged flywheelInputs = new FlywheelIOInputsAutoLogged(); - private FeederIOInputsAutoLogged feederInputs = new FeederIOInputsAutoLogged(); + private KitbotFlywheelIOInputsAutoLogged flywheelInputs = new KitbotFlywheelIOInputsAutoLogged(); + private KitbotFeederIOInputsAutoLogged feederInputs = new KitbotFeederIOInputsAutoLogged(); private Mode currentMode = null; diff --git a/src/main/java/frc/robot/util/AllianceFlipUtil.java b/src/main/java/frc/robot/util/AllianceFlipUtil.java new file mode 100644 index 00000000..c60bc6f1 --- /dev/null +++ b/src/main/java/frc/robot/util/AllianceFlipUtil.java @@ -0,0 +1,66 @@ +package frc.robot.util; + +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.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import frc.robot.FieldConstants; +import frc.robot.util.trajectory.HolonomicDriveController; + +/** Utility functions for flipping from the blue to red alliance. */ +public class AllianceFlipUtil { + /** Flips an x coordinate to the correct side of the field based on the current alliance color. */ + public static double apply(double xCoordinate) { + if (shouldFlip()) { + return FieldConstants.fieldLength - xCoordinate; + } else { + return xCoordinate; + } + } + + /** Flips a translation to the correct side of the field based on the current alliance color. */ + public static Translation2d apply(Translation2d translation) { + if (shouldFlip()) { + return new Translation2d(apply(translation.getX()), translation.getY()); + } else { + return translation; + } + } + + /** Flips a rotation based on the current alliance color. */ + public static Rotation2d apply(Rotation2d rotation) { + if (shouldFlip()) { + return new Rotation2d(-rotation.getCos(), rotation.getSin()); + } else { + return rotation; + } + } + + /** Flips a pose to the correct side of the field based on the current alliance color. */ + public static Pose2d apply(Pose2d pose) { + if (shouldFlip()) { + return new Pose2d(apply(pose.getTranslation()), apply(pose.getRotation())); + } else { + return pose; + } + } + + /** + * Flips a trajectory state to the correct side of the field based on the current alliance color. + */ + public static HolonomicDriveController.HolonomicDriveState apply( + HolonomicDriveController.HolonomicDriveState state) { + if (shouldFlip()) { + return new HolonomicDriveController.HolonomicDriveState( + apply(state.pose()), -state.velocityX(), state.velocityY(), -state.angularVelocity()); + } else { + return state; + } + } + + public static boolean shouldFlip() { + return DriverStation.getAlliance().isPresent() + && DriverStation.getAlliance().get() == Alliance.Red; + } +} diff --git a/src/main/java/frc/robot/util/GeomUtil.java b/src/main/java/frc/robot/util/GeomUtil.java new file mode 100644 index 00000000..fa87fa4f --- /dev/null +++ b/src/main/java/frc/robot/util/GeomUtil.java @@ -0,0 +1,143 @@ +package frc.robot.util; +// 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. + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.geometry.Twist2d; + +/** Geometry utilities for working with translations, rotations, transforms, and poses. */ +public class GeomUtil { + /** + * Creates a pure translating transform + * + * @param translation The translation to create the transform with + * @return The resulting transform + */ + public static Transform2d translationToTransform(Translation2d translation) { + return new Transform2d(translation, new Rotation2d()); + } + + /** + * Creates a pure translating transform + * + * @param x The x componenet of the translation + * @param y The y componenet of the translation + * @return The resulting transform + */ + public static Transform2d translationToTransform(double x, double y) { + return new Transform2d(new Translation2d(x, y), new Rotation2d()); + } + + /** + * Creates a pure rotating transform + * + * @param rotation The rotation to create the transform with + * @return The resulting transform + */ + public static Transform2d rotationToTransform(Rotation2d rotation) { + return new Transform2d(new Translation2d(), rotation); + } + + /** + * Converts a Pose2d to a Transform2d to be used in a kinematic chain + * + * @param pose The pose that will represent the transform + * @return The resulting transform + */ + public static Transform2d poseToTransform(Pose2d pose) { + return new Transform2d(pose.getTranslation(), pose.getRotation()); + } + + /** + * Converts a Transform2d to a Pose2d to be used as a position or as the start of a kinematic + * chain + * + * @param transform The transform that will represent the pose + * @return The resulting pose + */ + public static Pose2d transformToPose(Transform2d transform) { + return new Pose2d(transform.getTranslation(), transform.getRotation()); + } + + /** + * Creates a pure translated pose + * + * @param translation The translation to create the pose with + * @return The resulting pose + */ + public static Pose2d translationToPose(Translation2d translation) { + return new Pose2d(translation, new Rotation2d()); + } + + /** + * Creates a pure rotated pose + * + * @param rotation The rotation to create the pose with + * @return The resulting pose + */ + public static Pose2d rotationToPose(Rotation2d rotation) { + return new Pose2d(new Translation2d(), rotation); + } + + /** + * Multiplies a twist by a scaling factor + * + * @param twist The twist to multiply + * @param factor The scaling factor for the twist components + * @return The new twist + */ + public static Twist2d multiplyTwist(Twist2d twist, double factor) { + return new Twist2d(twist.dx * factor, twist.dy * factor, twist.dtheta * factor); + } + + /** + * Converts a Pose3d to a Transform3d to be used in a kinematic chain + * + * @param pose The pose that will represent the transform + * @return The resulting transform + */ + public static Transform3d pose3dToTransform3d(Pose3d pose) { + return new Transform3d(pose.getTranslation(), pose.getRotation()); + } + + /** + * Converts a Transform3d to a Pose3d to be used as a position or as the start of a kinematic + * chain + * + * @param transform The transform that will represent the pose + * @return The resulting pose + */ + public static Pose3d transform3dToPose3d(Transform3d transform) { + return new Pose3d(transform.getTranslation(), transform.getRotation()); + } + + /** + * Converts a Translation3d to a Translation2d by extracting two dimensions (X and Y). chain + * + * @param transform The original translation + * @return The resulting translation + */ + public static Translation2d translation3dTo2dXY(Translation3d translation) { + return new Translation2d(translation.getX(), translation.getY()); + } + + /** + * Converts a Translation3d to a Translation2d by extracting two dimensions (X and Z). chain + * + * @param transform The original translation + * @return The resulting translation + */ + public static Translation2d translation3dTo2dXZ(Translation3d translation) { + return new Translation2d(translation.getX(), translation.getZ()); + } +} diff --git a/src/main/java/frc/robot/util/trajectory/ChoreoTrajectoryReader.java b/src/main/java/frc/robot/util/trajectory/ChoreoTrajectoryReader.java new file mode 100644 index 00000000..59917753 --- /dev/null +++ b/src/main/java/frc/robot/util/trajectory/ChoreoTrajectoryReader.java @@ -0,0 +1,124 @@ +package frc.robot.util.trajectory; + +import static frc.robot.util.trajectory.HolonomicDriveController.HolonomicDriveState; + +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 java.io.File; +import java.io.IOException; +import java.util.ArrayList; +import java.util.List; +import java.util.Optional; + +public final class ChoreoTrajectoryReader { + private static ObjectMapper objectMapper = new ObjectMapper(); + + /** Load Trajectory file (.traj) */ + public Optional generate(File file) { + String type = file.getName().substring(file.getName().lastIndexOf('.')); + // Not generated from code or choreo + if (!type.equals(".traj") || !type.equals(".json")) return Optional.empty(); + + List choreoStates = new ArrayList<>(); + try { + choreoStates = objectMapper.readValue(file, new TypeReference<>() {}); + } catch (IOException e) { + System.out.println("Failed to read states from file"); + return Optional.empty(); + } + + // Generate trajectory object + return Optional.of(generateTrajectoryImplementation(choreoStates)); + } + + private static Trajectory generateTrajectoryImplementation( + final List states) { + return new Trajectory() { + @Override + public double getTotatTimeSeconds() { + 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 HolonomicDriveState sample(double timeSeconds) { + ChoreoTrajectoryState before = null; + ChoreoTrajectoryState after = null; + + for (ChoreoTrajectoryState state : states) { + if (state.timestamp() == timeSeconds) { + return fromChoreoState(state); + } + + if (state.timestamp() < timeSeconds) { + before = state; + } else { + after = state; + break; + } + } + + if (before == null) { + return fromChoreoState(states.get(0)); + } + + if (after == null) { + return fromChoreoState(states.get(states.size() - 1)); + } + + double s = (timeSeconds - before.timestamp()) / (after.timestamp() - before.timestamp()); + + HolonomicDriveState beforeState = fromChoreoState(before); + HolonomicDriveState afterState = fromChoreoState(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 HolonomicDriveState( + interpolatedPose, + interpolatedVelocityX, + interpolatedVelocityY, + interpolatedAngularVelocity); + } + }; + } + + private static HolonomicDriveState fromChoreoState(ChoreoTrajectoryState state) { + return new HolonomicDriveState( + new Pose2d(state.x(), state.y(), new Rotation2d(state.heading())), + state.velocityX(), + state.velocityY(), + state.angularVelocity()); + } + + interface Trajectory { + double getTotatTimeSeconds(); + + Pose2d[] getTrajectoryPoses(); + + HolonomicDriveState sample(double timeSeconds); + } + + record ChoreoTrajectoryState( + double timestamp, + double x, + double y, + double heading, + double velocityX, + double velocityY, + double angularVelocity) {} +} diff --git a/src/main/java/frc/robot/util/trajectory/HolonomicDriveController.java b/src/main/java/frc/robot/util/trajectory/HolonomicDriveController.java new file mode 100644 index 00000000..9c1ca8e0 --- /dev/null +++ b/src/main/java/frc/robot/util/trajectory/HolonomicDriveController.java @@ -0,0 +1,52 @@ +package frc.robot.util.trajectory; + +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 HolonomicDriveController { + private PIDController linearController, thetaController; + + private Pose2d m_poseError; + private Rotation2d m_rotationError; + + public HolonomicDriveController(PIDController linearController, PIDController thetaController) { + this.linearController = linearController; + this.thetaController = thetaController; + + this.thetaController.enableContinuousInput(-Math.PI, Math.PI); + } + + public ChassisSpeeds calculate(Pose2d currentPose, HolonomicDriveState state) { + 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 = linearController.calculate(currentPose.getX(), poseRef.getX()); + double yFeedback = linearController.calculate(currentPose.getY(), poseRef.getY()); + double thetaFeedback = + thetaController.calculate( + currentPose.getRotation().getRadians(), poseRef.getRotation().getRadians()); + + // Return next output. + return ChassisSpeeds.fromFieldRelativeSpeeds( + state.velocityX() + xFeedback, + state.velocityY() + yFeedback, + state.angularVelocity() + thetaFeedback, + currentPose.getRotation()); + } + + public Pose2d getPoseError() { + return m_poseError; + } + + public Rotation2d getRotationError() { + return m_rotationError; + } + + public record HolonomicDriveState( + Pose2d pose, double velocityX, double velocityY, double angularVelocity) {} +}