-
Notifications
You must be signed in to change notification settings - Fork 0
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Support PathPlanner using custom units
* update to 1.1.28 * update pplib * implement units compatibility with pplib
- Loading branch information
Showing
7 changed files
with
224 additions
and
42 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
110 changes: 110 additions & 0 deletions
110
src/main/kotlin/org/team4099/lib/pplib/PathPlannerHolonomicDriveController.kt
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,110 @@ | ||
| package org.team4099.lib.pplib | ||
|
|
||
| import com.pathplanner.lib.controllers.PPHolonomicDriveController | ||
| import com.pathplanner.lib.path.PathConstraints | ||
| import com.pathplanner.lib.path.PathPlannerTrajectory | ||
| import com.pathplanner.lib.util.ReplanningConfig | ||
| import org.team4099.lib.geometry.Pose2d | ||
| import org.team4099.lib.kinematics.ChassisSpeeds | ||
| import org.team4099.lib.units.AngularAcceleration | ||
| import org.team4099.lib.units.AngularVelocity | ||
| import org.team4099.lib.units.LinearAcceleration | ||
| import org.team4099.lib.units.LinearVelocity | ||
| import org.team4099.lib.units.base.Length | ||
| import org.team4099.lib.units.base.Time | ||
| import org.team4099.lib.units.base.inMeters | ||
| import org.team4099.lib.units.base.inSeconds | ||
| import org.team4099.lib.units.base.seconds | ||
| import org.team4099.lib.units.derived.Angle | ||
| import org.team4099.lib.units.derived.inRotation2ds | ||
| import org.team4099.lib.units.inMetersPerSecond | ||
| import org.team4099.lib.units.inMetersPerSecondPerSecond | ||
| import org.team4099.lib.units.inRadiansPerSecond | ||
| import org.team4099.lib.units.inRadiansPerSecondPerSecond | ||
| import java.util.Optional | ||
| import com.pathplanner.lib.path.GoalEndState as pplibGoalEndState | ||
| import com.pathplanner.lib.util.HolonomicPathFollowerConfig as pplibHolonomicPathFollowerConfig | ||
|
|
||
| class PathPlannerHolonomicDriveController( | ||
| translationConstants: PathPlannerTranslationPID, | ||
| rotationConstants: PathPlannerRotationPID, | ||
| maxModuleSpeed: LinearVelocity, | ||
| driveBaseRadius: Length, | ||
| period: Time = 0.02.seconds | ||
| ) { | ||
| private val pplibController: PPHolonomicDriveController = | ||
| PPHolonomicDriveController( | ||
| translationConstants.pplibTranslationConstants, | ||
| rotationConstants.pplibRotationConstants, | ||
| period.inSeconds, | ||
| maxModuleSpeed.inMetersPerSecond, | ||
| driveBaseRadius.inMeters | ||
| ) | ||
|
|
||
| val positionalError = pplibController.positionalError | ||
| val isHolonomic = pplibController.isHolonomic | ||
|
|
||
| fun calculateRobotRelativeSpeeds( | ||
| currentPose: Pose2d, | ||
| targetState: PathPlannerTrajectory.State | ||
| ): ChassisSpeeds { | ||
| return ChassisSpeeds( | ||
| pplibController.calculateRobotRelativeSpeeds(currentPose.pose2d, targetState) | ||
| ) | ||
| } | ||
|
|
||
| fun reset(currentPose: Pose2d, currentSpeeds: ChassisSpeeds) { | ||
| pplibController.reset(currentPose.pose2d, currentSpeeds.chassisSpeedsWPILIB) | ||
| } | ||
|
|
||
| fun setEnabled(enabled: Boolean) { | ||
| pplibController.setEnabled(enabled) | ||
| } | ||
|
|
||
| fun setRotationTargetOverride(rotationTargetOverride: () -> Angle) { | ||
| PPHolonomicDriveController.setRotationTargetOverride { | ||
| Optional.of(rotationTargetOverride().inRotation2ds) | ||
| } | ||
| } | ||
|
|
||
| companion object { | ||
| data class HolonomicPathFollowerConfig( | ||
| val translationConstants: PathPlannerTranslationPID, | ||
| val rotationConstants: PathPlannerRotationPID, | ||
| val maxModuleSpeed: LinearVelocity, | ||
| val driveBaseRadius: Length, | ||
| ) { | ||
| val pplibConfig = | ||
| pplibHolonomicPathFollowerConfig( | ||
| translationConstants.pplibTranslationConstants, | ||
| rotationConstants.pplibRotationConstants, | ||
| maxModuleSpeed.inMetersPerSecond, | ||
| driveBaseRadius.inMeters, | ||
| ReplanningConfig() | ||
| ) | ||
| } | ||
| data class GoalEndState( | ||
| val velocity: LinearVelocity, | ||
| val rotation: Angle, | ||
| val rotateFast: Boolean | ||
| ) { | ||
| val pplibGoalEndState: pplibGoalEndState = | ||
| pplibGoalEndState(velocity.inMetersPerSecond, rotation.inRotation2ds, rotateFast) | ||
| } | ||
|
|
||
| data class PathConstraints( | ||
| val maxVelocity: LinearVelocity, | ||
| val maxAcceleration: LinearAcceleration, | ||
| val maxAngularVelocity: AngularVelocity, | ||
| val maxAngularAcceleration: AngularAcceleration | ||
| ) { | ||
| val pplibConstraints = | ||
| PathConstraints( | ||
| maxVelocity.inMetersPerSecond, | ||
| maxAcceleration.inMetersPerSecondPerSecond, | ||
| maxAngularVelocity.inRadiansPerSecond, | ||
| maxAngularAcceleration.inRadiansPerSecondPerSecond | ||
| ) | ||
| } | ||
| } | ||
| } |
29 changes: 29 additions & 0 deletions
29
src/main/kotlin/org/team4099/lib/pplib/PathPlannerRotationPID.kt
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,29 @@ | ||
| package org.team4099.lib.pplib | ||
|
|
||
| import com.pathplanner.lib.util.PIDConstants | ||
| import org.team4099.lib.units.Velocity | ||
| import org.team4099.lib.units.derived.Angle | ||
| import org.team4099.lib.units.derived.DerivativeGain | ||
| import org.team4099.lib.units.derived.IntegralGain | ||
| import org.team4099.lib.units.derived.ProportionalGain | ||
| import org.team4099.lib.units.derived.Radian | ||
| import org.team4099.lib.units.derived.inRadians | ||
| import org.team4099.lib.units.derived.inRadiansPerSecondPerRadian | ||
| import org.team4099.lib.units.derived.inRadiansPerSecondPerRadianPerSecond | ||
| import org.team4099.lib.units.derived.inRadiansPerSecondPerRadianSeconds | ||
| import org.team4099.lib.units.derived.radians | ||
|
|
||
| data class PathPlannerRotationPID( | ||
| val kP: ProportionalGain<Radian, Velocity<Radian>>, | ||
| val kI: IntegralGain<Radian, Velocity<Radian>>, | ||
| val kD: DerivativeGain<Radian, Velocity<Radian>>, | ||
| val iZone: Angle = 1.0.radians | ||
| ) { | ||
| val pplibRotationConstants = | ||
| PIDConstants( | ||
| kP.inRadiansPerSecondPerRadian, | ||
| kI.inRadiansPerSecondPerRadianSeconds, | ||
| kD.inRadiansPerSecondPerRadianPerSecond, | ||
| iZone.inRadians | ||
| ) | ||
| } |
29 changes: 29 additions & 0 deletions
29
src/main/kotlin/org/team4099/lib/pplib/PathPlannerTranslationPID.kt
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,29 @@ | ||
| package org.team4099.lib.pplib | ||
|
|
||
| import com.pathplanner.lib.util.PIDConstants | ||
| import org.team4099.lib.units.Velocity | ||
| import org.team4099.lib.units.base.Length | ||
| import org.team4099.lib.units.base.Meter | ||
| import org.team4099.lib.units.base.inMeters | ||
| import org.team4099.lib.units.base.meters | ||
| import org.team4099.lib.units.derived.DerivativeGain | ||
| import org.team4099.lib.units.derived.IntegralGain | ||
| import org.team4099.lib.units.derived.ProportionalGain | ||
| import org.team4099.lib.units.derived.inMetersPerSecondPerMeter | ||
| import org.team4099.lib.units.derived.inMetersPerSecondPerMeterPerSecond | ||
| import org.team4099.lib.units.derived.inMetersPerSecondPerMeterSeconds | ||
|
|
||
| data class PathPlannerTranslationPID( | ||
| val kP: ProportionalGain<Meter, Velocity<Meter>>, | ||
| val kI: IntegralGain<Meter, Velocity<Meter>>, | ||
| val kD: DerivativeGain<Meter, Velocity<Meter>>, | ||
| val iZone: Length = 1.0.meters | ||
| ) { | ||
| val pplibTranslationConstants = | ||
| PIDConstants( | ||
| kP.inMetersPerSecondPerMeter, | ||
| kI.inMetersPerSecondPerMeterSeconds, | ||
| kD.inMetersPerSecondPerMeterPerSecond, | ||
| iZone.inMeters | ||
| ) | ||
| } |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Oops, something went wrong.