Merge pull request #258 from thedropbears/heading-pure-pid

Use pure PID controller for heading lock

Author: james-ward

autonomous/auto_base.py

@@ -18,12 +18,9 @@
 
 x_controller = PIDController(0.5, 0.0, 0.0)
 y_controller = PIDController(0.5, 0.0, 0.0)
-heading_controller = PIDController(3.0, 0, 0)
-heading_controller.enableContinuousInput(-math.pi, math.pi)
 
 wpilib.SmartDashboard.putData("Auto X PID", x_controller)
 wpilib.SmartDashboard.putData("Auto Y PID", y_controller)
-wpilib.SmartDashboard.putData("Auto Heading PID", heading_controller)
 
 
 class AutoBase(AutonomousStateMachine):
@@ -129,7 +126,9 @@ def follow_trajectory(self, sample: SwerveSample):
             sample.vx + x_controller.calculate(pose.X(), sample.x),
             sample.vy + y_controller.calculate(pose.Y(), sample.y),
             sample.omega
-            + heading_controller.calculate(pose.rotation().radians(), sample.heading),
+            + self.chassis.heading_controller.calculate(
+                pose.rotation().radians(), sample.heading
+            ),
         )
 
         # Apply the generated speeds

components/chassis.py

@@ -16,7 +16,7 @@
 from phoenix6.hardware import CANcoder, Pigeon2, TalonFX
 from phoenix6.signals import InvertedValue, NeutralModeValue
 from wpimath.controller import (
-    ProfiledPIDControllerRadians,
+    PIDController,
     SimpleMotorFeedforwardMeters,
 )
 from wpimath.estimator import SwerveDrive4PoseEstimator
@@ -27,7 +27,6 @@
     SwerveModulePosition,
     SwerveModuleState,
 )
-from wpimath.trajectory import TrapezoidProfileRadians
 
 from ids import CancoderId, TalonId
 from utilities.ctre import FALCON_FREE_RPS
@@ -246,13 +245,13 @@ class ChassisComponent:
     swerve_lock = magicbot.tunable(False)
 
     # TODO: Read from positions.py once autonomous is finished
-
     def __init__(
         self, track_width: float, wheel_base: float, swerve_config: SwerveConfig
     ) -> None:
         self.imu = Pigeon2(0)
-        self.heading_controller = ProfiledPIDControllerRadians(
-            3, 0, 0, TrapezoidProfileRadians.Constraints(100, 100)
+        self.heading_controller = PIDController(3.0, 0.0, 0.0)
+        wpilib.SmartDashboard.putData(
+            "Chassis heading_controller", self.heading_controller
         )
         self.heading_controller.enableContinuousInput(-math.pi, math.pi)
         self.snapping_to_heading = False
@@ -384,7 +383,7 @@ def drive_local(self, vx: float, vy: float, omega: float) -> None:
     def snap_to_heading(self, heading: float) -> None:
         """set a heading target for the heading controller"""
         self.snapping_to_heading = True
-        self.heading_controller.setGoal(heading)
+        self.heading_controller.setSetpoint(heading)
 
     def stop_snapping(self) -> None:
         """stops the heading_controller"""
@@ -399,9 +398,7 @@ def execute(self) -> None:
                 self.get_rotation().radians()
             )
         else:
-            self.heading_controller.reset(
-                self.get_rotation().radians(), self.get_rotational_velocity()
-            )
+            self.heading_controller.reset()
 
         desired_speed_translation = Translation2d(
             self.chassis_speeds.vx, self.chassis_speeds.vy
@@ -529,7 +526,7 @@ def get_rotation(self) -> Rotation2d:
 
     @feedback
     def at_desired_heading(self) -> bool:
-        return self.heading_controller.atGoal()
+        return abs(self.heading_controller.getError()) <= self.HEADING_TOLERANCE
 
     def set_coast_in_neutral(self, coast_mode: bool = True) -> None:
         mode = NeutralModeValue.COAST if coast_mode else NeutralModeValue.BRAKE