diff --git a/src/main/java/frc/lib/util/Util.java b/src/main/java/frc/lib/util/Util.java index ea2dda8..a40cc8c 100644 --- a/src/main/java/frc/lib/util/Util.java +++ b/src/main/java/frc/lib/util/Util.java @@ -363,9 +363,9 @@ public void clearStatesBeforeTime(Time time) { } public Pose2dTimeInterpolable(Trajectory trajwithTan, Rotation2d startHeading, Rotation2d endHeading) { - double totalTimeSecpnods = trajwithTan.getTotalTime(); + double totalTimeSeconds = trajwithTan.getTotalTime(); for (State state : trajwithTan.getStates()) { - Rotation2d poseRotation = startHeading.interpolate(endHeading, state.time / totalTimeSecpnods); + Rotation2d poseRotation = startHeading.interpolate(endHeading, state.time / totalTimeSeconds); poseList.add(new Pair<>( new Pose2d(state.pose.getTranslation(), poseRotation), Units.Seconds.of(state.time))); diff --git a/src/main/java/frc/robot/trajectories/FollowSyncedPIDToPose.java b/src/main/java/frc/robot/trajectories/FollowSyncedPIDToPose.java index 3e1b15a..a05df43 100644 --- a/src/main/java/frc/robot/trajectories/FollowSyncedPIDToPose.java +++ b/src/main/java/frc/robot/trajectories/FollowSyncedPIDToPose.java @@ -85,7 +85,7 @@ public boolean closeEnoughToRaiseElevator() { public boolean slowEnoughToRaiseElevator() { return Math.hypot( Drive.mInstance.getState().Speeds.vx, - Drive.mInstance.getState().Speeds.vx) + Drive.mInstance.getState().Speeds.vy) < velToRaiseElevator.in(Units.MetersPerSecond); } diff --git a/src/main/java/frc/robot/trajectories/FollowSyncedTrajectory.java b/src/main/java/frc/robot/trajectories/FollowSyncedTrajectory.java index ef0e05a..31177e0 100644 --- a/src/main/java/frc/robot/trajectories/FollowSyncedTrajectory.java +++ b/src/main/java/frc/robot/trajectories/FollowSyncedTrajectory.java @@ -67,7 +67,7 @@ public boolean closeEnoughToRaiseElevator() { public boolean slowEnoughToRaiseElevator() { return Math.hypot( Drive.mInstance.getState().Speeds.vx, - Drive.mInstance.getState().Speeds.vx) + Drive.mInstance.getState().Speeds.vy) < DriveConstants.kMaxSpeedTippy.times(1.1).in(Units.MetersPerSecond); }