diff --git a/naowalk.py b/naowalk.py index c156980..1013335 100644 --- a/naowalk.py +++ b/naowalk.py @@ -460,6 +460,8 @@ class VideoServerLauncher: # MAIN TELEOP # ========================================== class NaoTeleop: + HEAD_RATE = 1.4 # rad/sec - how fast a held IJKL key sweeps the head target angle + def __init__(self, session, nao_ip, audio_device_index=None, mic_gain=8.0, video_scale=2.0, nao_ssh_user="nao", nao_ssh_password="nao", nao_ssh_port=22, relay_rate=16000, media_relay=None, deploy_video_server=True, video_server_path=None): @@ -501,6 +503,7 @@ class NaoTeleop: self.running = True self.head_yaw = 0.0 self.head_pitch = 0.0 + self._last_head_update = time.time() self.battery_level = 100 self.last_batt_check = 0 self.volume = 50 @@ -783,15 +786,21 @@ class NaoTeleop: def handle_head_movement(self): if self.typing_mode or self.music_search_mode: return keys = pygame.key.get_pressed() - speed = 0.3 + + now = time.time() + dt = now - self._last_head_update + self._last_head_update = now + step = self.HEAD_RATE * dt + changed = False - if keys[pygame.K_i]: self.head_pitch = max(self.head_pitch - speed, -0.5); changed = True - if keys[pygame.K_k]: self.head_pitch = min(self.head_pitch + speed, 0.5); changed = True - if keys[pygame.K_j]: self.head_yaw = min(self.head_yaw + speed, 2.0); changed = True - if keys[pygame.K_l]: self.head_yaw = max(self.head_yaw - speed, -2.0); changed = True + if keys[pygame.K_i]: self.head_pitch = max(self.head_pitch - step, -0.5); changed = True + if keys[pygame.K_k]: self.head_pitch = min(self.head_pitch + step, 0.5); changed = True + if keys[pygame.K_j]: self.head_yaw = min(self.head_yaw + step, 2.0); changed = True + if keys[pygame.K_l]: self.head_yaw = max(self.head_yaw - step, -2.0); changed = True + if changed: self._safe("Head move", lambda: self.motion.setAngles( - ["HeadYaw", "HeadPitch"], [self.head_yaw, self.head_pitch], 0.3)) + ["HeadYaw", "HeadPitch"], [self.head_yaw, self.head_pitch], 0.18)) def reset_head(self): self.head_yaw = 0.0