add models
This commit is contained in:
Sebastian Unterschütz
@@ -4,19 +4,24 @@ from typing import List, Tuple, Dict
|
||||
from .config import Config
|
||||
|
||||
class FlightController:
|
||||
"""
|
||||
Verantwortlich für die Berechnung der Flugvektoren basierend auf KI-Ergebnissen.
|
||||
Unterstützt Normal-Modus (sequenziell) und Sport-Modus (simultan/LR).
|
||||
"""
|
||||
def __init__(self):
|
||||
self.last_sent_rc = [0, 0, 0, 0]
|
||||
self.smooth_face = None
|
||||
self.search_start = time.time()
|
||||
self.status = "INITIALIZING"
|
||||
|
||||
# Memory for lost targets
|
||||
self.last_target_side = 0 # -1 for left, 1 for right
|
||||
# Speicher für verloren gegangene Ziele
|
||||
self.last_target_side = 0
|
||||
self.lost_time = 0
|
||||
|
||||
def calculate(self,
|
||||
faces: List[Tuple],
|
||||
is_manual: bool,
|
||||
is_sport: bool, # NEU: Sport-Modus Flag
|
||||
emergency_stop: bool,
|
||||
is_locked: bool,
|
||||
locked_person: Tuple,
|
||||
@@ -29,7 +34,6 @@ class FlightController:
|
||||
|
||||
lr, fb, ud, yv = 0, 0, 0, 0
|
||||
|
||||
# Face smoothing for UI/Visuals
|
||||
if len(faces) > 0:
|
||||
target = max(faces, key=lambda f: f[2] * f[3])
|
||||
if self.smooth_face is None: self.smooth_face = target
|
||||
@@ -42,13 +46,13 @@ class FlightController:
|
||||
self.status = "EMERGENCY STOP"
|
||||
return (0, 0, 0, 0), self.status
|
||||
|
||||
# Obstacle Avoidance (always active if flying)
|
||||
# Hindernisvermeidung
|
||||
center_blocked = zones["CENTER"] or tof < Config.OBSTACLE_TOF_CM
|
||||
if center_blocked:
|
||||
self.status = "AVOIDING OBSTACLE"
|
||||
yv = 80 if zone_scores["LEFT"] < zone_scores["RIGHT"] else -80
|
||||
fb = -30
|
||||
return self._smooth(lr, fb, ud, yv)
|
||||
return self._smooth(0, fb, 0, yv)
|
||||
|
||||
if is_manual:
|
||||
self.status = "MANUAL CONTROL"
|
||||
@@ -59,54 +63,57 @@ class FlightController:
|
||||
# AI LOGIC
|
||||
if is_locked:
|
||||
if locked_person is not None:
|
||||
# Target is visible -> Normal Pursuit
|
||||
self.search_start = time.time()
|
||||
self.lost_time = 0
|
||||
(x, y, w, h) = locked_person
|
||||
center_x = x + w // 2
|
||||
err_x = center_x - (Config.WIDTH // 2)
|
||||
|
||||
# Remember which side it was on
|
||||
self.last_target_side = 1 if err_x > 0 else -1
|
||||
|
||||
# Rotation (Yaw) - SMOOTHER
|
||||
if abs(err_x) > Config.FACE_DEADZONE:
|
||||
yv = int(np.clip(Config.YAW_GAIN * err_x, -50, 50))
|
||||
|
||||
# Forward/Backward pursuit - EXTREME SPEED
|
||||
alignment_factor = max(0.4, 1.0 - (abs(err_x) / Config.FACE_ROT_ONLY))
|
||||
target_fb = int(np.clip(Config.FORWARD_GAIN * (Config.TARGET_PERSON_SIZE - w), -90, 90))
|
||||
fb = int(target_fb * alignment_factor)
|
||||
|
||||
self.status = "PURSUIT: EXTREME"
|
||||
if is_sport:
|
||||
# SPORT MODUS: Alles gleichzeitig + LR-Strafing
|
||||
yv = int(np.clip(Config.SPORT_YAW_GAIN * err_x, -100, 100))
|
||||
fb = int(np.clip(Config.SPORT_FB_GAIN * (Config.TARGET_PERSON_SIZE - w), -100, 100))
|
||||
lr = int(np.clip(Config.SPORT_LR_GAIN * err_x, -60, 60))
|
||||
self.status = "SPORT PURSUIT: FULL AXIS"
|
||||
else:
|
||||
# NORMAL MODUS: Sequenziell (Drehen ODER Fliegen)
|
||||
if abs(err_x) > Config.FACE_DEADZONE:
|
||||
yv = int(np.clip(Config.YAW_GAIN * err_x, -50, 50))
|
||||
fb = 0
|
||||
self.status = "PURSUIT: AIMING"
|
||||
else:
|
||||
yv = 0
|
||||
fb = int(np.clip(Config.FORWARD_GAIN * (Config.TARGET_PERSON_SIZE - w), -80, 80))
|
||||
self.status = "PURSUIT: APPROACHING"
|
||||
else:
|
||||
# Target is LOST -> Rapid Search logic
|
||||
# Target verloren
|
||||
if self.lost_time == 0: self.lost_time = time.time()
|
||||
elapsed = time.time() - self.lost_time
|
||||
|
||||
if elapsed < 10.0: # Search longer and faster
|
||||
yv = 40 * self.last_target_side
|
||||
search_speed = 60 if is_sport else 40
|
||||
if elapsed < 10.0:
|
||||
yv = search_speed * self.last_target_side
|
||||
self.status = f"LOST TARGET: SCANNING {'RIGHT' if self.last_target_side > 0 else 'LEFT'}"
|
||||
else:
|
||||
self.status = "TARGET LOST: AGGRESSIVE PATROL"
|
||||
self.status = "TARGET LOST: PATROL"
|
||||
yv = 30
|
||||
|
||||
elif self.smooth_face is not None:
|
||||
# Face found but not locked
|
||||
(x, y, w, h) = self.smooth_face
|
||||
err_x = (x + w // 2) - (Config.WIDTH // 2)
|
||||
if abs(err_x) > Config.FACE_DEADZONE:
|
||||
yv = int(np.clip(Config.YAW_GAIN * err_x, -40, 40))
|
||||
yv = int(np.clip(Config.YAW_GAIN * err_x, -40, 40))
|
||||
self.status = "AWAITING LOCK"
|
||||
else:
|
||||
# Patrol mode - faster
|
||||
elapsed = (time.time() - self.search_start) % 6.0
|
||||
if elapsed < 2.0:
|
||||
self.status = "PATROL: DASH"
|
||||
fb = 40
|
||||
# Patrouille
|
||||
elapsed = (time.time() - self.search_start) % 8.0
|
||||
if elapsed < 3.0:
|
||||
self.status = "PATROL: ADVANCE"
|
||||
fb = 35
|
||||
else:
|
||||
self.status = "PATROL: SCAN"
|
||||
yv = 30
|
||||
yv = 35
|
||||
|
||||
return self._smooth(lr, fb, ud, yv)
|
||||
|
||||
@@ -117,10 +124,10 @@ class FlightController:
|
||||
sud = int(self.last_sent_rc[2] * (1-alpha) + ud * alpha)
|
||||
syv = int(self.last_sent_rc[3] * (1-alpha) + yv * alpha)
|
||||
|
||||
if abs(slr) < 2: slr = 0
|
||||
if abs(sfb) < 2: sfb = 0
|
||||
if abs(sud) < 2: sud = 0
|
||||
if abs(syv) < 2: syv = 0
|
||||
if abs(slr) < 3: slr = 0
|
||||
if abs(sfb) < 3: sfb = 0
|
||||
if abs(sud) < 3: sud = 0
|
||||
if abs(syv) < 3: syv = 0
|
||||
|
||||
self.last_sent_rc = [slr, sfb, sud, syv]
|
||||
return (slr, sfb, sud, syv), self.status
|
||||
|
||||
Reference in New Issue
Block a user