made the chassis heading PID controller used in autobase

Author: DannyPomeranian

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

@@ -245,7 +245,6 @@ 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: