feed shot is working

config.py

@@ -276,7 +276,7 @@ class LimelightPosition:
 flywheel_feed_speed_min: meters_per_second = 13
 flywheel_distance_scalar: float = 1.8
 flywheel_distance_feed_scalar: float = 2.6
-feed_shot_target_height: meters = 5
+feed_shot_target_height: meters = 4
 v0_flywheel_minimum: meters_per_second = 14
 v0_flywheel_maximum: meters_per_second = 28
 # v0_effective_flywheel: meters_per_second = 12

constants.py

@@ -68,6 +68,8 @@ class FieldPos:
     pose_reverse = Rotation2d(math.radians(180))
 
     wing_boundary = 231 * inches_to_meters
+
+    op_wing_boundary = 10
 
     # all poses are relative to the blue field origin
     class Wing:

sensors/trajectory_calc.py

@@ -56,6 +56,8 @@ def init(self):
         self.speaker_z = POI.Coordinates.Structures.Scoring.kSpeaker.getZ()
         self.feed_zone = POI.Coordinates.Structures.Scoring.kFeed.getTranslation()
         self.feed_zone_z = POI.Coordinates.Structures.Scoring.kFeed.getZ()
+        self.feed_zone_midline = POI.Coordinates.Structures.Scoring.kFeedMidline.getTranslation()
+        self.feed_zone_midline_z = POI.Coordinates.Structures.Scoring.kFeedMidline.getTranslation()
         if self.tuning:
             self.table.putNumber('flywheel distance scalar', config.flywheel_distance_scalar)
             self.table.putNumber('flywheel minimum value', config.v0_flywheel_minimum)
@@ -122,7 +124,7 @@ def calculate_lob_speeds(self, distance_to_target, target_height, z_init=0) -> t
 
         time = vy * (2/constants.g)
 
-        vx = distance_to_target / time
+        vx = distance_to_target / time 
 
         return vy, vx
 
@@ -132,14 +134,17 @@ def calculate_angle_feed_zone(self, distance_to_target: float, z_init: float) ->
 
         theta = np.arctan(vy / vx) if vx != 0 else np.radians(90)
 
+        if theta > np.radians(60):
+            theta = np.radians(55)
+
         return theta
 
     def get_flywheel_speed_feed(self, distance_to_target: float) -> float:
 
 
         vy, vx = self.calculate_lob_speeds(distance_to_target, config.feed_shot_target_height, self.get_shooter_height())
 
-        v_total = np.sqrt(vy ** 2 + vx **2)
+        v_total = np.sqrt(vy ** 2 + vx **2) + 4
         # print(vx, vy, v_total)
 
         return  min(v_total, config.v0_flywheel_maximum)
@@ -172,14 +177,26 @@ def update_shooter(self):
 
         if type(self.feed_zone) == Translation3d:
             self.feed_zone = self.feed_zone.toTranslation2d()
+
+        if type(self.feed_zone_midline) == Translation3d:
+            self.feed_zone_midline = self.feed_zone.toTranslation2d()
 
         self.distance_to_target = (
             self.odometry.getPose().translation().distance(self.speaker) - constants.shooter_offset_y
         )
 
+        active_feed_zone = self.feed_zone
+
+        pose_x = self.odometry.getPose().X()
+
+        if pose_x > constants.FieldPos.op_wing_boundary:
+            active_feed_zone = self.feed_zone_midline
+
         self.distance_to_feed_zone = (
-            self.odometry.getPose().translation().distance(self.feed_zone) - constants.shooter_offset_y
+            self.odometry.getPose().translation().distance(active_feed_zone) - constants.shooter_offset_y
         )
+
+
         # print("distance_to_target", self.distance_to_target)
 
         self.delta_z = (
@@ -234,7 +251,13 @@ def get_rotation_to_feed_zone(self):
         returns rotation of base to face target
         :return: base target angle
         """
-        speaker_translation:Translation2d = POI.Coordinates.Structures.Scoring.kFeed.getTranslation()
+
+        pose_x = self.odometry.getPose().X()
+        feed_location = POI.Coordinates.Structures.Scoring.kFeed.getTranslation()
+        if pose_x > constants.FieldPos.op_wing_boundary:
+            feed_location = POI.Coordinates.Structures.Scoring.kFeedMidline.getTranslation()
+
+        speaker_translation:Translation2d = feed_location
         robot_pose_2d = self.odometry.getPose()
         robot_to_speaker = speaker_translation - robot_pose_2d.translation()
         return robot_to_speaker.angle()

utils/POI.py

@@ -336,6 +336,14 @@ class Scoring:
                         constants.FieldPos.Scoring.amp_rotation,
                     )
                 )
+
+                kFeedMidline = POIPose(
+                    Pose2d(
+                        6.460,
+                        constants.field_width - 0.623,
+                        3.109,
+                    )
+                )
 
                 kAmpActual = POIPose(
                     Pose2d(