diff --git a/src/main/kotlin/org/team4099/lib/pplib/PathPlannerHolonomicDriveController.kt b/src/main/kotlin/org/team4099/lib/pplib/PathPlannerHolonomicDriveController.kt new file mode 100644 index 0000000..5bdd180 --- /dev/null +++ b/src/main/kotlin/org/team4099/lib/pplib/PathPlannerHolonomicDriveController.kt @@ -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 + ) + } + } +} diff --git a/src/main/kotlin/org/team4099/lib/pplib/PathPlannerRotationPID.kt b/src/main/kotlin/org/team4099/lib/pplib/PathPlannerRotationPID.kt new file mode 100644 index 0000000..47772a6 --- /dev/null +++ b/src/main/kotlin/org/team4099/lib/pplib/PathPlannerRotationPID.kt @@ -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>, + val kI: IntegralGain>, + val kD: DerivativeGain>, + val iZone: Angle = 1.0.radians +) { + val pplibRotationConstants = + PIDConstants( + kP.inRadiansPerSecondPerRadian, + kI.inRadiansPerSecondPerRadianSeconds, + kD.inRadiansPerSecondPerRadianPerSecond, + iZone.inRadians + ) +} diff --git a/src/main/kotlin/org/team4099/lib/pplib/PathPlannerTranslationPID.kt b/src/main/kotlin/org/team4099/lib/pplib/PathPlannerTranslationPID.kt new file mode 100644 index 0000000..d743e88 --- /dev/null +++ b/src/main/kotlin/org/team4099/lib/pplib/PathPlannerTranslationPID.kt @@ -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>, + val kI: IntegralGain>, + val kD: DerivativeGain>, + val iZone: Length = 1.0.meters +) { + val pplibTranslationConstants = + PIDConstants( + kP.inMetersPerSecondPerMeter, + kI.inMetersPerSecondPerMeterSeconds, + kD.inMetersPerSecondPerMeterPerSecond, + iZone.inMeters + ) +} diff --git a/src/main/kotlin/org/team4099/lib/units/Derivatives.kt b/src/main/kotlin/org/team4099/lib/units/Derivatives.kt index 28d059d..85f2d95 100644 --- a/src/main/kotlin/org/team4099/lib/units/Derivatives.kt +++ b/src/main/kotlin/org/team4099/lib/units/Derivatives.kt @@ -71,7 +71,5 @@ inline val AngularAcceleration.inDegreesPerSecondPerSecond: Double inline val AngularAcceleration.inRotationsPerSecondPerSecond: Double get() = value / (2 * PI) - - inline val AngularAcceleration.inRotationsPerMinutePerMinute: Double get() = value * SECONDS_PER_MINUTE * SECONDS_PER_MINUTE / (2 * PI) diff --git a/src/main/kotlin/org/team4099/lib/units/derived/Controller.kt b/src/main/kotlin/org/team4099/lib/units/derived/Controller.kt index 1701d37..33639d9 100644 --- a/src/main/kotlin/org/team4099/lib/units/derived/Controller.kt +++ b/src/main/kotlin/org/team4099/lib/units/derived/Controller.kt @@ -71,8 +71,6 @@ inline val Value.perDegreePerSecond inline val Value.perRotationPerMinute get() = (perRadianPerSecond * SECONDS_PER_MINUTE) / RADIANS_PER_ROTATION - - inline val Double.VoltsPerVolts get() = Value>(this) @@ -163,7 +161,6 @@ inline val DerivativeGain, Volt>.inVoltsPerDegreesPerSecondPerS inline val DerivativeGain, Volt>.inVoltsPerRotationsPerMinutePerSecond: Double get() = inVoltsPerRadiansPerSecondPerSecond / SECONDS_PER_MINUTE * RADIANS_PER_ROTATION - inline val Double.AmpsPerAmps get() = Value>(this) @@ -254,7 +251,6 @@ inline val DerivativeGain, Ampere>.inAmpsPerDegreesPerSecondPer inline val DerivativeGain, Ampere>.inAmpsPerRotationsPerMinutePerSecond: Double get() = inAmpsPerRadiansPerSecondPerSecond / SECONDS_PER_MINUTE * RADIANS_PER_ROTATION - inline val Double.radiansPerSecondPerRadiansPerSecond get() = Value, Velocity>>(this) @@ -276,13 +272,16 @@ inline val ProportionalGain>.inRadiansPerSecondPerDegre inline val ProportionalGain>.inRadiansPerSecondPerRotation: Double get() = inRadiansPerSecondPerRadian * RADIANS_PER_ROTATION -inline val ProportionalGain, Velocity>.inRadiansPerSecondPerMetersPerSecond: Double - get() = value +inline val ProportionalGain, Velocity>.inRadiansPerSecondPerMetersPerSecond: + Double + get() = value -inline val ProportionalGain, Velocity>.inRadiansPerSecondPerRadiansPerSecond: Double +inline val ProportionalGain< + Velocity, Velocity>.inRadiansPerSecondPerRadiansPerSecond: Double get() = value -inline val ProportionalGain, Velocity>.inRadiansPerSecondPerRotationPerMinute: Double +inline val ProportionalGain< + Velocity, Velocity>.inRadiansPerSecondPerRotationPerMinute: Double get() = inRadiansPerSecondPerRadiansPerSecond * RADIANS_PER_ROTATION / SECONDS_PER_MINUTE inline val IntegralGain>.inRadiansPerSecondPerMeterSeconds: Double @@ -324,7 +323,8 @@ inline val DerivativeGain>.inRadiansPerSecondPerInchPerS inline val DerivativeGain>.inRadiansPerSecondPerFootPerSecond: Double get() = inRadiansPerSecondPerMeterPerSecond * METERS_PER_FOOT -inline val DerivativeGain, Velocity>.inRadiansPerSecondPerMetersPerSecondPerSecond: Double +inline val DerivativeGain< + Velocity, Velocity>.inRadiansPerSecondPerMetersPerSecondPerSecond: Double get() = value inline val DerivativeGain>.inRadiansPerSecondPerRadianPerSecond: Double @@ -336,16 +336,18 @@ inline val DerivativeGain>.inRadiansPerSecondPerDegreeP inline val DerivativeGain>.inRadiansPerSecondPerRotationsPerMinute: Double get() = inRadiansPerSecondPerRadianPerSecond / SECONDS_PER_MINUTE * RADIANS_PER_ROTATION -inline val DerivativeGain, Velocity>.inRadiansPerSecondPerRadiansPerSecondPerSecond: Double +inline val DerivativeGain< + Velocity, Velocity>.inRadiansPerSecondPerRadiansPerSecondPerSecond: Double get() = value -inline val DerivativeGain, Velocity>.inRadiansPerSecondPerDegreesPerSecondPerSecond: Double +inline val DerivativeGain< + Velocity, Velocity>.inRadiansPerSecondPerDegreesPerSecondPerSecond: Double get() = inRadiansPerSecondPerRadiansPerSecondPerSecond * RADIANS_PER_DEGREES -inline val DerivativeGain, Velocity>.inRadiansPerSecondPerRotationsPerMinutePerSecond: Double +inline val DerivativeGain< + Velocity, Velocity>.inRadiansPerSecondPerRotationsPerMinutePerSecond: Double get() = inRadiansPerSecondPerRadiansPerSecondPerSecond / SECONDS_PER_MINUTE * RADIANS_PER_ROTATION - inline val Double.metersPerSecondPerMetersPerSecond get() = Value, Velocity>>(this) @@ -367,13 +369,16 @@ inline val ProportionalGain>.inMetersPerSecondPerDegree: inline val ProportionalGain>.inMetersPerSecondPerRotation: Double get() = inMetersPerSecondPerRadian * RADIANS_PER_ROTATION -inline val ProportionalGain, Velocity>.inMetersPerSecondPerMetersPerSecond: Double - get() = value +inline val ProportionalGain, Velocity>.inMetersPerSecondPerMetersPerSecond: + Double + get() = value -inline val ProportionalGain, Velocity>.inMetersPerSecondPerRadiansPerSecond: Double - get() = value +inline val ProportionalGain, Velocity>.inMetersPerSecondPerRadiansPerSecond: + Double + get() = value -inline val ProportionalGain, Velocity>.inMetersPerSecondPerRotationPerMinute: Double +inline val ProportionalGain< + Velocity, Velocity>.inMetersPerSecondPerRotationPerMinute: Double get() = inMetersPerSecondPerRadiansPerSecond * RADIANS_PER_ROTATION / SECONDS_PER_MINUTE inline val IntegralGain>.inMetersPerSecondPerMeterSeconds: Double @@ -415,7 +420,8 @@ inline val DerivativeGain>.inMetersPerSecondPerInchPerSec inline val DerivativeGain>.inMetersPerSecondPerFootPerSecond: Double get() = inMetersPerSecondPerMeterPerSecond * METERS_PER_FOOT -inline val DerivativeGain, Velocity>.inMetersPerSecondPerMetersPerSecondPerSecond: Double +inline val DerivativeGain< + Velocity, Velocity>.inMetersPerSecondPerMetersPerSecondPerSecond: Double get() = value inline val DerivativeGain>.inMetersPerSecondPerRadianPerSecond: Double @@ -427,16 +433,18 @@ inline val DerivativeGain>.inMetersPerSecondPerDegreePer inline val DerivativeGain>.inMetersPerSecondPerRotationsPerMinute: Double get() = inMetersPerSecondPerRadianPerSecond / SECONDS_PER_MINUTE * RADIANS_PER_ROTATION -inline val DerivativeGain, Velocity>.inMetersPerSecondPerRadiansPerSecondPerSecond: Double +inline val DerivativeGain< + Velocity, Velocity>.inMetersPerSecondPerRadiansPerSecondPerSecond: Double get() = value -inline val DerivativeGain, Velocity>.inMetersPerSecondPerDegreesPerSecondPerSecond: Double +inline val DerivativeGain< + Velocity, Velocity>.inMetersPerSecondPerDegreesPerSecondPerSecond: Double get() = inMetersPerSecondPerRadiansPerSecondPerSecond * RADIANS_PER_DEGREES -inline val DerivativeGain, Velocity>.inMetersPerSecondPerRotationsPerMinutePerSecond: Double +inline val DerivativeGain< + Velocity, Velocity>.inMetersPerSecondPerRotationsPerMinutePerSecond: Double get() = inMetersPerSecondPerRadiansPerSecondPerSecond / SECONDS_PER_MINUTE * RADIANS_PER_ROTATION - inline val Double.MetersPerMeters get() = Value>(this) @@ -527,7 +535,6 @@ inline val DerivativeGain, Meter>.inMetersPerDegreesPerSecondPe inline val DerivativeGain, Meter>.inMetersPerRotationsPerMinutePerSecond: Double get() = inMetersPerRadiansPerSecondPerSecond / SECONDS_PER_MINUTE * RADIANS_PER_ROTATION - inline val Double.DegreesPerSecondPerDegreesPerSecond get() = Value, Velocity>>(this) @@ -549,13 +556,16 @@ inline val ProportionalGain>.inDegreesPerSecondPerDegre inline val ProportionalGain>.inDegreesPerSecondPerRotation: Double get() = inDegreesPerSecondPerRadian * RADIANS_PER_ROTATION -inline val ProportionalGain, Velocity>.inDegreesPerSecondPerMetersPerSecond: Double - get() = value +inline val ProportionalGain, Velocity>.inDegreesPerSecondPerMetersPerSecond: + Double + get() = value -inline val ProportionalGain, Velocity>.inDegreesPerSecondPerRadiansPerSecond: Double +inline val ProportionalGain< + Velocity, Velocity>.inDegreesPerSecondPerRadiansPerSecond: Double get() = value -inline val ProportionalGain, Velocity>.inDegreesPerSecondPerRotationPerMinute: Double +inline val ProportionalGain< + Velocity, Velocity>.inDegreesPerSecondPerRotationPerMinute: Double get() = inDegreesPerSecondPerRadiansPerSecond * RADIANS_PER_ROTATION / SECONDS_PER_MINUTE inline val IntegralGain>.inDegreesPerSecondPerMeterSeconds: Double @@ -597,7 +607,8 @@ inline val DerivativeGain>.inDegreesPerSecondPerInchPerS inline val DerivativeGain>.inDegreesPerSecondPerFootPerSecond: Double get() = inDegreesPerSecondPerMeterPerSecond * METERS_PER_FOOT -inline val DerivativeGain, Velocity>.inDegreesPerSecondPerMetersPerSecondPerSecond: Double +inline val DerivativeGain< + Velocity, Velocity>.inDegreesPerSecondPerMetersPerSecondPerSecond: Double get() = value inline val DerivativeGain>.inDegreesPerSecondPerRadianPerSecond: Double @@ -609,11 +620,14 @@ inline val DerivativeGain>.inDegreesPerSecondPerDegreeP inline val DerivativeGain>.inDegreesPerSecondPerRotationsPerMinute: Double get() = inDegreesPerSecondPerRadianPerSecond / SECONDS_PER_MINUTE * RADIANS_PER_ROTATION -inline val DerivativeGain, Velocity>.inDegreesPerSecondPerRadiansPerSecondPerSecond: Double +inline val DerivativeGain< + Velocity, Velocity>.inDegreesPerSecondPerRadiansPerSecondPerSecond: Double get() = value -inline val DerivativeGain, Velocity>.inDegreesPerSecondPerDegreesPerSecondPerSecond: Double +inline val DerivativeGain< + Velocity, Velocity>.inDegreesPerSecondPerDegreesPerSecondPerSecond: Double get() = inDegreesPerSecondPerRadiansPerSecondPerSecond * RADIANS_PER_DEGREES -inline val DerivativeGain, Velocity>.inDegreesPerSecondPerRotationsPerMinutePerSecond: Double +inline val DerivativeGain< + Velocity, Velocity>.inDegreesPerSecondPerRotationsPerMinutePerSecond: Double get() = inDegreesPerSecondPerRadiansPerSecondPerSecond / SECONDS_PER_MINUTE * RADIANS_PER_ROTATION diff --git a/vendordeps/PathPlanner.json b/vendordeps/PathPlanner.json index e8fa18b..3c539df 100644 --- a/vendordeps/PathPlanner.json +++ b/vendordeps/PathPlanner.json @@ -1,18 +1,18 @@ { - "fileName": "PathplannerLib2023.json", + "fileName": "PathplannerLib.json", "name": "PathplannerLib", - "version": "2023.4.4", + "version": "2024.2.1", "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", - "frcYear": "2023", + "frcYear": "2024", "mavenUrls": [ "https://3015rangerrobotics.github.io/pathplannerlib/repo" ], - "jsonUrl": "https://3015rangerrobotics.github.io/pathplannerlib/PathplannerLib2023.json", + "jsonUrl": "https://3015rangerrobotics.github.io/pathplannerlib/PathplannerLib.json", "javaDependencies": [ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-java", - "version": "2023.4.4" + "version": "2024.2.1" } ], "jniDependencies": [], @@ -20,7 +20,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-cpp", - "version": "2023.4.4", + "version": "2024.2.1", "libName": "PathplannerLib", "headerClassifier": "headers", "sharedLibrary": false, @@ -29,7 +29,9 @@ "windowsx86-64", "linuxx86-64", "osxuniversal", - "linuxathena" + "linuxathena", + "linuxarm32", + "linuxarm64" ] } ]