""" [단계 4.5] HC-SR04 초음파 센서로 자율 회피·순찰·따라가기 학습 목표: - UDP 패킷에 want_distance 플래그를 붙여 거리값을 받는 법 - 거리 임계값 조건문으로 모드별 동작 분기 - 시간 기반 상태 머신(K-턴 시퀀스) 흐름 이해 조작: 1 : 수동 (정지 상태) 2 : 회피 - 장애물 만나면 K-턴으로 우회 (항상 우회전) 3 : 순찰 - 장애물마다 좌·우 교대 회전 4 : 따라가기 - 전방 물체와 18~35cm 간격 유지 SPACE : 즉시 정지 q : 종료 ⚠️ 안전 수칙 - 첫 실행은 반드시 차량을 책 위에 올려 바퀴가 공중에 뜨게! - 영상 분석은 사용하지 않습니다 (HC-SR04 거리값만 사용) - STA 펌웨어에는 통신 워치독이 없으니 종료 시 정지 명령 필수 """ import socket import json import time import cv2 import numpy as np # ────────────────────────────────────────── # 설정 # ────────────────────────────────────────── # TODO 1: ESP32_IP를 본인 차량 IP로 변경 ESP32_IP = "192.168.137.74" ESP32_PORT = 4210 # 차량 안전 한계 STEERING_NEUTRAL = 90 AUTO_SPEED = 110 # 자율 모드 속도 (0~200) AUTO_STEER_DELTA = 30 # K-턴 시 서보 꺾는 각 # 거리 임계값 (cm) OBSTACLE_DIST_CM = 18 # 회피·순찰: 이 거리 이하면 장애물 FOLLOW_NEAR_CM = 18 # 따라가기: 이보다 가까우면 후진 FOLLOW_FAR_CM = 35 # 따라가기: 이보다 멀면 전진 FOLLOW_LOST_CM = 120 # 따라가기: 이보다 멀면 놓침 # K-턴 단계별 시간 (초) T_STOP = 0.15 T_BACK = 0.60 T_FWD = 0.60 # ────────────────────────────────────────── # UDP 통신 # ────────────────────────────────────────── def send_and_recv(sock, speed, steering): """제어 명령 + 거리 요청을 한 번의 UDP 왕복으로 처리. Returns: int or None : 거리(cm). None이면 응답 없음. 999면 측정 불가. """ payload = {"speed": int(speed), "steering": int(steering), "want_distance": True} try: sock.sendto(json.dumps(payload).encode('utf-8'), (ESP32_IP, ESP32_PORT)) data, _ = sock.recvfrom(256) resp = json.loads(data.decode('utf-8')) # TODO 2: resp 딕셔너리에서 'distance' 키 값을 꺼내 int로 반환 # (응답에 distance가 없으면 None 반환) pass except (socket.timeout, OSError, json.JSONDecodeError, ValueError): return None # ────────────────────────────────────────── # K-턴 회피 시퀀스 (시간 기반 상태 머신) # STOP → 후진+서보꺾기 → STOP → 전진+서보반대 → STOP # # turn_left=True 일 때: # 후진 시 서보 우(>90) → 정지 → 전진 시 서보 좌(<90) # → 결과적으로 차량은 좌회전 (Ackermann 후진 회전 특성) # ────────────────────────────────────────── def start_kturn(state, turn_left): """회피 시퀀스 시작 (state 딕셔너리만 갱신, 명령은 다음 tick부터).""" state['phase'] = 'STOP1' state['until'] = time.time() + T_STOP state['turn_left'] = turn_left state['cmd'] = (0, STEERING_NEUTRAL) def tick_kturn(state): """K-턴 진행. (speed, steering) 반환. None이면 시퀀스 종료.""" if state['phase'] == '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'] == 'STOP1': cmd = (-AUTO_SPEED, back_s) state['phase'], state['until'] = 'BACK', time.time() + T_BACK elif state['phase'] == 'BACK': cmd = (0, back_s) state['phase'], state['until'] = 'STOP2', time.time() + T_STOP elif state['phase'] == 'STOP2': cmd = (AUTO_SPEED, fwd_s) state['phase'], state['until'] = 'FWD', time.time() + T_FWD elif state['phase'] == 'FWD': cmd = (0, STEERING_NEUTRAL) state['phase'], state['until'] = 'STOP3', time.time() + T_STOP else: # STOP3 state['phase'] = 'NONE' return None state['cmd'] = cmd return cmd # ────────────────────────────────────────── # 모드별 동작 결정 (★ 학생 작성 영역) # ────────────────────────────────────────── def mode_avoid(distance, state): """회피 모드: 장애물이면 우회전 K-턴, 아니면 직진.""" # TODO 3: 회피 모드 동작 # - distance가 OBSTACLE_DIST_CM 미만이면: # start_kturn(state, turn_left=False) ← 우회전 # (0, STEERING_NEUTRAL) 반환 (다음 tick부터 K-턴이 진행됨) # - 그 외에는 (AUTO_SPEED, STEERING_NEUTRAL) 반환 (직진) # # ※ distance가 None이거나 999인 경우는 "장애물 없음"으로 처리하면 됨 pass def mode_patrol(distance, state): """순찰 모드: 장애물마다 좌·우 교대 K-턴.""" # TODO 4: 순찰 모드 동작 # - distance가 OBSTACLE_DIST_CM 미만이면: # start_kturn(state, turn_left=state['patrol_left']) # state['patrol_left']을 not으로 토글 # (0, STEERING_NEUTRAL) 반환 # - 그 외에는 직진 pass def mode_follow(distance): """따라가기 모드: 거리 구간별 동작. 서보는 항상 중립.""" # TODO 5: 따라가기 모드 # - distance가 None이거나 999 이상 : 정지 (대상 없음) # - distance < FOLLOW_NEAR_CM : 후진 (-AUTO_SPEED) # - distance > FOLLOW_LOST_CM : 정지 (놓침) # - distance > FOLLOW_FAR_CM : 전진 (AUTO_SPEED) # - 그 외 (적정 거리) : 정지 (0) pass # ────────────────────────────────────────── # 화면 표시 # ────────────────────────────────────────── def draw(mode, distance, speed, steering, phase): canvas = np.zeros((300, 520, 3), dtype=np.uint8) cv2.putText(canvas, f"MODE: {mode}", (20, 45), cv2.FONT_HERSHEY_SIMPLEX, 1.0, (0, 255, 255), 2) if distance is None: d_text, d_color = "Distance: --", (120, 120, 120) elif distance >= 999: d_text, d_color = "Distance: INF", (180, 180, 180) else: d_text = f"Distance: {distance} cm" d_color = (0, 0, 255) if distance < OBSTACLE_DIST_CM else (0, 255, 0) cv2.putText(canvas, d_text, (20, 95), cv2.FONT_HERSHEY_SIMPLEX, 0.9, d_color, 2) cv2.putText(canvas, f"Speed: {speed} Steer: {steering}", (20, 140), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 255, 255), 1) cv2.putText(canvas, f"K-turn phase: {phase}", (20, 175), cv2.FONT_HERSHEY_SIMPLEX, 0.55, (200, 200, 200), 1) cv2.putText(canvas, "1:Manual 2:Avoid 3:Patrol 4:Follow", (20, 230), cv2.FONT_HERSHEY_SIMPLEX, 0.55, (200, 200, 200), 1) cv2.putText(canvas, "SPACE: STOP q: Quit", (20, 265), cv2.FONT_HERSHEY_SIMPLEX, 0.55, (200, 200, 200), 1) return canvas # ────────────────────────────────────────── # 메인 루프 # ────────────────────────────────────────── def main(): sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) sock.settimeout(0.2) print(f"ESP32 대상: {ESP32_IP}:{ESP32_PORT}") print("⚠️ 안전: 첫 실행은 차량 바퀴를 공중에 띄운 상태로!") mode = 'manual' state = {'phase': 'NONE', 'until': 0.0, 'turn_left': False, 'cmd': (0, STEERING_NEUTRAL), 'patrol_left': False} distance = None while True: # 1) K-턴이 진행 중이면 그쪽이 우선 cmd = tick_kturn(state) # 2) 시퀀스 진행 중이 아니면 모드별 결정 if cmd is None: if mode == 'avoid': cmd = mode_avoid(distance, state) elif mode == 'patrol': cmd = mode_patrol(distance, state) elif mode == 'follow': cmd = mode_follow(distance) else: # manual cmd = (0, STEERING_NEUTRAL) if cmd is None: cmd = (0, STEERING_NEUTRAL) # TODO 미작성 시 안전 기본값 speed, steering = cmd # 3) UDP 전송 + 거리 수신 (한 번의 왕복) distance = send_and_recv(sock, speed, steering) # 4) 화면 표시 cv2.imshow('Ultrasonic Drive', draw(mode, distance, speed, steering, state['phase'])) # 5) 키 입력 key = cv2.waitKey(50) & 0xFF if key == ord('q'): break elif key == ord(' '): mode = 'manual'; state['phase'] = 'NONE' elif key == ord('1'): mode = 'manual'; state['phase'] = 'NONE' elif key == ord('2'): mode = 'avoid'; state['phase'] = 'NONE' elif key == ord('3'): mode = 'patrol'; state['phase'] = 'NONE'; state['patrol_left'] = False elif key == ord('4'): mode = 'follow'; state['phase'] = 'NONE' # 종료 시 반드시 정지 (STA 펌웨어는 워치독 없음) send_and_recv(sock, 0, STEERING_NEUTRAL) sock.close() cv2.destroyAllWindows() print("종료") if __name__ == "__main__": main() # ════════════════════════════════════════════════════════════ # 정답 # ════════════════════════════════════════════════════════════ """ [TODO 2] 거리 파싱 if 'distance' in resp: return int(resp['distance']) return None [TODO 3] 회피 모드 if distance is not None and distance < OBSTACLE_DIST_CM and distance > 0: start_kturn(state, turn_left=False) return (0, STEERING_NEUTRAL) return (AUTO_SPEED, STEERING_NEUTRAL) [TODO 4] 순찰 모드 if distance is not None and distance < OBSTACLE_DIST_CM and distance > 0: start_kturn(state, turn_left=state['patrol_left']) state['patrol_left'] = not state['patrol_left'] return (0, STEERING_NEUTRAL) return (AUTO_SPEED, STEERING_NEUTRAL) [TODO 5] 따라가기 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) """