Skip to content

Commit

Permalink
fixed path planner errors
Browse files Browse the repository at this point in the history
  • Loading branch information
Shom770 committed Feb 7, 2024
1 parent dc6d239 commit 641aa09
Show file tree
Hide file tree
Showing 2 changed files with 8 additions and 8 deletions.
2 changes: 1 addition & 1 deletion build.gradle
Original file line number Diff line number Diff line change
Expand Up @@ -39,7 +39,7 @@ publishing {
release(MavenPublication) {
groupId = 'org.team4099'
artifactId = 'falconutils'
version = '1.1.30'
version = '1.1.30r1'

from(components["kotlin"])
}
Expand Down
14 changes: 7 additions & 7 deletions src/main/kotlin/org/team4099/lib/pathfollow/TrajectoryUtils.kt
Original file line number Diff line number Diff line change
@@ -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
Expand Down Expand Up @@ -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
)
}
)
Expand Down

0 comments on commit 641aa09

Please sign in to comment.