Add trajectory following with Path Planner

FIRST1778 · Nov 23, 2023 · 7c5fa6e · 7c5fa6e
1 parent f0fd257
commit 7c5fa6e
Show file tree

Hide file tree

Showing 3 changed files with 10 additions and 9 deletions.
diff --git a/wpi/src/main/kotlin/org/ghrobotics/lib/subsystems/drive/FalconDriveHelper.kt b/wpi/src/main/kotlin/org/ghrobotics/lib/subsystems/drive/FalconDriveHelper.kt
@@ -169,7 +169,7 @@ class FalconDriveHelper {
             },
             rotationInput,
             if (RobotBase.isReal()) {
-                drivetrain.robotPosition.rotation
+                drivetrain.swerveDriveInputs.robotPosition.rotation
             } else Rotation2d.fromDegrees(0.0),
         )
     }

diff --git a/wpi/src/main/kotlin/org/ghrobotics/lib/subsystems/drive/swerve/FalconSwerveDrivetrain.kt b/wpi/src/main/kotlin/org/ghrobotics/lib/subsystems/drive/swerve/FalconSwerveDrivetrain.kt
@@ -12,7 +12,6 @@ import com.pathplanner.lib.commands.FollowPathCommand
 import com.pathplanner.lib.commands.PathfindThenFollowPathHolonomic
 import com.pathplanner.lib.path.GoalEndState
 import com.pathplanner.lib.path.PathPlannerPath
-import com.pathplanner.lib.util.HolonomicPathFollowerConfig
 import com.pathplanner.lib.util.ReplanningConfig
 import edu.wpi.first.math.geometry.Pose2d
 import edu.wpi.first.math.geometry.Rotation2d
@@ -44,7 +43,6 @@ abstract class FalconSwerveDrivetrain : TrajectoryTrackerSwerveDriveBase(), Sens
 
     abstract val motorOutputLimiter: Source<Double>
 
-
     val field = Field2d()
     private val fieldTab = Shuffleboard.getTab("Field")
 
@@ -53,21 +51,21 @@ abstract class FalconSwerveDrivetrain : TrajectoryTrackerSwerveDriveBase(), Sens
     fun navigateToPose(pose: Pose2d) = PathfindThenFollowPathHolonomic(
         PathPlannerPath(PathPlannerPath.bezierFromPoses(pose), pathConstraints, GoalEndState(0.0, pose.rotation)),
         pathConstraints,
-        swerveDriveInputs::robotPose,
+        swerveDriveInputs::robotPosition,
         swerveDriveInputs::chassisSpeeds,
         ::setOutputSI,
         pathFollowingConfig,
-        this
+        this,
     )
 
     fun follow(path: PathPlannerPath) = FollowPathCommand(
         path,
-        swerveDriveInputs::robotPose,
+        swerveDriveInputs::robotPosition,
         swerveDriveInputs::chassisSpeeds,
         ::setOutputSI,
         controller,
         ReplanningConfig(),
-        this
+        this,
     )
 
     fun resetPosition(newPose: Pose2d) {

diff --git a/...ain/kotlin/org/ghrobotics/lib/subsystems/drive/swerve/TrajectoryTrackerSwerveDriveBase.kt b/...ain/kotlin/org/ghrobotics/lib/subsystems/drive/swerve/TrajectoryTrackerSwerveDriveBase.kt
@@ -37,18 +37,21 @@ abstract class TrajectoryTrackerSwerveDriveBase : FalconSubsystem() {
      * The replanning configuration. Defaults to no replanning. override to change
      */
     val replanningConfig: ReplanningConfig = ReplanningConfig()
+
     /**
      * The current inputs and outputs
      */
     abstract val swerveDriveIO: SwerveDriveIO
     abstract val swerveDriveInputs: AbstractSwerveDriveInputs
 
+    val robotPosition get() = swerveDriveInputs.robotPosition
+
     abstract fun setOutputSI(
         states: Array<SwerveModuleState>,
     )
 
     abstract fun setOutputSI(
-        speeds: ChassisSpeeds
+        speeds: ChassisSpeeds,
     )
 }
 
@@ -96,7 +99,7 @@ interface AbstractSwerveDriveInputs {
     var rightBackFeedforward: SIUnit<Volt>
     var leftBackFeedforward: SIUnit<Volt>
 
-    var robotPose: Pose2d
+    var robotPosition: Pose2d
 
     var chassisSpeeds: ChassisSpeeds