PPLib Updates and ProfiledFieldCentricAngle Request

pyproject.toml

@@ -6,7 +6,7 @@
 [tool.robotpy]
 
 # Version of robotpy this project depends on
-robotpy_version = "2025.0.0b2"
+robotpy_version = "2025.0.0b3"
 
 # Which extra RobotPy components should be installed
 # -> equivalent to `pip install robotpy[extra1, ...]
@@ -29,5 +29,5 @@ robotpy_extras = [
 # Other pip packages to install
 requires = [
     "phoenix6~=25.0b3",
-    "robotpy-pathplannerlib~=2025.0.0b5"
+    "robotpy-pathplannerlib~=2025.0.0b6"
 ]

robotcontainer.py

@@ -4,6 +4,7 @@
 # the WPILib BSD license file in the root directory of this project.
 #
 
+from annotated_types import T
 import commands2
 import commands2.button
 import commands2.cmd
@@ -17,20 +18,21 @@
 from subsystems.pivot import Pivot
 from robot_state import RobotState
 
-import math
 from pathplannerlib.auto import AutoBuilder
 from pathplannerlib.path import PathConstraints, PathPlannerPath
 from phoenix6 import swerve
 from phoenix6.controls import DutyCycleOut
 from phoenix6.swerve.utility.phoenix_pid_controller import PhoenixPIDController
 from wpilib import DriverStation, SmartDashboard, XboxController
 from wpimath.geometry import Rotation2d
+from wpimath.trajectory import TrapezoidProfile
 from wpimath.units import rotationsToRadians
 
 from commands.manual_lift import ManualLift
 from commands.intake_and_stow import IntakeAndStow
 from commands.vibrate import VibrateController
 from subsystems.pivot import PivotStates
+from subsystems.swerve.requests import ProfiledFieldCentricFacingAngle
 
 class RobotContainer:
     """
@@ -44,9 +46,7 @@ def __init__(self) -> None:
         self._max_speed = (
             TunerConstants.speed_at_12_volts
         )  # speed_at_12_volts desired top speed
-        self._max_angular_rate = rotationsToRadians(
-            1
-        )  # 3/4 of a rotation per second max angular velocity
+        self._max_angular_rate = rotationsToRadians(0.75)
 
         self._driver_controller = commands2.button.CommandXboxController(0)
         self._function_controller = commands2.button.CommandXboxController(1)
@@ -69,17 +69,10 @@ def __init__(self) -> None:
         self._brake = swerve.requests.SwerveDriveBrake()
         self._point = swerve.requests.PointWheelsAt()
         self._face = (
-            swerve.requests.FieldCentricFacingAngle()
-            .with_deadband(self._max_speed * 0.1)
-            .with_rotational_deadband(
-                self._max_angular_rate * 0.1
-            )
-            .with_drive_request_type(
-                swerve.SwerveModule.DriveRequestType.OPEN_LOOP_VOLTAGE
-            )
+            ProfiledFieldCentricFacingAngle(TrapezoidProfile.Constraints(self._max_angular_rate, self._max_angular_rate / 0.25))
+            .with_deadband(self._max_speed * 0.1).with_rotational_deadband(self._max_angular_rate * 0.1)
+            .with_drive_request_type(swerve.SwerveModule.DriveRequestType.OPEN_LOOP_VOLTAGE)
         )
-        self._face.heading_controller = PhoenixPIDController(18.749, 0.45773, 0)
-        self._face.heading_controller.enableContinuousInput(-math.pi, math.pi)
 
         self.intake = Intake()
         self.leds = LedSubsystem()
@@ -99,9 +92,9 @@ def configureButtonBindings(self) -> None:
         instantiating a :GenericHID or one of its subclasses (Joystick or XboxController),
         and then passing it to a JoystickButton.
         """
+        self._face.heading_controller.setPID(18.749, 0, 0.45774)
+        #self._face.heading_controller.enableContinuousInput(-math.pi, math.pi)
 
-        # Note that X is defined as forward according to WPILib convention,
-        # and Y is defined as to the left according to WPILib convention.
         self.drivetrain.setDefaultCommand(
             # Drivetrain will execute this command periodically
             self.drivetrain.apply_request(
@@ -130,6 +123,7 @@ def configureButtonBindings(self) -> None:
 
         # We can't test these until we get a Limelight onto MM and until PathPlanner Beta 5 releases :(
         self._driver_controller.x().whileTrue(
+            self.drivetrain.runOnce(lambda: self._face.reset_profile(self.drivetrain.get_state().pose.rotation())).andThen(
             self.drivetrain.apply_request(
                 lambda: self._face.with_velocity_x(
                     -self._driver_controller.getLeftY() * self._max_speed
@@ -141,7 +135,22 @@ def configureButtonBindings(self) -> None:
                     # Gets the angle to our alliance's speaker
                     (Constants.k_apriltag_layout.getTagPose(4 if (DriverStation.getAlliance() or DriverStation.Alliance.kBlue) == DriverStation.Alliance.kRed else 7).toPose2d().translation() - self.drivetrain.get_state().pose.translation()).angle() + Rotation2d.fromDegrees(180)
                 )
-            )
+            ))
+        )
+
+        commands2.button.Trigger(lambda: self._driver_controller.getHID().getPOV() != -1).whileTrue(
+            self.drivetrain.runOnce(lambda: self._face.reset_profile(self.drivetrain.get_state().pose.rotation())).andThen(
+            self.drivetrain.apply_request(
+                lambda: self._face.with_velocity_x(
+                    -self._driver_controller.getLeftY() * self._max_speed
+                )
+                .with_velocity_y(
+                    -self._driver_controller.getLeftX() * self._max_speed
+                )
+                .with_target_direction(
+                    Rotation2d.fromDegrees(-self._driver_controller.getHID().getPOV())
+                )
+            ))
         )
 
         self._driver_controller.y().whileTrue(

simgui-ds.json

@@ -93,7 +93,8 @@
   ],
   "robotJoysticks": [
     {
-      "guid": "Keyboard0"
+      "guid": "78696e70757401000000000000000000",
+      "useGamepad": true
     }
   ]
 }

subsystems/swerve.py → subsystems/swerve/__init__.py

@@ -4,11 +4,15 @@
 import math
 from pathplannerlib.auto import AutoBuilder, RobotConfig
 from pathplannerlib.controller import PIDConstants, PPHolonomicDriveController
+from pathplannerlib.util import DriveFeedforwards
+from pathplannerlib.util.swerve import SwerveSetpoint, SwerveSetpointGenerator
 from phoenix6 import swerve, units, utils
 from phoenix6.swerve.requests import ApplyRobotSpeeds
 from typing import Callable, overload
 from wpilib import DriverStation, Notifier, RobotController
 from wpimath.geometry import Rotation2d
+from wpimath.kinematics import ChassisSpeeds
+from wpimath.units import rotationsToRadians
 
 
 class SwerveSubsystem(Subsystem, swerve.SwerveDrivetrain):
@@ -203,10 +207,7 @@ def _configure_auto_builder(self):
             lambda: self.get_state().speeds, # Supplier of current robot speeds
             # Consumer of ChassisSpeeds and feedforwards to drive the robot
             lambda speeds, feedforwards: self.set_control(
-                self._apply_robot_speeds
-                .with_speeds(speeds)
-                .with_wheel_force_feedforwards_x(feedforwards.robotRelativeForcesXNewtons)
-                .with_wheel_force_feedforwards_y(feedforwards.robotRelativeForcesYNewtons)
+                self._apply_speeds_from_setpoint(speeds)
             ),
             PPHolonomicDriveController(
                 # PID constants for translation
@@ -219,6 +220,28 @@ def _configure_auto_builder(self):
             lambda: (DriverStation.getAlliance() or DriverStation.Alliance.kBlue) == DriverStation.Alliance.kRed,
             self # Subsystem for requirements
         )
+
+        self._setpoint_generator = SwerveSetpointGenerator(config, rotationsToRadians(10.0))
+
+        self._previous_setpoint = SwerveSetpoint(
+            self.get_state().speeds,
+            self.get_state().module_states,
+            DriveFeedforwards.zeros(config.numModules)
+        )
+
+    def _apply_speeds_from_setpoint(self, speeds: ChassisSpeeds) -> ApplyRobotSpeeds:
+        self._previous_setpoint = self._setpoint_generator.generateSetpoint(
+            self._previous_setpoint, 
+            speeds, 
+            0.02
+        )
+
+        return (self._apply_robot_speeds
+                .with_speeds(self._previous_setpoint.robot_relative_speeds)
+                .with_wheel_force_feedforwards_x(self._previous_setpoint.feedforwards.robotRelativeForcesXNewtons)
+                .with_wheel_force_feedforwards_y(self._previous_setpoint.feedforwards.robotRelativeForcesYNewtons)
+        )
+
 
     def apply_request(
         self, request: Callable[[], swerve.requests.SwerveRequest]