add KI

tello_sim/ursina_adapter.py

@@ -4,7 +4,6 @@ from OpenGL.GL import glReadPixels, GL_RGBA, GL_UNSIGNED_BYTE
 import numpy as np
 from typing import Literal
 import cv2
-import numpy as np
 from ursina import (
     Ursina,
     window,
@@ -175,6 +174,15 @@ class UrsinaAdapter():
             cast_shadow=True
         )
 
+        self.walking_person = Entity(
+            model='entities/business_man.glb',
+            scale=7.3,
+            position=(0, 13.0, 40),
+            rotation=(0, 90, 0),
+            collider='box',
+            cast_shadow=True
+        )
+
         self.light1 = Entity(
             model='entities/street_light.glb',
             scale=(4, 6.5, 5),  
@@ -303,6 +311,12 @@ class UrsinaAdapter():
         self.max_roll = 20  
         self.tilt_smoothness = 0.05  
 
+        # RC control state
+        self.rc_lr = 0.0
+        self.rc_fb = 0.0
+        self.rc_ud = 0.0
+        self.rc_yaw = 0.0
+
         self.create_meters()
         
     def run(self):
@@ -478,6 +492,7 @@ class UrsinaAdapter():
         return int(self.velocity.x * 3.6)  
 
     def get_speed_y(self) -> int:
+        from ursina import time as ursina_time
         current_time = time()
         elapsed_time = current_time - self.last_time
 
@@ -585,35 +600,90 @@ class UrsinaAdapter():
             self.emergency()
     
     def update_movement(self) -> None:
-        self.velocity += self.acceleration
+        import ursina
+        dt = ursina.time.dt
+        t = time()
+
+        # Update walking person movement
+        # Moves between -15 and 15 on X axis
+        walk_speed = 2.0
+        old_x = self.walking_person.x
+        self.walking_person.x = sin(t * 0.5) * 15
         
-        if self.velocity is None:
-            raise Exception("Velocity is None")
+        # Rotate person based on direction
+        if self.walking_person.x > old_x:
+            self.walking_person.rotation_y = 90
+        else:
+            self.walking_person.rotation_y = -90
 
-        if self.velocity.length() > self.max_speed:
-            self.velocity = self.velocity.normalized() * self.max_speed
+        # Apply RC control in world space based on local drone orientation
+        if self.is_flying:
+            # Tello RC values are -100 to 100. 
+            speed_mult_h = 0.18  # Increased: 100 -> 18.0 m/s for faster forward
+            speed_mult_v = 0.05  # Vertical: 100 -> 5.0 m/s
+            
+            # Robust local-to-world conversion using yaw angle
+            rad = np.radians(self.drone.rotation_y)
+            sin_y = np.sin(rad)
+            cos_y = np.cos(rad)
+            
+            # vx = right-axis component, vz = forward-axis component
+            vx = (self.rc_fb * speed_mult_h * sin_y) + (self.rc_lr * speed_mult_h * cos_y)
+            vz = (self.rc_fb * speed_mult_h * cos_y) - (self.rc_lr * speed_mult_h * sin_y)
+            vy = self.rc_ud * speed_mult_v
+            
+            target_velocity = Vec3(vx, vy, vz)
+            
+            # Smoothly interpolate current velocity towards target
+            self.velocity = lerp(self.velocity, target_velocity, 10 * dt)
+            
+            # Apply yaw rotation (degrees per second)
+            # Reduced significantly: multiplier from 3.5 to 1.2
+            self.drone.rotation_y += self.rc_yaw * 1.2 * dt
+            
+            # Set target tilt angles for visual feedback
+            self.pitch_angle = (self.rc_fb / 100.0) * self.max_pitch
+            self.roll_angle = (self.rc_lr / 100.0) * self.max_roll
 
-        self.velocity *= self.drag
-        new_position = self.drone.position + self.velocity
-        hit_info = raycast(self.drone.position, self.velocity.normalized(), distance=self.velocity.length(), ignore=(self.drone,)) # type: ignore
+        # Acceleration (m/s^2) from move() commands
+        self.velocity += self.acceleration * dt
+        
+        if self.velocity.length() > 20.0: # Max speed 20 m/s
+            self.velocity = self.velocity.normalized() * 20.0
 
-        if not hit_info.hit:
-            self.drone.position = new_position  
+        # Apply drag when no active RC input
+        if abs(self.rc_fb) < 1 and abs(self.rc_lr) < 1 and abs(self.rc_ud) < 1:
+            self.velocity = lerp(self.velocity, Vec3(0,0,0), 3 * dt)
+        
+        # Frame-independent position update
+        new_position = self.drone.position + self.velocity * dt
+        
+        # Collision check
+        if self.velocity.length() > 0.01:
+            hit_info = raycast(self.drone.position + Vec3(0,0.1,0), self.velocity.normalized(), 
+                               distance=self.velocity.length() * dt + 0.1, ignore=(self.drone,)) # type: ignore
+            if not hit_info.hit:
+                self.drone.position = new_position  
+        else:
+            self.drone.position = new_position
 
-        if self.drone.y < 3:
-            self.drone.y = 3
+        if self.drone.y < 2.6: # Ground level
+            self.drone.y = 2.6
+            if self.velocity.y < 0: self.velocity.y = 0
 
         self.acceleration = Vec3(0, 0, 0)
 
         # Apply pitch and roll to the drone
         self.drone.rotation_x = lerp(self.drone.rotation_x, self.pitch_angle, self.tilt_smoothness)
         self.drone.rotation_z = lerp(self.drone.rotation_z, self.roll_angle, self.tilt_smoothness)
-        current_time = time()
-        dt = current_time - self.last_time_accel
+        
+        from time import time as wall_time
+        current_time = wall_time()
+        dt_accel = current_time - self.last_time_accel
 
-        if dt > 0:
+        if dt_accel > 0:
             velocity_change = self.velocity - self.last_velocity_accel
-            self.calculated_acceleration = velocity_change / dt # type: ignore
+            self.calculated_acceleration = velocity_change / dt_accel # type: ignore
 
             self.last_velocity_accel = Vec3(self.velocity.x, self.velocity.y, self.velocity.z)
             self.last_time_accel = current_time
@@ -697,15 +767,11 @@ class UrsinaAdapter():
         self.drone.rotation_z = lerp(self.drone.rotation_z, self.roll_angle, self.tilt_smoothness)
         
     def send_rc_control(self, left_right_velocity_ms: float, forward_backward_velocity_ms: float, up_down_velocity_ms: float, yaw_velocity_ms: float):
-        
-        self.velocity = Vec3(
-            -left_right_velocity_ms / 100,        # LEFT/RIGHT mapped to X
-            up_down_velocity_ms / 100,            # UP/DOWN mapped to Y
-            forward_backward_velocity_ms / 100   # FORWARD/BACKWARD mapped to Z
-        )
-
-        self.drone.rotation_y += -yaw_velocity_ms * 0.05  # Smooth yaw rotation
-        print(f"[RC Control] Velocities set -> LR: {left_right_velocity_ms}, FB: {forward_backward_velocity_ms}, UD: {up_down_velocity_ms}, Yaw: {yaw_velocity_ms}")
+        # Only store the target RC values to be applied in the main thread (update_movement)
+        self.rc_lr = left_right_velocity_ms
+        self.rc_fb = forward_backward_velocity_ms
+        self.rc_ud = up_down_velocity_ms
+        self.rc_yaw = yaw_velocity_ms
 
     @staticmethod
     def map_coords(x: float, y: float, z: float) -> Vec3:
@@ -892,7 +958,8 @@ class UrsinaAdapter():
         rolling = False
         
         if self.bezier_mode:
-            t_now = time()
+            from time import time as wall_time
+            t_now = wall_time()
             elapsed = t_now - self.bezier_start_time # type: ignore
             t = min(1.0, elapsed / self.bezier_duration)
 
@@ -929,4 +996,5 @@ class UrsinaAdapter():
         self.update_movement()
         self.update_pitch_roll()
 
-    
+    def update(self):
+        self.tick()