From 641aa091a4986b595ba022e4d0be2dac19e32e24 Mon Sep 17 00:00:00 2001 From: Shom770 Date: Wed, 7 Feb 2024 17:28:14 -0500 Subject: [PATCH] fixed path planner errors --- build.gradle | 2 +- .../org/team4099/lib/pathfollow/TrajectoryUtils.kt | 14 +++++++------- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/build.gradle b/build.gradle index 85ba536..f119de7 100644 --- a/build.gradle +++ b/build.gradle @@ -39,7 +39,7 @@ publishing { release(MavenPublication) { groupId = 'org.team4099' artifactId = 'falconutils' - version = '1.1.30' + version = '1.1.30r1' from(components["kotlin"]) } diff --git a/src/main/kotlin/org/team4099/lib/pathfollow/TrajectoryUtils.kt b/src/main/kotlin/org/team4099/lib/pathfollow/TrajectoryUtils.kt index e138dc1..1e2a8db 100644 --- a/src/main/kotlin/org/team4099/lib/pathfollow/TrajectoryUtils.kt +++ b/src/main/kotlin/org/team4099/lib/pathfollow/TrajectoryUtils.kt @@ -1,6 +1,6 @@ package org.team4099.lib.pathfollow -import com.pathplanner.lib.PathPlannerTrajectory +import com.pathplanner.lib.path.PathPlannerTrajectory import edu.wpi.first.math.trajectory.TrajectoryParameterizer import org.team4099.lib.geometry.Pose2d import org.team4099.lib.geometry.Translation2d @@ -75,14 +75,14 @@ fun trajectoryFromPath( fun trajectoryFromPathPlanner(pathPlannerTrajectory: PathPlannerTrajectory): Trajectory { return Trajectory( pathPlannerTrajectory.states.map { state -> - state as PathPlannerTrajectory.PathPlannerState + state as PathPlannerTrajectory.State TrajectoryState( state.timeSeconds.seconds, - Pose2d(Translation2d(state.poseMeters.translation), state.holonomicRotation.angle), - state.poseMeters.rotation.angle, - state.velocityMetersPerSecond.meters.perSecond, - state.accelerationMetersPerSecondSq.meters.perSecond.perSecond, - angularVelocity = state.angularVelocityRadPerSec.radians.perSecond + Pose2d(Translation2d(state.positionMeters), state.heading.angle), + state.heading.angle, + state.velocityMps.meters.perSecond, + state.accelerationMpsSq.meters.perSecond.perSecond, + angularVelocity = state.headingAngularVelocityRps.radians.perSecond ) } )