final changes bf worlsssssssss slay guys

config.py

@@ -243,9 +243,9 @@ class LimelightPosition:
 driver_centric: bool = True
 drivetrain_reversed: bool = False
 
-drivetrain_rotation_P: float = 5
+drivetrain_rotation_P: float = 5.4
 drivetrain_rotation_I: float = 0.0
-drivetrain_rotation_D: float = 0.0
+drivetrain_rotation_D: float = 0.06
 drivetrain_aiming_max_angular_speed: radians = 50#constants.drivetrain_max_angular_vel
 drivetrain_aiming_max_angular_accel: radians = 35 #constants.drivetrain_max_angular_accel
 
@@ -260,7 +260,7 @@ class LimelightPosition:
 drivetrain_aiming_tilt_threshold: radians = 3 * degrees_to_radians
 shot_height_offset: inches = 0 # inches
 shot_angle_offset: degrees = 0.7
-wrist_shot_tolerance: degrees = 2#1.75 if comp_bot.get() else 2 
+wrist_shot_tolerance: degrees = 3#1.75 if comp_bot.get() else 2 
 wrist_velocity_shot_tolerance: degrees = 1
 shot_height_offset_scalar: float = 0.014
 speaker_length: meters = 41.83 * inches_to_meters

sensors/field_odometry.py

@@ -285,8 +285,11 @@ def compensate_speaker(deviation):
 
         dist_calculations = (std_dev, std_dev, std_dev_omega)
         self.std_dev = dist_calculations
+
+        final_pose = Pose2d(vision_pose.toPose2d().translation(), self.drivetrain.get_heading())
+
         self.drivetrain.odometry_estimator.addVisionMeasurement(
-            vision_pose.toPose2d(), vision_time, self.std_dev
+            final_pose, vision_time, self.std_dev
         )
 
     def get_vision_poses(self):

sensors/limelight.py

@@ -436,7 +436,7 @@ def get_estimated_robot_pose(self) -> list[tuple[Pose3d, float, float, float, fl
 
                 if abs(math.degrees(self.gyro.get_robot_heading_rate())) < config.odometry_megatag2_max_angular_velocity:
                     use_megatag_2 = True
-                    print('using megatag2')
+
 
             if (
                 limelight.april_tag_exists()