""" ESP32-CAM 초음파 자율주행 (OpenCV/카메라 미사용) ───────────────────────────────────────────── HC-SR04 초음파 센서 거리값만 사용해 차량을 자율 주행합니다. 영상 처리(차선/객체 인식)와 별도로 독립 실행되는 단순 프로그램입니다. 모드 (키보드로 전환): 1 : 수동 - 정지 상태로 대기 2 : 회피 - 18cm 이하 장애물 만나면 K-턴으로 우회 3 : 순찰 - 회피와 동일하나 좌·우 교대로 회전 4 : 따라가기 - 전방 물체와 18~35cm 간격 유지 기타: SPACE : 즉시 정지 (모드를 수동으로 복귀) q : 종료 통신: UDP {"speed":N,"steering":N,"want_distance":true} → ESP32:4210 응답 {"status":"ok","speed":N,"steering":N,"distance":N} • distance == 999 → 측정 불가 (5m 이상 또는 타임아웃) ⚠️ 안전 수칙 - 첫 실행은 반드시 차량을 책 위에 올려 바퀴가 공중에 뜨게! - SPACE 키 위에 항상 손가락 대기 - STA 펌웨어에는 통신 워치독이 없습니다. 종료 시 finally에서 반드시 정지 명령이 나가야 차가 멈춥니다. """ import socket import json import time import os import cv2 import numpy as np # ───── 통신 설정 (config.json에서 자동 로드) ───── DEFAULT_ESP32_IP = "192.168.137.74" DEFAULT_UDP_PORT = 4210 RECV_TIMEOUT_S = 0.2 # ───── 차량 안전 한계 (펌웨어와 동일) ───── STEERING_MIN = 45 STEERING_MAX = 135 STEERING_NEUTRAL = 90 SPEED_MAX = 200 # ───── 자율주행 파라미터 (AP 펌웨어 esp32_cam_phone_drive.ino와 동일) ───── AUTO_SPEED = 110 # 자율 모드 기본 속도 (0~SPEED_MAX) AUTO_STEER_DELTA = 30 # K-턴 시 서보 꺾는 각 (중립 ±) OBSTACLE_DIST_CM = 18 # 회피·순찰: 이 거리 이하면 장애물 FOLLOW_NEAR_CM = 18 # 따라가기: 이보다 가까우면 후진 FOLLOW_FAR_CM = 35 # 따라가기: 이보다 멀면 전진 FOLLOW_LOST_CM = 120 # 따라가기: 이보다 멀면 대상 놓침 PHASE_STOP_S = 0.15 PHASE_BACK_S = 0.60 PHASE_FWD_S = 0.60 LOOP_INTERVAL_S = 0.10 # 100ms 주기 (10Hz) # ───── 모드 ───── MODE_MANUAL = 'manual' MODE_AVOID = 'avoid' MODE_PATROL = 'patrol' MODE_FOLLOW = 'follow' # ───── K-턴 phase ───── PH_NONE, PH_STOP1, PH_BACK, PH_STOP2, PH_FORWARD, PH_STOP3 = range(6) PHASE_NAME = {PH_NONE: "-", PH_STOP1: "STOP1", PH_BACK: "BACK", PH_STOP2: "STOP2", PH_FORWARD: "FWD", PH_STOP3: "STOP3"} # ────────────────────────────────────────── # 설정 로드 / IP 입력 # ────────────────────────────────────────── def resolve_esp32_ip(): """config.json에서 IP를 읽고 사용자가 끝자리만 바꿀 수 있게 한다.""" config_path = os.path.join(os.path.dirname(__file__), '..', '..', 'config', 'config.json') ip_base = "192.168.137." ip_last = "74" udp_port = DEFAULT_UDP_PORT if os.path.exists(config_path): try: with open(config_path, 'r', encoding='utf-8') as f: cfg = json.load(f) full_ip = cfg['esp32']['ip'] parts = full_ip.rsplit('.', 1) ip_base = parts[0] + '.' ip_last = parts[1] udp_port = cfg['esp32'].get('udp_port', DEFAULT_UDP_PORT) except Exception as e: print(f"config.json 읽기 실패 → 기본값 사용 ({e})") print(f"\n=== ESP32-CAM IP 설정 ===") print(f"기본 IP: {ip_base}{ip_last}") user_in = input(f"IP 끝자리 (엔터=기본값 {ip_last}): ").strip() if user_in: try: n = int(user_in) if 1 <= n <= 254: ip_last = user_in except ValueError: pass return ip_base + ip_last, udp_port # ────────────────────────────────────────── # UDP 통신 # ────────────────────────────────────────── def send_and_recv(sock, esp32_ip, udp_port, speed, steering): """제어 명령 + 거리 요청을 한 번의 UDP 왕복으로 처리. Returns: (distance:int|None, rtt_ms:float) • distance None → 응답 없음 (타임아웃/파싱 오류) • distance 999 → 펌웨어 측 측정 불가 (반사파 없음) """ payload = {"speed": int(speed), "steering": int(steering), "want_distance": True} t0 = time.time() try: sock.sendto(json.dumps(payload).encode('utf-8'), (esp32_ip, udp_port)) data, _ = sock.recvfrom(256) resp = json.loads(data.decode('utf-8')) rtt = (time.time() - t0) * 1000.0 if 'distance' in resp: return int(resp['distance']), rtt return None, rtt except (socket.timeout, OSError, json.JSONDecodeError, ValueError): return None, (time.time() - t0) * 1000.0 def send_stop(sock, esp32_ip, udp_port): """정지 명령 (응답 안 기다림).""" try: sock.sendto( json.dumps({"speed": 0, "steering": STEERING_NEUTRAL}).encode('utf-8'), (esp32_ip, udp_port)) except OSError: pass # ────────────────────────────────────────── # K-턴 회피 시퀀스 (시간 기반 상태 머신) # STOP1 → 후진+서보꺾기 → STOP2 → 전진+서보반대 → STOP3 # # Ackermann 차량 직관: # - 전진 + 서보 우(>90) → 차가 우회전 # - 후진 + 서보 우(>90) → 차의 앞이 좌측을 향함 (차체 시계반대 회전) # "최종적으로 좌회전" 하려면(turn_left=True): # 후진 시 서보 우(>90) → 정지 → 전진 시 서보 좌(<90) # ────────────────────────────────────────── def start_kturn(state, turn_left): """회피 시퀀스 시작.""" state['phase'] = PH_STOP1 state['until'] = time.time() + PHASE_STOP_S state['turn_left'] = turn_left state['cmd'] = (0, STEERING_NEUTRAL) def tick_kturn(state): """K-턴 한 단계 진행. (speed, steering) 반환. None이면 시퀀스 종료.""" if state['phase'] == PH_NONE: return None if time.time() < state['until']: return state['cmd'] back_s = (STEERING_NEUTRAL + AUTO_STEER_DELTA) if state['turn_left'] \ else (STEERING_NEUTRAL - AUTO_STEER_DELTA) fwd_s = (STEERING_NEUTRAL - AUTO_STEER_DELTA) if state['turn_left'] \ else (STEERING_NEUTRAL + AUTO_STEER_DELTA) if state['phase'] == PH_STOP1: cmd = (-AUTO_SPEED, back_s) state['phase'], state['until'] = PH_BACK, time.time() + PHASE_BACK_S elif state['phase'] == PH_BACK: cmd = (0, back_s) state['phase'], state['until'] = PH_STOP2, time.time() + PHASE_STOP_S elif state['phase'] == PH_STOP2: cmd = (AUTO_SPEED, fwd_s) state['phase'], state['until'] = PH_FORWARD, time.time() + PHASE_FWD_S elif state['phase'] == PH_FORWARD: cmd = (0, STEERING_NEUTRAL) state['phase'], state['until'] = PH_STOP3, time.time() + PHASE_STOP_S else: # PH_STOP3 state['phase'] = PH_NONE return None state['cmd'] = cmd return cmd # ────────────────────────────────────────── # 모드별 동작 결정 # ────────────────────────────────────────── def decide(mode, distance, state): """현재 모드와 거리로 (speed, steering) 결정.""" # 회피 시퀀스가 진행 중이면 그쪽이 우선 cmd = tick_kturn(state) if cmd is not None: return cmd obstacle = distance is not None and 0 < distance < OBSTACLE_DIST_CM if mode == MODE_AVOID: if obstacle: start_kturn(state, turn_left=False) # 회피는 항상 우회전 return (0, STEERING_NEUTRAL) return (AUTO_SPEED, STEERING_NEUTRAL) if mode == MODE_PATROL: if obstacle: start_kturn(state, turn_left=state['patrol_left']) state['patrol_left'] = not state['patrol_left'] return (0, STEERING_NEUTRAL) return (AUTO_SPEED, STEERING_NEUTRAL) if mode == MODE_FOLLOW: if distance is None or distance >= 999: return (0, STEERING_NEUTRAL) if distance < FOLLOW_NEAR_CM: return (-AUTO_SPEED, STEERING_NEUTRAL) if distance > FOLLOW_LOST_CM: return (0, STEERING_NEUTRAL) if distance > FOLLOW_FAR_CM: return (AUTO_SPEED, STEERING_NEUTRAL) return (0, STEERING_NEUTRAL) # 적정 거리 return (0, STEERING_NEUTRAL) # MANUAL # ────────────────────────────────────────── # 화면 표시 # ────────────────────────────────────────── MODE_COLOR = { MODE_MANUAL: (180, 180, 180), MODE_AVOID: (0, 165, 255), MODE_PATROL: (0, 255, 255), MODE_FOLLOW: (0, 255, 100), } def draw_canvas(mode, distance, speed, steering, phase, rtt_ms, esp32_ip): canvas = np.zeros((380, 580, 3), dtype=np.uint8) cv2.putText(canvas, f"MODE: {mode.upper()}", (20, 50), cv2.FONT_HERSHEY_SIMPLEX, 1.1, MODE_COLOR[mode], 2) if distance is None: d_text, d_color = "Distance: --- (no reply)", (100, 100, 100) elif distance >= 999: d_text, d_color = "Distance: INF (out of range)", (180, 180, 180) else: d_text = f"Distance: {distance:>3} cm" d_color = (0, 0, 255) if distance < OBSTACLE_DIST_CM else (0, 255, 0) cv2.putText(canvas, d_text, (20, 100), cv2.FONT_HERSHEY_SIMPLEX, 0.9, d_color, 2) cv2.putText(canvas, f"Speed: {speed:>5}", (20, 150), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 255), 1) cv2.putText(canvas, f"Steering: {steering:>5}", (20, 180), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 255), 1) cv2.putText(canvas, f"K-turn phase: {PHASE_NAME[phase]}", (20, 210), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (200, 200, 200), 1) cv2.putText(canvas, f"RTT: {rtt_ms:>4.0f} ms ESP32: {esp32_ip}", (20, 240), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (180, 180, 180), 1) cv2.line(canvas, (20, 260), (560, 260), (60, 60, 60), 1) cv2.putText(canvas, "1: Manual 2: Avoid 3: Patrol 4: Follow", (20, 290), cv2.FONT_HERSHEY_SIMPLEX, 0.55, (200, 200, 200), 1) cv2.putText(canvas, "SPACE: STOP (back to manual) q: Quit", (20, 320), cv2.FONT_HERSHEY_SIMPLEX, 0.55, (200, 200, 200), 1) cv2.putText(canvas, "[!] Lift wheels off ground for first run!", (20, 360), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 100, 255), 1) return canvas # ────────────────────────────────────────── # 메인 # ────────────────────────────────────────── def main(): esp32_ip, udp_port = resolve_esp32_ip() sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) sock.settimeout(RECV_TIMEOUT_S) print(f"\nESP32 대상: {esp32_ip}:{udp_port}") print("⚠️ 안전: 첫 실행은 반드시 차량 바퀴를 공중에 띄운 상태로 시작.") print("준비되면 OpenCV 창을 클릭한 뒤 키보드(1~4, SPACE, q)를 사용하세요.\n") mode = MODE_MANUAL state = { 'phase': PH_NONE, 'until': 0.0, 'turn_left': False, 'cmd': (0, STEERING_NEUTRAL), 'patrol_left': False, } distance = None speed, steering = 0, STEERING_NEUTRAL rtt_ms = 0.0 cv2.namedWindow('Ultrasonic Drive') try: while True: loop_start = time.time() # 1) 모드/상태로 명령 결정 speed, steering = decide(mode, distance, state) # 2) UDP 전송 + 거리 수신 (한 왕복) distance, rtt_ms = send_and_recv( sock, esp32_ip, udp_port, speed, steering) # 3) 화면 cv2.imshow( 'Ultrasonic Drive', draw_canvas(mode, distance, speed, steering, state['phase'], rtt_ms, esp32_ip)) # 4) 키 입력 key = cv2.waitKey(1) & 0xFF if key == ord('q'): break elif key == ord(' '): mode = MODE_MANUAL state['phase'] = PH_NONE send_stop(sock, esp32_ip, udp_port) print("긴급 정지 → 수동 모드") elif key == ord('1'): mode, state['phase'] = MODE_MANUAL, PH_NONE print("모드: 수동") elif key == ord('2'): mode, state['phase'] = MODE_AVOID, PH_NONE print("모드: 회피") elif key == ord('3'): mode, state['phase'] = MODE_PATROL, PH_NONE state['patrol_left'] = False print("모드: 순찰 (좌·우 교대)") elif key == ord('4'): mode, state['phase'] = MODE_FOLLOW, PH_NONE print("모드: 따라가기 (18~35cm 유지)") # 5) 주기 유지 sleep_s = LOOP_INTERVAL_S - (time.time() - loop_start) if sleep_s > 0: time.sleep(sleep_s) except KeyboardInterrupt: print("\nKeyboardInterrupt") finally: # STA 펌웨어는 워치독이 없음 → 두 번 보내 확실히 정지 send_stop(sock, esp32_ip, udp_port) time.sleep(0.1) send_stop(sock, esp32_ip, udp_port) sock.close() cv2.destroyAllWindows() print("종료 — 정지 명령 전송 완료") if __name__ == "__main__": main()