Cortex-M4(uBrain) 기반 격자 미로 탈출 전략 명세서 (최종 구현: main.c)
| 항목 | 내용 |
|---|---|
| 목표 | 사전 지도 없이 Start에서 출발해 북쪽(Goal) 탈출구에 도달 |
| 로봇 | 원형, 반지름 15 cm, 후진·후방 센서 없음 |
| 초음파(US) | 정면 / 좌 90° / 우 90° |
| IR | 좌 45° / 우 45° (ADC, 값 클수록 가까움) |
| 미로 | 통로 폭 40~70 cm, 중심부 직사각형 정적 섬 장애물 |
| MCU | STM32F429, FreeRTOS 멀티태스크 |
그래프·스택 기반 경로 계획을 기각하고, SRAM을 거의 쓰지 않는 전역 절대 방위(current_heading) 와 국소 센서 반응 규칙만으로 탐색한다.
- 전역 상태:
current_heading1바이트 (0=북, 1=서, 2=남, 3=동) - 후진 루프 없음 — 항상 전진 기반
- IR은 측면 긴급 회피(micro_steer) 용도만 사용
volatile uint8_t current_heading = 0; // 0:북(Goal) 1:서 2:남 3:동90° 회전이 완료될 때마다 current_heading을 갱신한다.
| 회전 | heading 변화 |
|---|---|
| 좌회전 90° | (heading + 1) & 3 |
| 우회전 90° | (heading + 3) & 3 |
| 180° (좌×2) | (heading + 2) & 3 |
GOAL_HEADING = 0 (북) 이 모든 복귀·갈림길 판단의 기준이다.
Sense_task가 100 ms 주기로 필터링한 front_cm, left_cm, right_cm을 Drive_task가 사용한다.
| 판정 | 조건 | 용도 |
|---|---|---|
| 전방 벽 | front_cm < FRONT_THRESH (12 cm) |
CASE 1 트리거 |
| 측면 벽 | cm < WALL_DETECT_DIST_CM (50 cm) |
1.a / 2.b |
| 측면 개방 | cm >= WALL_DETECT_DIST_CM (50 cm) 또는 outlier → SIDE_OPEN_SENTINEL |
1.b / 2.a |
측면 초음파가 연속 outlier이면 트임(open) 으로 간주해 코너·갈림길을 놓치지 않도록 한다 (SIDE_OUTLIER_STREAK = 5).
| 임계값 | 동작 |
|---|---|
IR_THRESH (700) — 양쪽 동시 |
정면 장애물 → CASE 1 (strategy_turn) |
IR_THRESH (700) — 한쪽 |
반대 방향 micro_steer (전진 유지) |
- 90° 회전 시간:
TURN_90_MS = 1840ms (PWM 17000 기준 실측 캘리브레이션) - 회전 전
Motor_Stop()→ 20 ms 대기 → 회전 → 정지
| 상황 | 직진 후 회전 |
|---|---|
CASE 1 전방 벽 (strategy_turn) |
없음 — 벽 앞에서 즉시 회전 |
CASE 2.a 북 복귀 회전 (strategy_turn_*) |
있음 — CORNER_ADVANCE_MS = 950 ms 전진 후 90° |
반지름 15 cm만큼 코너 중심을 지나기 위한 직진이며, 전방에 이미 막힌 상태에서는 적용하지 않는다.
Drive_task는 50 ms 주기로 아래 우선순위를 적용한다.
우선순위 1 — CASE 1: 전방 벽
우선순위 2 — CASE 2: 전방 개방 (2.b → 2.a)
우선순위 3 — IR 측면 긴급 micro_steer
flowchart TD
START([사이클 시작 50 ms]) --> READ["센서 읽기<br/>front_cm, left_cm, right_cm<br/>irL, irR → irL_wall, irR_wall"]
READ --> C1{"CASE 1<br/>F_wall ∨<br/>(irL_wall ∧ irR_wall)?"}
C1 -->|Yes| C1ACT["Motor_Stop<br/>strategy_turn(L,R)<br/>Motor_Forward"]
C1ACT --> C1LED["led_cls = RECV<br/>continue"]
C1LED --> END([osDelay 50 ms])
C1 -->|No| C2{"L_wall ∧ R_wall?<br/>(2.b 협수로)"}
C2 -->|Yes| FWD_B["Motor_Forward<br/>led_cls = FWD"]
C2 -->|No| C2H{"current_heading<br/>== GOAL_HEADING?"}
C2H -->|Yes 2.a| FWD_N["Motor_Forward<br/>측면 트임 무시<br/>led_cls = FWD"]
C2H -->|No| C2L{"좌 개방 ∧<br/>좌회전 1회 후 북?"}
C2L -->|Yes| TL["strategy_turn_left<br/>CORNER_ADVANCE + 90°<br/>led_cls = RSTR"]
C2L -->|No| C2R{"우 개방 ∧<br/>우회전 1회 후 북?"}
C2R -->|Yes| TR["strategy_turn_right<br/>CORNER_ADVANCE + 90°<br/>led_cls = RSTR"]
C2R -->|No| C2TL{"tl, tr 비교<br/>개방 측면"}
C2TL -->|tl < tr, 좌 개방| TL
C2TL -->|tr < tl, 우 개방| TR
C2TL -->|tl == tr > 0, 좌 개방| TL
C2TL -->|그 외| FWD_E["Motor_Forward<br/>led_cls = FWD"]
FWD_B --> IR
FWD_N --> IR
TL --> IR
TR --> IR
FWD_E --> IR
IR{"IR 측면<br/>irL_wall?"}
IR -->|Yes| STEER_L["micro_steer_right<br/>led_cls = STEER"]
IR -->|No| IR2{"irR_wall?"}
IR2 -->|Yes| STEER_R["micro_steer_left<br/>led_cls = STEER"]
IR2 -->|No| KEEP[led_cls 유지]
STEER_L --> LED["update_state_leds"]
STEER_R --> LED
KEEP --> LED
LED --> END
정지 후 strategy_turn(left_cm, right_cm) — CORNER_ADVANCE 없음
| 조건 | 동작 | 규칙 |
|---|---|---|
| 1.c 좌·우 모두 벽 | 180° (좌×2) | 막다른 길 |
| 1.a 우측만 벽 | 좌회전 90° | 개방 측(좌)으로 |
| 1.a 좌측만 벽 | 우회전 90° | 개방 측(우)으로 |
| 1.b 양측 개방 (T자) | turn_toward_north |
열린 쪽 중 북(0)에 가까운 90° 선택, 동률이면 좌회전 |
1.b turn_toward_north 로직
tl = (GOAL_HEADING - heading) & 3 // 좌회전만으로 북까지 필요한 90° 횟수
tr = (heading - GOAL_HEADING) & 3 // 우회전만으로 북까지 필요한 90° 횟수
좌 개방 && (우 막힘 || tl <= tr) → 좌회전
우 개방 → 우회전
둘 다 막힘 → 좌회전 (fallback)
flowchart TD
ST(["strategy_turn(L, R)"]) --> W{"L_wall ∧ R_wall?<br/>(1.c)"}
W -->|Yes| U180["strategy_turn_180<br/>좌회전 × 2"]
W -->|No| RA{"R_wall ∧ ¬L_wall?<br/>(1.a)"}
RA -->|Yes| L90["strategy_turn_left_core<br/>즉시 90°"]
RA -->|No| LB{"L_wall ∧ ¬R_wall?<br/>(1.a)"}
LB -->|Yes| R90["strategy_turn_right_core<br/>즉시 90°"]
LB -->|No| TN["turn_toward_north<br/>(1.b T자)"]
TN --> TN1{"좌 개방 ∧<br/>(¬우 개방 ∨ tl ≤ tr)?"}
TN1 -->|Yes| L90
TN1 -->|No| TN2{"우 개방?"}
TN2 -->|Yes| R90
TN2 -->|No| L90
U180 --> DONE([heading 갱신])
L90 --> DONE
R90 --> DONE
CASE 1 경로는 CORNER_ADVANCE 없음. CASE 2.a의
strategy_turn_left/right만 950 ms 직진 후 회전한다.
current_heading과 무관하게 무조건 직진 — 직사각형 섬 측면 통과.
| 조건 | 동작 |
|---|---|
current_heading == GOAL_HEADING (북향) |
측면 트임 무시, 무조건 직진 (루프 공전 방지) |
| 좌 개방 && 좌회전 1회 후 북향 | strategy_turn_left (950 ms 직진 + 90°) |
| 우 개방 && 우회전 1회 후 북향 | strategy_turn_right |
좌 개방 && tl < tr |
strategy_turn_left |
우 개방 && tr < tl |
strategy_turn_right |
좌 개방 && tl == tr > 0 |
strategy_turn_left (동률 시 좌 우선) |
| 그 외 | 직진 |
tl, tr은 §4와 동일한 북까지 남은 좌/우 90° 횟수이다.
flowchart LR
subgraph defs["정의"]
TL["tl = (GOAL − heading) & 3"]
TR["tr = (heading − GOAL) & 3"]
end
subgraph rules["2.a 비북향 분기"]
R0["heading == 북 → 직진"]
R1["좌 개방 ∧ 좌1회→북 → turn_left"]
R2["우 개방 ∧ 우1회→북 → turn_right"]
R3["좌 개방 ∧ tl < tr → turn_left"]
R4["우 개방 ∧ tr < tl → turn_right"]
R5["좌 개방 ∧ tl==tr>0 → turn_left"]
R6["그 외 → 직진"]
end
defs --> rules
flowchart TB
subgraph Sensors["센서 태스크 (우선순위 2)"]
S["Sense_task\n주기 100 ms\n초음파 outlier 필터"]
I["IR_task\n주기 50 ms\nADC 폴링"]
end
subgraph Control["제어 태스크 (우선순위 1)"]
D["Drive_task\n주기 50 ms\nHeuristic Decision Matrix"]
end
subgraph Actuators["모터 출력"]
F["Motor_Forward / Stop"]
T["strategy_turn"]
TS["strategy_turn_left / right"]
end
S -->|"volatile front_cm, left_cm, right_cm"| D
I -->|"uhADCxLeft, uhADCxRight"| D
D --> F
D --> T
D --> TS
- 단방향 데이터 흐름:
Sense_task·IR_task는 측정만, 상태 전이는Drive_task만 수행 - LED: 4비트 패턴으로
current_heading+ 주행 클래스 표시
| 값 | 의미 |
|---|---|
FWD |
직진 |
STEER |
IR micro_steer |
RECV |
CASE 1 전방 벽 회전 |
RSTR |
CASE 2.a 북 복귀 회전 |
매 Drive_task 사이클 (50 ms):
1. 센서 읽기 (front_cm, left_cm, right_cm, IR)
2. CASE 1? → strategy_turn (즉시 회전)
3. CASE 2.b? → 직진
4. CASE 2.a? → 북향/개방 판단 → 직진 또는 strategy_turn
5. IR 측면 근접? → micro_steer
6. LED 갱신
strategy_turn_* 실행 중(약 950 ms + 1840 ms)에는 Drive_task 루프가 블로킹되므로, 그 동안 CASE 1 재판정은 수행되지 않는다. Sense_task는 계속 거리를 갱신한다.
| 파라미터 | 값 | 설명 |
|---|---|---|
FRONT_THRESH |
12 cm | 전방 벽 (CASE 1) |
WALL_DETECT_DIST_CM |
50 cm | 측면 벽 |
CORNER_ADVANCE_MS |
950 ms | CASE 2.a 회전 전 직진 |
TURN_90_MS |
1840 ms | 90° 회전 시간 |
MICRO_STEER_MS |
70 ms | IR 미세 조향 |
GOAL_HEADING |
0 | 북 = 탈출 방향 |
INITIAL_PWM_PULSE |
17000 | 주행 PWM |
DRIVE_PERIOD_MS |
50 ms | Drive_task 주기 |
SENSE_PERIOD_MS |
100 ms | Sense_task 주기 |
IR_THRESH |
700 | IR 측면 근접 |
- 맵 없이
current_heading만으로 북(Goal) 방향을 유지·복귀한다. - 전방 막힘 → 즉시 측면 상태에 따라 회전 (벽 반대 / T자 시 북 우선).
- 전방 열림 + 북향 → 측면 트임을 무시하고 직진해 섬 둘레 무한 루프를 막는다.
- 전방 열림 + 비북향 → 열린 쪽 중 북에 가까운 방향으로 코너 직진 후 회전.
- 협수로 → 무조건 직진.
- IR → 초음파 보완용 측면 긴급 조향만 수행.
이 규칙 집합은 SRAM 수 바이트 수준의 상태로 격자 미로 + 직사각형 섬 환경에서 Goal(북) 방향 탈출을 목표로 한다.