diff --git a/autopilot.py b/autopilot.py index 4282ebc..5391a42 100644 --- a/autopilot.py +++ b/autopilot.py @@ -326,7 +326,7 @@ class AutoPilot(Pilot): distance_to_target = math.sqrt( (self.points[self.target_idx][0] - self.pos.x) ** 2 + (self.points[self.target_idx][1] - self.pos.y) ** 2 - ) + ) * self.pixel_ratio if distance_to_target < 35: self.target_idx += 1 @@ -339,10 +339,32 @@ class AutoPilot(Pilot): (self.points[self.target_idx][1] - self.pos.y) ** 2 ) + angle_trajectory = self.pos.yaw + math.pi / 2 + + # Проверка на слепую зону + R = 120 + blind = np.array([ + [ + self.pos.x * self.pixel_ratio + R * np.cos(angle_trajectory - np.pi / 2), + self.pos.y * self.pixel_ratio + R * np.sin(angle_trajectory - np.pi / 2), + ], + [ + self.pos.x * self.pixel_ratio + R * np.cos(angle_trajectory + np.pi / 2), + self.pos.y * self.pixel_ratio + R * np.sin(angle_trajectory + np.pi / 2), + ] + ]) + + blind -= self.points[self.target_idx] * self.pixel_ratio + blind = np.hypot(blind[:, 0], blind[:, 1]) + + print("R: ", blind) + if np.min(blind) < R: + return PilotCommand(0, 10, False, self.timer.get_elapsed()) + + + # Вычисляем угол к цели target_angle = math.atan2(self.points[self.target_idx][1] - self.pos.y, self.points[self.target_idx][0] - self.pos.x) - - angle_trajectory = self.pos.yaw + math.pi / 2 # Вычисляем разность углов (направление поворота) angle_diff = target_angle - angle_trajectory @@ -352,7 +374,7 @@ class AutoPilot(Pilot): if angle_diff >= math.pi: angle_diff -= 2 * math.pi - d_r = max(5, min(10., distance_to_target / 2 * self.pixel_ratio)) + d_r = max(5, min(10., distance_to_target / 2)) d_a_limit = np.radians(5) command = PilotCommand(