Skip to content

About

No description, website, or topics provided.

Resources

Stars

2 stars

Watchers

0 watching

Forks

Latest commit

ย 

History

4 Commits

Folders and files

NameName
Last commit message
Last commit date
ย 
ย 
ย 
ย 
ย 
ย 
ย 
ย 
ย 
ย 
ย 
ย 

Repository files navigation

์ดˆ์ŒํŒŒยท์ ์™ธ์„  ์„ผ์„œ ๊ธฐ๋ฐ˜ ์ž์œจ์ฃผํ–‰ ํ”„๋กœ์ ํŠธ

ํœด์ธ์Šค uBrain ๋ชจ๋ธ

์ด๋ฆ„ GitHub
์ •์ค€๋ชจ @i3months
๊น€๋ฏผ์ˆ˜ @new-borned

STM32F429(Cortex-M4) + FreeRTOS ๊ธฐ๋ฐ˜์œผ๋กœ ์ดˆ์ŒํŒŒ ์„ผ์„œ 3๊ฐœ์™€ ์ ์™ธ์„ (IR) ์„ผ์„œ 2๊ฐœ๋ฅผ ์œตํ•ฉํ•˜์—ฌ, ์‚ฌ์ „ ์ „์—ญ ๊ฒฝ๋กœ ์—†์ด ๋ถ์ชฝ(North) ํƒˆ์ถœ๊ตฌ๋กœ ์ˆ˜๋ ดํ•˜๋Š” ๋ฏธ๋กœ ์ž์œจ์ฃผํ–‰ ๋กœ๋ด‡์ž…๋‹ˆ๋‹ค.

์ตœ์ข… ์•„ํ‚คํ…์ฒ˜

ํ•˜๋“œ์ฝ”๋”ฉํ•œ ๋ฏธ๋กœ(๊ณ ์ • ๋ฒฝ) + EKF ์„ผ์„œ ํ“จ์ „ + Greedy Cardinal ๋‚ด๋น„๊ฒŒ์ด์…˜ + ๊ฒฐ์Šน ์ง์ „ ๋™์  ์žฅ์• ๋ฌผ ํ™•๋ฅ  ๋ชจ๋ธ๋ง


1. ๋ฌธ์ œ ์ •์˜์™€ ์„ค๊ณ„ ์ฒ ํ•™

๋ฏธ๋กœ๋Š” ์•ฝ 5 m ร— 10 m ์ง์‚ฌ๊ฐํ˜• ๊ณต๊ฐ„์ด๋ฉฐ, ์ถœ๋ฐœ์„ ์—์„œ ์ถœ๋ฐœํ•ด ๋ถ์ชฝ ํƒˆ์ถœ๊ตฌ์— ์ถฉ๋Œ ์—†์ด ๊ฐ€์žฅ ๋น ๋ฅด๊ฒŒ ๋„๋‹ฌํ•˜๋Š” ๊ฒƒ์ด ๋ชฉํ‘œ์ž…๋‹ˆ๋‹ค. ์žฅ์• ๋ฌผ์€ uBrain ํฌ์žฅ ๋ฐ•์Šค๋กœ ๊ตฌ์„ฑ๋œ ๊ณ ์ •ํ˜• ์žฅ์• ๋ฌผ๊ณผ ์ด๋™ํ˜• uBrain 1๋Œ€(๋™์  ์žฅ์• ๋ฌผ)๋กœ ๊ตฌ๋ถ„๋ฉ๋‹ˆ๋‹ค.

ํ‰๊ฐ€ ๋ฏธ๋กœ ๋งต

๋‹น์ผ ์˜ค์ „ ๊ณต๊ฐœ๋œ ๋ฏธ๋กœ ๋งต. ๋ชจ๋“  ๋ฒฝ์ด ์ง๊ต(orthogonal)์ด๋ฉฐ ํƒˆ์ถœ๊ตฌ๋Š” ๋ถ์ชฝ์— ์œ„์น˜ํ•œ๋‹ค.

ํ•ต์‹ฌ ๊ฐ€์ •์€ ๋‹ค์Œ๊ณผ ๊ฐ™์Šต๋‹ˆ๋‹ค.

  • ํƒˆ์ถœ๊ตฌ๋Š” ๋ฐ˜๋“œ์‹œ ๋ถ์ชฝ์— ์œ„์น˜ํ•œ๋‹ค.
  • ๋ฏธ๋กœ๋Š” 'ใ…'ํ˜•(๊ณ ๋ฆฝ ๋ฃจํ”„) ๋งต์ด ์•„๋‹ˆ๋‹ค.
  • ํƒˆ์ถœ๊ตฌ ๋„๋‹ฌ์— ํ•„์š”ํ•œ ๋ฐฉํ–ฅ ์ „ํ™˜ ํšŸ์ˆ˜๊ฐ€ ์ •ํ•ด์ ธ ์žˆ๋‹ค.

์ด ๊ฐ•ํ•œ ๊ฐ€์ • ์œ„์—์„œ ์™„์ „์„ฑ์ด ๋ณด์žฅ๋˜๋ฏ€๋กœ, ๋ฌธ์ œ๋ฅผ "๋””๋ฐ”์ด์Šค์— ๋ฐฉํ–ฅ ์ •๋ณด๋ฅผ ์ œ๊ณตํ•˜์—ฌ ๋ถ์ชฝ ํƒˆ์ถœ๊ตฌ๋กœ ์•ˆ์ •์ ์œผ๋กœ ์ˆ˜๋ ด์‹œํ‚ค๋Š” ๊ทธ๋ฆฌ๋”” ๋ฌธ์ œ"๋กœ ์žฌ์ •์˜ํ–ˆ์Šต๋‹ˆ๋‹ค.

ํ’€์Šคํƒ ๋‚ด๋น„๊ฒŒ์ด์…˜์„ ๊ธฐ๊ฐํ•œ ์ด์œ 

์ดˆ๊ธฐ์—๋Š” Occupancy Grid ๊ธฐ๋ฐ˜ mapping โ†’ ๋ฒฝ ์ถ”์ถœ โ†’ A*/D* ์ „์—ญ ๊ฒฝ๋กœ๊ณ„ํš์„ ํ†ตํ•ฉํ•œ ํ’€์Šคํƒ ์ž์œจ์ฃผํ–‰ ํŒŒ์ดํ”„๋ผ์ธ์„ ๊ตฌํ˜„ยท์‹ค์ธกํ–ˆ์Šต๋‹ˆ๋‹ค. ๊ทธ๋Ÿฌ๋‚˜ ์‹ค์ธก ๊ฒฐ๊ณผ,

  1. ์šฐ๋ฆฌ ํŒ€ ๋กœ๋ด‡์˜ ์ขŒยท์šฐ ๋ชจํ„ฐ ์ถœ๋ ฅ ๋น„๋Œ€์นญ์ด ํƒ€ ํŒ€ ๋Œ€๋น„ ์ปธ๊ณ ,
  2. ๋ณด๋“œ์— IMU ์„ผ์„œ๊ฐ€ ๋ถ€์žฌํ•˜์—ฌ ๋ถ€์ •ํ™•ํ•œ ์ƒํƒœ ์ถ”์ •์ด ๋ถˆ๊ฐ€ํ”ผํ–ˆ์Šต๋‹ˆ๋‹ค.

์ด๋กœ ์ธํ•ด ์ž์„ธ ์ถ”์ •์˜ ๋ˆ„์  ์˜ค์ฐจ๊ฐ€ ์ปค์ ธ Occupancy Grid ๋งคํ•‘ ์‹œ ์˜คํžˆ๋ ค ์ฃผํ–‰ ์•ˆ์ •์„ฑ์ด ๋–จ์–ด์กŒ์Šต๋‹ˆ๋‹ค. ๋Œ€์‹  ๋‹น์ผ ์˜ค์ „์— ๋งต์ด ๊ณต๊ฐœ๋œ๋‹ค๋Š” ์ ์— ์ฐฉ์•ˆํ•˜์—ฌ, 2D ๋งต์„ ํ•˜๋“œ์ฝ”๋”ฉํ•˜๊ณ  ๋™์  ์žฅ์• ๋ฌผ ๊ตฌ๊ฐ„๋งŒ y์ขŒํ‘œ ์ž„๊ณ„์ ์œผ๋กœ ํ™•๋ฅ  ๋ชจ๋ธ๋งํ•˜๋Š” ๋ฐฉํ–ฅ์œผ๋กœ ์„ค๊ณ„๋ฅผ ์ถ•์•ฝํ–ˆ์Šต๋‹ˆ๋‹ค.

์ด๋ก ์  ์ตœ์ ์„ฑ๋ณด๋‹ค, ํ•˜๋“œ์›จ์–ด ์ œ์•ฝ ํ•˜์—์„œ ๊ฒฌ๊ณ ์„ฑ์„ ๋ณด์žฅํ•˜๋Š” ์•Œ๊ณ ๋ฆฌ์ฆ˜์ด ์ž„๋ฒ ๋””๋“œ ์‹œ์Šคํ…œ์—์„œ ๋” ์ค‘์š”ํ•˜๋‹ค๊ณ  ํŒ๋‹จํ–ˆ์Šต๋‹ˆ๋‹ค.


2. Finite State Machine ๊ธฐ๋ฐ˜ ๊ตฌ์กฐ

๋ฏธ๋กœ์—๋Š” ๊ณก์„  ๊ตฌ์กฐ๊ฐ€ ์ „ํ˜€ ์—†๊ณ  ๋ชจ๋“  ๊ตฌ์„ฑ์ด ์ง๊ต(orthogonal)์ž…๋‹ˆ๋‹ค. ๋”ฐ๋ผ์„œ ๋กœ๋ด‡์˜ ์ฃผํ–‰์„ ์ƒํ•˜์ขŒ์šฐ + 90ยฐ ์ฝ”๋„ˆ ํšŒ์ „์œผ๋กœ ๋‹จ์ˆœํ™”ํ•  ์ˆ˜ ์žˆ๊ณ , ๋ณต์žกํ•œ ๊ฒฝ๋กœ ์ถ”์ข… ๋กœ์ง ๋Œ€์‹  FSM์œผ๋กœ ํ™˜์›ํ–ˆ์Šต๋‹ˆ๋‹ค.

stateDiagram-v2
    [*] --> INIT
    INIT --> DRIVE: dF > 0
    DRIVE --> EMERGENCY: isEmergency()
    EMERGENCY --> DRIVE: rotate done
Loading
์ƒํƒœ ์—ญํ• 
INIT ์ „์› ์ธ๊ฐ€ ํ›„ ์œ ํšจ ์ธก๊ฑฐ(dF > 0) ๋Œ€๊ธฐ
DRIVE ์ฝ”๋„ˆ ํŒ์ • ยท VEER ยท ์ง์ง„ ๋“ฑ ์ •์ƒ ์ฃผํ–‰
EMERGENCY ์ „๋ฉด ์ถฉ๋Œ ์ž„๋ฐ•(์ดˆ์ŒํŒŒ/IR) ์‹œ ํšŒํ”ผ ํšŒ์ „ ํ›„ ๋ณต๊ท€

์ด FSM์ด ์ œ๋Œ€๋กœ ์ž‘๋™ํ•˜๋ ค๋ฉด ์ง์ง„ ์ˆ˜๋ ด์„ฑ๊ณผ 90ยฐ ํšŒ์ „ ์ •๋ฐ€๋„๊ฐ€ ๋งค์šฐ ์ค‘์š”ํ•ฉ๋‹ˆ๋‹ค. ํšŒ์ „ยท์ง์ง„์ด ์กฐ๊ธˆ๋งŒ ์–ด๊ธ‹๋‚˜๋„ ์˜ค์ฐจ๊ฐ€ ๋ˆ„์ ๋˜์–ด ๊ฒฝ๋กœ๊ฐ€ ๋ฌด๋„ˆ์ง€๋ฏ€๋กœ, ํšŒ์ „ยท์ง์ง„ ๋ณด์ •์„ ์ง‘์ค‘์ ์œผ๋กœ ๊ฒ€ํ† ํ–ˆ์Šต๋‹ˆ๋‹ค.


3. ํ•ต์‹ฌ ์ฃผํ–‰ ์•Œ๊ณ ๋ฆฌ์ฆ˜

3.1 Greedy ๋ถํ–ฅ ๋‚ด๋น„๊ฒŒ์ด์…˜ (Cardinal FSM)

Non-wall-following ๋ฐฉ์‹์œผ๋กœ, ์ธก๋ฒฝ ๊ฑฐ๋ฆฌ์˜ ๋ณ€ํ™”๋Ÿ‰ ฮ”d๋ฅผ ๊ธฐ์ค€์œผ๋กœ ์ฝ”๋„ˆ ๋ฐ ๊ต์ฐจ๋กœ๋ฅผ ์ด์‚ฐ ๊ฐ์ง€ํ•˜์—ฌ ํšŒ์ „ยทํ†ต๊ณผํ•ฉ๋‹ˆ๋‹ค. ์˜์‚ฌ๊ฒฐ์ •์€ ํ•ญ์ƒ ๋ถ์ชฝ(๋„์ฐฉ์ง€)์„ ์„ ํ˜ธํ•˜๋„๋ก ์„ค์ •ํ–ˆ์Šต๋‹ˆ๋‹ค.

  • ์ง„ํ–‰ ๋ฐฉํ–ฅ์— ์šฐ์„ ์ˆœ์œ„๋ฅผ ๋ถ€์—ฌํ•œ๋‹ค: ๋ถ:0, ์„œยท๋™:1, ๋‚จ:2.
  • 90ยฐ ํšŒ์ „(EMERGENCY ํฌํ•จ) ์‹œ ์ด ์šฐ์„ ์ˆœ์œ„๋กœ ํšŒํ”ผ ๋ฐฉํ–ฅ์„ ๊ฒฐ์ •ํ•œ๋‹ค (priority-prefer rotation).
  • ์šฐ์„ ์ˆœ์œ„๊ฐ€ ๋™์ผํ•˜๋ฉด ์ขŒยท์šฐ ์„ผ์„œ ๊ฐ’์„ ๋น„๊ตํ•ด ๋” ๋„“์€ ์ชฝ์œผ๋กœ ์ง„ํ–‰ํ•œ๋‹ค.
  • ๊ต์ฐจ๋กœ ํŒ๋ณ„ ์‹œ ๋ถํ–ฅ ์ง„ํ–‰์ด ๊ฐ€๋Šฅํ•˜๋ฉด ๋ถ์ชฝ์„ ๊ฐ•์ œ ์„ ํƒํ•œ๋‹ค.

์ด๋กœ์จ ๋ถํ–ฅ ๋‹จ์กฐ ๊ตฌ์กฐ ๊ฐ€์ • ํ•˜์—์„œ ์‚ฌ์ „ ์ „์—ญ ๊ฒฝ๋กœ ์—†์ด๋„ ์ถœ๊ตฌ ๋„๋‹ฌ์ด ๋ณด์žฅ๋ฉ๋‹ˆ๋‹ค. ์ ˆ๋Œ€ ์ขŒํ‘œ๊ฐ€ ์•„๋‹ˆ๋ผ ๊ตญ์†Œ ์ธก๋ฒฝ ๊ฑฐ๋ฆฌ์™€ cardinal heading(์ƒ๋Œ€ ๋ฐฉํ–ฅ)์œผ๋กœ ๋ถ„๊ธฐ๋ฅผ ํŒ๋‹จํ•˜๋ฏ€๋กœ, ์œ„์น˜ ์ถ”์ • ๋ˆ„์  ์˜ค์ฐจ(drift)์— ๊ฐ•ํ•ฉ๋‹ˆ๋‹ค.

3.2 ๋™์  ์žฅ์• ๋ฌผ ํ™•๋ฅ  ๋ชจ๋ธ๋ง

์ด๋™ํ˜• uBrain 1๋Œ€๋Š” ์œ„์น˜๊ฐ€ ๋ณ€ํ•˜๋ฏ€๋กœ ํ•˜๋“œ์ฝ”๋”ฉ ๋งต์œผ๋กœ ๋‹ค๋ฃฐ ์ˆ˜ ์—†๊ณ , IMUยท์ •๋ฐ€ ์„ผ์„œ๊ฐ€ ์—†๋Š” ๋ณด๋“œ์—์„œ ์ด๋™์ฒด๋ฅผ ์ •๋ฐ€ ์ถ”์ ํ•˜๋Š” ๊ฒƒ์€ ์˜คํžˆ๋ ค ๋ถˆ์•ˆ์ •ํ•ฉ๋‹ˆ๋‹ค. ๋”ฐ๋ผ์„œ ์ •๋ฐ€ ์ถ”์  ๋Œ€์‹  ๊ด€์ธก-ํ›„-๊ฒฐ๋‹จ ํœด๋ฆฌ์Šคํ‹ฑ์„ ์ฑ„ํƒํ–ˆ์Šต๋‹ˆ๋‹ค.

  • ๊ฒฐ์Šน์  ์ง์ „ ๋งˆ์ง€๋ง‰ ๊ตฌ๊ฐ„(์‹ค์Šต ์ฑ„์  ๊ธฐ์ค€ 4๊ตฌ๊ฐ„)์— ์ง„์ž…ํ•˜๋ฉด ๋™์  ํšŒํ”ผ ๋ชจ๋“œ๋ฅผ ํ™œ์„ฑํ™”ํ•œ๋‹ค.
  • ์„ผ์„œ ๋น” ์ข…์ ์— ์ ์œ  ํ™•๋ฅ ์„ ์‹œ๊ฐ„ ๊ฐ€์ค‘์œผ๋กœ ๋ˆ„์ ยท๊ฐ์‡ ์‹œ์ผœ ์ •์  ๋ฒฝ๊ณผ ์ด๋™ ๋ฌผ์ฒด๋ฅผ ๋ถ„๋ฆฌํ•œ๋‹ค.
  • ๋กœ๋ด‡์€ NํšŒ ๋ถํ–ฅ ์‚ฌ์ดํด์„ ์ˆ˜ํ–‰ํ•˜๋ฉฐ ์ด๋™ ์žฅ์• ๋ฌผ์˜ ํ†ต๊ณผ ๋ฆฌ๋“ฌยท๊ฐ„๊ฒฉ์„ ๊ธฐ๋กํ•œ๋‹ค.
  • ์ „๋ฉด์ด ์ž„๊ณ„ ๊ฑฐ๋ฆฌ ์ด์ƒ์œผ๋กœ ์—ด๋ฆฐ ํ†ต๊ณผ ์ฐฝ(window)์ด ํ™•์ธ๋˜๋ฉด ์ˆœํ•ญ ์†๋„๋ฅผ ์ผ์‹œ์ ์œผ๋กœ ๋†’์—ฌ(V_cruise โ†’ V_dash) ์žฅ์• ๋ฌผ์ด ๋˜๋Œ์•„์˜ค๊ธฐ ์ „์— ๋น ๋ฅด๊ฒŒ ํ†ต๊ณผํ•œ๋‹ค.
if ( front_clear โ‰ฅ D_pass  AND  cycle_count โ‰ฅ N )  โ‡’  v โ† V_dash

์„ค๊ณ„ ๊ทผ๊ฑฐ: ์ •ํ™•ํ•œ ์œ„์น˜ยท์†๋„ ์ถ”์ •์—” ๊ณ ํ’ˆ์งˆ ์„ผ์„œยทํ•„ํ„ฐ๊ฐ€ ํ•„์š”ํ•˜์ง€๋งŒ, "ํ†ต๊ณผ ๊ฐ€๋Šฅํ•œ ์‹œ์ ๋งŒ ํฌ์ฐฉ"ํ•˜๋Š” ๋ฌธ์ œ๋กœ ํ™˜์›ํ•˜๋ฉด ์ €ํ’ˆ์งˆ ์„ผ์„œ๋กœ๋„ ์ถฉ๋ถ„ํ•ฉ๋‹ˆ๋‹ค. ํ™•๋ฅ  ๊ฐ์‡  ๋•๋ถ„์— ํ•œ ๋ฒˆ์˜ ์ˆœ๊ฐ„์  ๊ฐ์ง€๊ฐ€ ํ†ต๋กœ๋ฅผ ์˜๊ตฌ ์ฐจ๋‹จํ•˜์ง€ ์•Š์œผ๋ฉฐ, NํšŒ ๊ด€์ธก์€ ์šฐ์—ฐํ•œ ์˜ค๊ฐ์ง€์— ์˜ํ•œ ๋ฌด๋ชจํ•œ ๋Œ์ง„์„ ๋ง‰๋Š” ์•ˆ์ „์žฅ์น˜๋กœ ์ž‘๋™ํ•ฉ๋‹ˆ๋‹ค.

3.3 ๊ต์ฐจ๋กœ ํŒ์ • + ์ •์ง€์‹œ๊ฐ„ ๋ณด์ • ํ•จ์ˆ˜

์ธก๋ฒฝ ๊ฐœ๊ตฌ๋Š” ์ €์„ฑ๋Šฅ ์ธก๋ฉด ์ดˆ์ŒํŒŒ ์„ผ์„œ๋กœ ํŒ์ •ํ•˜๋ฏ€๋กœ, ๋‹จ๋ฐœ ์ŠคํŒŒ์ดํฌ๊ฐ€ ์•„๋‹Œ ์ผ์ • ์‹œ๊ฐ„ ์ง€์†์ ์œผ๋กœ ์—ด๋ ค ์žˆ๋Š”์ง€๋ฅผ ํ™•์ธํ•˜๋„๋ก ์ฆ๊ฑฐ๋ฅผ ๋ˆ„์ ํ•˜๊ณ  ์‹œ๊ฐ„์ด ์ง€๋‚˜๋ฉด leak ํ•˜๋Š” ๊ตฌ์กฐ๋กœ ๋งŒ๋“ค์—ˆ์Šต๋‹ˆ๋‹ค.

$$w_{open}(t) = \alpha \cdot w_{open}(t-1) + (1-\alpha)\cdot \mathbf{1}[\Delta d(t) > \tau_\Delta]$$

  • $\alpha \in (0,1)$: ๋ˆ„์ˆ˜ ๊ณ„์ˆ˜(ํด์ˆ˜๋ก ๊ณผ๊ฑฐ ์ฆ๊ฑฐ ์˜ค๋ž˜ ์œ ์ง€). ๋ณดํ†ต 0.7 ~ 0.9.
  • $\mathbf{1}[\cdot]$: ์ด๋ฒˆ ํ‹ฑ์— ๊ฐœ๊ตฌ ์กฐ๊ฑด ๋งŒ์กฑ์ด๋ฉด 1, ์•„๋‹ˆ๋ฉด 0.
  • ํŒ์ •: $w_{open}(t) > \theta_{trigger}$ ์ด๋ฉด ๊ฐœ๊ตฌ ํ™•์ •.

์—ฐ์†์ ์œผ๋กœ ์—ฌ๋Ÿฌ tick์ด ์—ด๋ ค์•ผ ์ž„๊ณ„๋ฅผ ๋„˜์œผ๋ฏ€๋กœ ์˜คํŒ์ด ์–ต์ œ๋˜๊ณ , ํ•œ ๋ฒˆ ํŒ๋‹จํ•œ ๊ต์ฐจ๋กœ๋ฅผ ์ฆ‰์‹œ ์žฌํŠธ๋ฆฌ๊ฑฐํ•˜์ง€ ์•Š๋Š” debounce ํšจ๊ณผ๋„ ๋ถ€์ˆ˜์ ์œผ๋กœ ์–ป์Šต๋‹ˆ๋‹ค.

์ •์ง€์‹œ๊ฐ„ ๋ณด์ •: ๊ฐœ๊ตฌ ํŒ์ • ์ฆ‰์‹œ ํšŒ์ „ํ•˜๋ฉด ๊ต์ฐจ๋กœ ์ค‘์•™์ด ์•„๋‹Œ ์ง„์ž…๋ถ€์—์„œ ๋Œ์•„ ๋ฒฝ์— ์ถฉ๋Œํ•ฉ๋‹ˆ๋‹ค. ์ง„ํ–‰ ์‹œ๊ฐ„์„ ๋‹จ์ผ ์ƒ์ˆ˜๋กœ ํ•˜๋“œ์ฝ”๋”ฉํ•˜๋ฉด ์†๋„์— ๋”ฐ๋ผ ํšŒ์ „ ์œ„์น˜๊ฐ€ ํ”๋“ค๋ฆฌ๋ฏ€๋กœ, ์ง์ „ motor_stop() ์ดํ›„ ๊ฒฝ๊ณผ ์‹œ๊ฐ„ ฮ”t๋ฅผ ํ˜„์žฌ ์†๋„์˜ ๋Œ€๋ฆฌ ๋ณ€์ˆ˜๋กœ ์‚ผ์•„ ์ง„ํ–‰ ์‹œ๊ฐ„์„ ์ •์˜ํ–ˆ์Šต๋‹ˆ๋‹ค.

$$t_{advance}(\Delta t) = \max(800,; 900 - 50,\Delta t), \quad \Delta t = t_{now} - t_{last\_stop}$$

์ด๋กœ์จ ๊ต์ฐจ๋กœ ํŒ๋‹จ์˜ ์•ˆ์ •์„ฑ๊ณผ ๋ฐ˜์‘์„ฑ์„ ๋™์‹œ์— ํ™•๋ณดํ•ฉ๋‹ˆ๋‹ค.


4. ์˜ค๋ฅ˜ ๋ณด์ • ์ „๋žต โ€” ๊ทนํ•œ์˜ Calibration

๊ฐ€์žฅ ๋งŽ์€ ์‹œ๊ฐ„์„ ํˆฌ์žํ•œ ์˜์—ญ์ž…๋‹ˆ๋‹ค. ์ด์ƒ์  ์•Œ๊ณ ๋ฆฌ์ฆ˜์ด ํ˜„์‹ค ํ•˜๋“œ์›จ์–ด์—์„œ ๊ทธ๋Œ€๋กœ ๋™์ž‘ํ•˜์ง€ ์•Š์œผ๋ฏ€๋กœ, ๋ฌผ๋ฆฌ ์˜ค์ฐจ๋ฅผ ์ถ”์ ยท๋ณด์ •ํ•˜๋Š” ์ผ์ด ๋งค์šฐ ์ค‘์š”ํ–ˆ์Šต๋‹ˆ๋‹ค.

4.1 ๋ฌผ๋ฆฌ ๊ณ„์ธต ๋””๋ฒ„๊น…

๋ฌธ์ œ ์›์ธ ๋Œ€์‘
๋ฐฐํ„ฐ๋ฆฌ ์˜์กด์„ฑ ์ถฉ์ „ ์ƒํƒœ์— ๋”ฐ๋ผ ์ขŒยท์šฐ ๋ชจํ„ฐ ๊ฑฐ๋™์ด ๋‹ฌ๋ผ์ ธ ์ง์ง„์ด ํœ˜๊ณ  90ยฐ ํšŒ์ „์ด ์–ด๊ธ‹๋‚จ (TICKS_PER_CM, pivot substep์ด ๋ฐฐํ„ฐ๋ฆฌ ์ƒํƒœ์— ๋”ฐ๋ผ ๋ณ€ํ•จ) ํ•ญ์ƒ 100% ์ถฉ์ „ ์ƒํƒœ๋ฅผ ๊ฐ€์ •ํ•ด Calibration, ์‹ค์ „์—์„œ๋„ ํ’€์ถฉ์ „ ์œ ์ง€. ํšŒ์ „์„ ์ธ์ฝ”๋” ํ๋ฃจํ”„๋กœ ๊ตฌํ˜„ํ•ด ์ž๊ฐ€๋ณด์ •
๋ฐ”ํ€ด์— ์—‰ํ‚จ ๋จธ๋ฆฌ์นด๋ฝ ์šฐ์ธก ๋ฐ”ํ€ด ์ถ•์— ๋จธ๋ฆฌ์นด๋ฝ์ด ์—‰์ผœ ํšŒ์ „์„ ๋ฌผ๋ฆฌ์ ์œผ๋กœ ๋ฐฉํ•ด โ†’ ์ขŒยท์šฐ ์ธ์ฝ”๋” ๊ฐ’ ๋น„๋Œ€์นญ ํ•˜๋“œ์›จ์–ด ์ง์ ‘ ์ ๊ฒ€ ํ›„ ์ œ๊ฑฐํ•˜์—ฌ ์ขŒ์šฐ ๋Œ€์นญ์„ฑ ํšŒ๋ณต
์ขŒยท์šฐ ๋ชจํ„ฐ ๋น„๋Œ€์นญ ๋ฐ›์€ ๋กœ๋ด‡์˜ ๊ตฌ์กฐ์  ์ถœ๋ ฅ ํŽธ์ฐจ ์ง์ง„ยท๋“œ๋ฆฌํ”„ํŠธ์šฉ V_TRIM_L, ํšŒ์ „์šฉ ์ขŒ/์šฐ ๋ถ„๋ฆฌ substep, ํ๋ฃจํ”„ pivot์œผ๋กœ ๋น„๋Œ€์นญ ์–ต์ œ

4.2 (5-a) Wheel Odometry โ€” RK2(midpoint) ์ ๋ถ„

EXTI ์ธ์ฝ”๋” ISR์ด quadrature edge๋งˆ๋‹ค ์ขŒยท์šฐ ํœ  ์นด์šดํ„ฐ๋ฅผ ์ฆ๊ฐํ•˜๋ฉฐ, uint16 wrap์„ signed delta ์บ์ŠคํŒ…์œผ๋กœ ๋ณด์ •ํ•ด ํ•œ tick์— $|\Delta| < 2^{15}$ ๋ฒ”์œ„๊นŒ์ง€ ์•ˆ์ „ํ•ฉ๋‹ˆ๋‹ค.

์ขŒยท์šฐ ํœ  ๋ณ€์œ„๋กœ๋ถ€ํ„ฐ ๋ณธ์ฒด ๋ณ€์œ„ ฮ”s์™€ ํšŒ์ „ ฮ”ฮธ๋ฅผ ๊ณ„์‚ฐํ•ฉ๋‹ˆ๋‹ค.

$$\Delta s_R = \Delta\_R / K, \quad \Delta s_L = \Delta\_L / K$$ $$\Delta s = (\Delta s_R + \Delta s_L)/2, \quad \Delta\theta = (\Delta s_R - \Delta s_L)/b$$

ํ•œ tick ๋™์•ˆ ํ—ค๋”ฉ์ด ์„ ํ˜• ๋ณ€ํ™”ํ•œ๋‹ค๊ณ  ๋ณด๊ณ  ํ‰๊ท  ํ—ค๋”ฉ์„ ์‚ฌ์šฉํ•œ ์ค‘์ (RK2) ์ ๋ถ„์œผ๋กœ 1์ฐจ Euler๋ณด๋‹ค ํ•œ ์ฐจ์ˆ˜ ๋†’์€ ์ •ํ™•๋„๋ฅผ ํ™•๋ณดํ•ฉ๋‹ˆ๋‹ค.

$$\alpha = \theta + \Delta\theta/2$$ $$x \leftarrow x + \Delta s\cos\alpha, \quad y \leftarrow y + \Delta s\sin\alpha, \quad \theta \leftarrow \theta + \Delta\theta$$

  • Calibration: TICKS_PER_CM = 50.8 (์ง์ง„ 3 s ร— 3ํšŒ, ๋ถ„์‚ฐ < 1 %), WHEEL_BASE_CM = 22.0 (์บ˜๋ฆฌํผ ์‹ค์ธก ํ›„ ์ •์‚ฌ๊ฐ ํ…Œ์ŠคํŠธ๋กœ ์žฌ๋ณด์ •). 20 ms ์ฃผ๊ธฐ๋กœ SensorTask๊ฐ€ ํ˜ธ์ถœ.

4.3 (5-b) EKF ์„ผ์„œ ํ“จ์ „ (Predict + Update)

์ƒํƒœ๋ฒกํ„ฐ $x = [x, y, \theta]^T$, ๊ณต๋ถ„์‚ฐ $P \in \mathbb{R}^{3\times3}$. XยทY๋Š” ๋ชจํ„ฐ ์ธ์ฝ”๋” ์ ๋ถ„, ฮธ๋Š” ์„ผ์„œ๊ฐ’ ๊ธฐ๋ฐ˜์œผ๋กœ ๋ณด์ •๋˜๋Š” ๊ตฌ์กฐ๋กœ, ์ธ์ฝ”๋”์˜ ๋ˆ„์  ๋“œ๋ฆฌํ”„ํŠธ๋ฅผ ์ดˆ์ŒํŒŒ ์„ผ์„œ๊ฐ€ ์žก์•„์ค๋‹ˆ๋‹ค.

Predict โ€” ๋น„์„ ํ˜• ์šด๋™๋ชจ๋ธ์„ ์ „ํŒŒํ•˜๊ณ , ์ƒํƒœ Jacobian Fยท์ œ์–ด Jacobian V๋กœ ๊ณต๋ถ„์‚ฐ์„ propagate. ๋…ธ์ด์ฆˆ๋Š” ํœ  ์Šฌ๋ฆฝ์„ ๋ณ€์œ„ ๋น„๋ก€๋กœ ๋ชจ๋ธ๋ง ํ›„ V๋กœ ํˆฌ์˜ํ•˜์—ฌ (x,y,ฮธ) ์ƒ๊ด€๊ด€๊ณ„๋ฅผ ๋ณด์กดํ•ฉ๋‹ˆ๋‹ค.

$$P^- = FPF^\top + Q, \quad Q = VQ_{uu}V^\top, \quad \sigma_i = \max(k_{slip}|\Delta s_i|, \sigma_{floor})$$

ํ•ด์„์  Rayโ€“Segment ๊ต์ฐจ โ€” ์ธก์ •๊ฐ’์€ ์ดˆ์ŒํŒŒ 3๊ฐœ์˜ ๊ฑฐ๋ฆฌ $(z_F, z_L, z_R)$. ๊ฒฉ์ž ray-cast ๋Œ€์‹  ํ•˜๋“œ์ฝ”๋”ฉ๋œ ๋ฒฝ ์„ ๋ถ„ ๋ฆฌ์ŠคํŠธ์™€ ๋น”์˜ ํ•ด์„์  ๊ต์ฐจ๋กœ ์ธก์ • ์•ผ์ฝ”๋น„์•ˆ H๋ฅผ ๋‹ซํžŒ ํ˜•ํƒœ๋กœ ๊ณ„์‚ฐํ•ฉ๋‹ˆ๋‹ค.

$$\hat{z}_i = \frac{d_j - n_j \cdot o_i}{n_j \cdot \hat{b}_i}$$

์„ผ์„œ๋ณ„ ์ˆœ์ฐจ 1D ๊ฐฑ์‹  โ€” R์ด ๋Œ€๊ฐ์ด๋ฏ€๋กœ 3์„ผ์„œ๋ฅผ ๋™์‹œ์— ์ฒ˜๋ฆฌํ•˜์ง€ ์•Š๊ณ  ์ˆœ์ฐจ 1D Kalman update๋กœ ์ฒ˜๋ฆฌํ•˜์—ฌ ํ–‰๋ ฌ ์—ญ์—ฐ์‚ฐ์„ ํšŒํ”ผํ•ฉ๋‹ˆ๋‹ค.

$$y = z_i - \hat{z}_i, \quad S = H_iPH_i^\top + \sigma_{z,i}^2, \quad K = PH_i^\top/S$$

์ด๋…ธ๋ฒ ์ด์…˜ ๊ฒŒ์ดํŒ… + Joseph form โ€” ์ž˜๋ชป ์ •์˜๋œ ๋ฒฝ์œผ๋กœ ์ธํ•œ EKF ๋ฐœ์‚ฐ์„ ๋ง‰๊ธฐ ์œ„ํ•ด $|y| &gt; 3\sqrt{S}$ ์ธ ์„ผ์„œ ํ–‰์„ ์ œ์™ธํ•˜๊ณ , ๊ณต๋ถ„์‚ฐ์€ Joseph form์œผ๋กœ ๊ฐฑ์‹ ํ•˜์—ฌ ๋Œ€์นญ ์–‘์ •์น˜(PD)๋ฅผ ๋ณด์กดํ•ฉ๋‹ˆ๋‹ค.

$$|y| > 3\sqrt{S} \Rightarrow \text{reject}; \quad P^+ = (I - KH_i)P(I - KH_i)^\top + KRK^\top$$

4.4 (5-c) arctan ๊ธฐ๋ฐ˜ ํ—ค๋”ฉ ์ถ”๋ก 

์ง์ง„ยท90ยฐ ํšŒ์ „ ๊ฐ€์ •์˜ ์„ฑํŒจ๊ฐ€ ํ—ค๋”ฉ ์ •ํ™•๋„์— ๋‹ฌ๋ ค ์žˆ์œผ๋ฏ€๋กœ, ์ธ์ฝ”๋” ์ ๋ถ„์œผ๋กœ ์–ป์€ ์ ˆ๋Œ€ ํ—ค๋”ฉ ฮธ์— ๋”ํ•ด ์ธก๋ฒฝ ํ˜•์ƒ์œผ๋กœ๋ถ€ํ„ฐ ์‚ฐ์ถœํ•œ ํ—ค๋”ฉ์œผ๋กœ ๋ณด์ •ํ•ฉ๋‹ˆ๋‹ค. ์ธก๋ฉด ์ดˆ์ŒํŒŒ๊ฐ€ ๋™์ผ ๋ฒฝ๊นŒ์ง€ ์ธก์ •ํ•œ ์•ž๋’ค ๊ฑฐ๋ฆฌ์ฐจ ฮ”d์™€ ๋‘ ์ธก์ •์ ์˜ ์ข…๋ฐฉํ–ฅ ๋ฒ ์ด์Šค๋ผ์ธ L๋กœ๋ถ€ํ„ฐ,

$$\theta_{rel} = \arctan(\Delta d / L)$$

์‚ฌ์ „ ๊ณต๊ฐœ๋œ ํ•˜๋“œ์ฝ”๋”ฉ ๋งต์—์„œ ํ˜„์žฌ ์ถ”์ข… ์ค‘์ธ ๋ฒฝ์˜ ์ ˆ๋Œ€ ๋ฐฉํ–ฅ $\theta_{ref}^{wall}$(cardinal ์ด์‚ฐ๊ฐ’)์„ ์•Œ ์ˆ˜ ์žˆ์œผ๋ฏ€๋กœ, ๋กœ๋ด‡์˜ ์ ˆ๋Œ€ ํ—ค๋”ฉ ๊ด€์ธก๊ฐ’์„ $z_\theta = \theta_{ref}^{wall} + \theta_{rel}$ ๋กœ ํ™˜์‚ฐํ•ด EKF ์ธก์ • ๊ฐฑ์‹ ์— ๋ฐ˜์˜ํ•ฉ๋‹ˆ๋‹ค. ์ด๋ฅผ ํ†ตํ•ด IMU ๋ถ€์žฌ๋กœ ์ธํ•œ ํ—ค๋”ฉ ๋“œ๋ฆฌํ”„ํŠธ๋ฅผ ์ ˆ๋Œ€ ๊ธฐ์ค€์— ๊ณ ์ •(anchor)ํ•ฉ๋‹ˆ๋‹ค. ๋‹จ, ๊ต์ฐจ๋กœ์—์„œ๋Š” ํ‰ํ–‰ ๋ฒฝ ๊ฐ€์ •์ด ๊นจ์ ธ $\theta_{rel}$์ด ๋ฐœ์‚ฐํ•˜๋ฏ€๋กœ ์ธก๋ฒฝ ๋ณ€ํ™”๋Ÿ‰์ด ์ž„๊ณ„๋ฅผ ์ดˆ๊ณผํ•˜๋Š” ๊ฐœ๊ตฌ ์ด๋ฒคํŠธ ์‹œ ํ•ด๋‹น ๊ด€์ธก์„ ์ผ์‹œ ์ค‘๋‹จํ•ฉ๋‹ˆ๋‹ค.


5. ์„ผ์„œ ํ™œ์šฉ ์ „๋žต (์ดˆ์ŒํŒŒ + ์ ์™ธ์„ )

๋‘ ์„ผ์„œ์˜ ์—ญํ• ์„ ๋ถ„๋ฆฌํ•˜์—ฌ ์ƒํ˜ธ ๋ณด์™„ํ•ฉ๋‹ˆ๋‹ค. ์ดˆ์ŒํŒŒ๋Š” ์ „์—ญ ์ธก๊ฑฐ ๋ฐ ํ•ญ๋ฒ•, ์ ์™ธ์„ ์€ ๊ทผ์ ‘ ๋ฒ”ํผ๋ฅผ ๋‹ด๋‹นํ•ฉ๋‹ˆ๋‹ค.

์„ผ์„œ ๊ตฌํ˜„ ๋ฐ ์—ญํ• 
์ดˆ์ŒํŒŒ F/L/R TIM3 ์ž…๋ ฅ์บก์ฒ˜๋กœ HC-SR04 ์—์ฝ” ํญ ์ธก์ • โ†’ US_TICKS_PER_CM(58)๋กœ cm ํ™˜์‚ฐ. 7-์ƒ˜ํ”Œ ์Šฌ๋ผ์ด๋”ฉ ์œˆ๋„์šฐ์— median7(์ŠคํŒŒ์ดํฌ ์ œ๊ฑฐ)ยทstddev7(์‹ ๋ขฐ๋„) ์ ์šฉ. ๋ฒฝ์ถ”์ข…ยท์ฝ”๋„ˆ rate-jumpยท์ „๋ฉด EMERGENCYยทEKF ์ธก์ • ๊ฐฑ์‹ ์˜ ์ฃผ ์ž…๋ ฅ
์ ์™ธ์„  L/R/๋ฐ”๋‹ฅ ADC 3์ฑ„๋„(์ขŒ ADC3ยท์šฐ ADC2ยท๋ฐ”๋‹ฅ ADC1, 12-bit) ํด๋ง. ์ขŒ/์šฐ IR์„ ๊ทผ์ ‘ '๋ฒ”ํผ'๋กœ ์‚ฌ์šฉ: ๊ฐ’ > IR_BUMPER_THRESH(2100)์ด๋ฉด 30ยฐ EMERGENCY ํŠธ๋ฆฌ๊ฑฐ(์ดˆ์ŒํŒŒ์˜ ๊ทผ๊ฑฐ๋ฆฌ ์‚ฌ๊ฐ ๋ฐ ์ง€์—ฐ ๋ณด์™„)
์œตํ•ฉ ์›๋ฆฌ ์ดˆ์ŒํŒŒ = ์žฅ๊ฑฐ๋ฆฌ ์ธก๊ฑฐ/ํ—ค๋”ฉ/์ฝ”๋„ˆ, ์ ์™ธ์„  = ๊ตญ์†Œ ์ธก์‘/๊ทผ์ ‘ ์•ˆ์ „. ๋‹จ์ผ ์„ผ์„œ ํ•œ๊ณ„(์ดˆ์ŒํŒŒ ์ง€์—ฐ, IR ๋‹จ๊ฑฐ๋ฆฌ)๋ฅผ ์—ญํ•  ๋ถ„๋ฆฌ๋กœ ์ƒ์‡„

6. ์ฃผํ–‰ ์•Œ๊ณ ๋ฆฌ์ฆ˜ ํ๋ฆ„๋„

์ „์› ์ธ๊ฐ€๋ถ€ํ„ฐ ์™„์ฃผ๊นŒ์ง€ ControlTask FSM์˜ ์ œ์–ด ํ๋ฆ„์ž…๋‹ˆ๋‹ค. ๋งค 20 ms ํ‹ฑ๋งˆ๋‹ค EMERGENCY โ†’ ์ฝ”๋„ˆ ํ™•์ธ โ†’ VEER โ†’ ์ง์ง„ ์ˆœ์œผ๋กœ ์šฐ์„ ์ˆœ์œ„ ํŒ๋‹จ์„ ์ˆ˜ํ–‰ํ•ฉ๋‹ˆ๋‹ค.

flowchart TD
    PWR([์ „์› ON ยท ์‹œ์Šคํ…œ ์ดˆ๊ธฐํ™”]) --> INIT["INIT ์ƒํƒœ<br/>์œ ํšจ ์ธก๊ฑฐ dF &gt; 0 ๋Œ€๊ธฐ"]
    INIT -->|dF &gt; 0| TICK([์ œ์–ด ํ‹ฑ 20 ms])

    TICK --> C1{"isEmergency?<br/>์ „๋ฉด US โ‰ค 14cm or IR ๋ฒ”ํผ"}
    C1 -->|Yes| EMG["EMERGENCY ํšŒํ”ผ<br/>์ „๋ฉด: 90ยฐ N-์„ ํ˜ธ ํšŒ์ „<br/>IR: 30ยฐ ํšŒ์ „ + ์ „์ง„<br/>heading ๊ฐฑ์‹ "]
    C1 -->|No| C2{"์ฝ”๋„ˆ ๊ฐœ๊ตฌ๋ถ€?<br/>์ธก๋ฒฝ rate-jump & N-ํ–ฅ ๊ฐœ์„ "}

    C2 -->|Yes| LEAD["์ „์ง„(lead) โ†’ 90ยฐ N-ํ–ฅ ํšŒ์ „<br/>heading ๊ฐฑ์‹ "]
    LEAD --> HN{"heading == N?<br/>(๋ถํ–ฅ ์ฝ”๋„ˆ)"}
    HN -->|Yes| NTC["north_turn_count++<br/>๋ˆ„์  โ‰ฅ 4 โ†’ ์ˆœํ•ญ ๋ถ€์ŠคํŠธ ๋ž˜์น˜<br/>V_CRUISE โ†’ V_CRUISE_BOOST"]
    HN -->|No| DONE
    C2 -->|No| C3{"์ธก๋ฒฝ &lt; 5cm?"}

    C3 -->|Yes| VEER["VEER: 10ยฐ ํšŒํ”ผ ํšŒ์ „ + ์ „์ง„"]
    C3 -->|No| FWD["์ง์ง„ cruise_duty<br/>๋ถ€์ŠคํŠธ ์‹œ V_CRUISE_BOOST"]

    EMG --> DONE([๋™์ž‘ ์™„๋ฃŒ])
    NTC --> DONE
    VEER --> DONE
    FWD --> DONE
    DONE -->|๋ฃจํ”„ 20 ms| TICK
    FWD -->|๋ถ์ชฝ ํƒˆ์ถœ๊ตฌ ๋„๋‹ฌ| GOAL([์™„์ฃผ])
Loading

7. ํƒœ์Šคํฌ ๊ตฌ์„ฑ ๋ฐ ์„ค๊ณ„ (FreeRTOS)

์ฃผ๊ธฐยท์ด๋ฒคํŠธ ํŠน์„ฑ์— ๋”ฐ๋ผ 4๊ฐœ ํƒœ์Šคํฌ๋กœ ๋ถ„๋ฆฌํ•˜๊ณ , ์šฐ์„ ์ˆœ์œ„๋ฅผ Sensor = IR(3) > Control(2) > Debug(1)๋กœ ์„ค์ •ํ•ฉ๋‹ˆ๋‹ค. ๊ฐ์ง€๊ฐ€ ๋ชจ๋“  ํŒ๋‹จ์˜ ์ „์ œ์ด๋ฏ€๋กœ ์„ผ์„œ๋ฅผ ๊ฐ€์žฅ ์šฐ์„ ํ•ฉ๋‹ˆ๋‹ค.

ํƒœ์Šคํฌ ์šฐ์„ ์ˆœ์œ„ ์ฃผ๊ธฐ ์—ญํ• 
SensorTask 3 (1024) 20 ms ์ดˆ์ŒํŒŒ median7/stddev7 โ†’ odom_tick โ†’ ekf_predict โ†’ ekf_update
IR_Task 3 (512) 20 ms IR ADC 3์ฑ„๋„ ํด๋ง โ†’ ir_left/right/floor ๊ฐฑ์‹ 
ControlTask 2 (1024) 20 ms INIT/DRIVE/EMERGENCY FSM ยท ์ฝ”๋„ˆ ยท VEER ยท ํšŒ์ „
DebugTask 1 (512) 100 ms LED ์ƒํƒœ ์ธ์ฝ”๋”ฉ(์šฐ์„ ์ˆœ์œ„ Cascade)

7.1 ๋™๊ธฐํ™” ์„ค๊ณ„

  • ํƒœ์Šคํฌ ์ƒ์„ฑ/์Šค์ผ€์ค„๋ง: xTaskCreate()๋กœ 4 ํƒœ์Šคํฌ ์ƒ์„ฑ ํ›„ vTaskStartScheduler(). ์šฐ์„ ์ˆœ์œ„ ๊ธฐ๋ฐ˜ ์„ ์  ์Šค์ผ€์ค„๋ง.
  • ์ฃผ๊ธฐยท์›Œ๋ฐ์—… ์ œ์–ด: osDelay()๋กœ ๊ฐ ํƒœ์Šคํฌ ์ฃผ๊ธฐ(20/100 ms)์™€ ๋ถ€ํŒ… ์•ˆ์ •ํ™” ์ง€์—ฐ(TASK_WARMUP_MS, CTRL_WARMUP_MS) ๋ถ€์—ฌ.
  • Lock-Free ๊ณต์œ  ์ƒํƒœ: ์„ผ์‹ฑโ†’์ œ์–ด ๋ฐ์ดํ„ฐ(dF/dL/dR ๋“ฑ)๋Š” ๋‹จ์ผ ์ƒ์‚ฐ์ž-๋‹จ์ผ ์†Œ๋น„์ž ํŒจํ„ด. SensorTask๋งŒ ์“ฐ๊ณ  ControlTask๋งŒ ์ฝ์œผ๋ฉฐ, Cortex-M4์—์„œ ์ •๋ ฌ๋œ 32-bit int ์ ‘๊ทผ์ด ์›์ž์ ์ด๋ฏ€๋กœ Lock ์—†์ด ์•ˆ์ „.
  • ISR ๊ณ„์ธต ๋ถ„๋ฆฌ: ์ธ์ฝ”๋”(EXTI ๋ฐฉํ–ฅ ๋””์ฝ”๋“œ), ์ดˆ์ŒํŒŒ ์—์ฝ”ํญ(TIM3 ์ž…๋ ฅ์บก์ฒ˜, 16-bit wrap ์ฒ˜๋ฆฌ), IR(ADC ํด๋ง)์„ ISRยทํƒœ์Šคํฌ๋กœ ์ ์ ˆํžˆ ๋ถ„๋‹ด.

7.2 DebugTask โ€” LED ๊ธฐ๋ฐ˜ ๋””๋ฒ„๊น…

์ดˆ๊ธฐ์—๋Š” TeraTerm์œผ๋กœ UART ๋กœ๊ทธ๋ฅผ ํ™•์ธํ–ˆ์ง€๋งŒ, Calibrationยท์‹œํ—˜ ์ฃผํ–‰ ์ค‘ ์‹œ๋ฆฌ์–ผ ์ผ€์ด๋ธ”์„ ๊ณ„์† ๊ฝ‚๊ณ  ๋‹ค๋‹ˆ๋Š” ๊ฒƒ์ด ๋งค์šฐ ๋ถˆํŽธํ–ˆ์Šต๋‹ˆ๋‹ค. ์ด์— LED๋กœ ๋Ÿฐํƒ€์ž„ ์ƒํƒœ๋ฅผ ์ธ์ฝ”๋”ฉํ•ด ๋ฌด์„ ์œผ๋กœ ๋‚ด๋ถ€ ์ƒํƒœ๋ฅผ ์ฝ๋„๋ก ํ–ˆ๊ณ , ์—ฌ๋Ÿฌ ์ƒํƒœ๊ฐ€ ๊ฒน์น  ๋•Œ๋Š” ์šฐ์„ ์ˆœ์œ„ Cascade๋กœ ๊ฐ€์žฅ ์ค‘์š”ํ•œ ๊ฒƒ์„ ํ‘œ์‹œํ•ฉ๋‹ˆ๋‹ค.

์šฐ์„ ์ˆœ์œ„ LED ํŒจํ„ด ์˜๋ฏธ
1 VEER ์˜ค๋ฒ„๋ ˆ์ด LED1(์šฐ)/LED4(์ขŒ) ์ง์ „ VEER ๋ฐœ์ƒ ์ธก ํ‘œ์‹œ (์•ฝ 2์ดˆ)
2 EMERGENCY ์ „์ฒด ์ ๋ฉธ / LED2ยทLED3 ์ „๋ฉด US=์ „์ฒด ์ ๋ฉธ, IR ์ขŒ=LED2ยท์šฐ=LED3
3 ์†๋„ ๋ถ€์ŠคํŠธ ์ „์ฒด ์ ๋ฉธ(์ง€์†) cruise_boosted ํ™•์ • ์‹œ๊ฐ ํ™•์ธ
4 ๊ธฐ๋ณธ LED1โ€“3 ์ƒํƒœ 2์ง„ ยท LED4 ํ—ค๋”ฉ FSM ์ƒํƒœ + cardinal heading(N=off, S=on, E/W=์ ๋ฉธ)

8. ์„ฑ๋Šฅ ๋ถ„์„

8.1 ์ฃผ์š” ์บ˜๋ฆฌ๋ธŒ๋ ˆ์ด์…˜ ์ƒ์ˆ˜

์ƒ์ˆ˜ ๊ฐ’ ์˜๋ฏธ
US_TICKS_PER_CM 58 ์ดˆ์ŒํŒŒ IC ์นด์šดํŠธ โ†’ cm ํ™˜์‚ฐ
PIVOT_SUBSTEPS_90_L/R 20 / 25 90ยฐ ํšŒ์ „ substep (์ขŒ์šฐ ๋น„๋Œ€์นญ ๋ณด์ •)
EMG_FRONT / D_MIN / D_OPEN 14 / 5 / 150 ๊ธด๊ธ‰ยท๊ทผ์ ‘ยท๋ฌด๋ฒฝ ์ž„๊ณ„ (cm)
IR_BUMPER_THRESH 2100 IR ๋ฒ”ํผ ํŠธ๋ฆฌ๊ฑฐ ADC ์ž„๊ณ„
CORNER_RATE_CM 15 ์ฝ”๋„ˆ(๊ฐœ๊ตฌ๋ถ€) ํŒ์ • ๊ธ‰๋ณ€๋Ÿ‰
NORTH_TURN_THRESHOLD 4 ์ˆœํ•ญ ๋ถ€์ŠคํŠธ ๋ž˜์น˜ ๋ถํ–ฅ ์ฝ”๋„ˆ ์ˆ˜
V_CRUISE / _BOOST / _TURN 18k / 20k / 20k ์ˆœํ•ญยท๋ถ€์ŠคํŠธยทํšŒ์ „ PWM ๋“€ํ‹ฐ

8.2 ์ •์‚ฌ๊ฐ ์ฃผํ–‰ ๊ฒ€์ฆ๊ณผ ๋ฐฐํ„ฐ๋ฆฌ ์˜ํ–ฅ

์ •์‚ฌ๊ฐ ์ฃผํ–‰ ํ›„ ๋ณต๊ท€ ์œ„์น˜ $(x, y, \theta^\circ)$๋กœ ํํ•ฉ ์˜ค์ฐจ๋ฅผ ์ธก์ •ํ–ˆ์Šต๋‹ˆ๋‹ค.

Run (x, y, ฮธยฐ) ๊ฒฐ๊ณผ ํ•ด์„
1 (73, 94, 108) ๋ฐฐํ„ฐ๋ฆฌ ์ €์ „์•• ์˜์‹ฌ โ€” ์ถœ๋ ฅ ์ €ํ•˜๋กœ ํŽธ์ฐจ ํผ
2 (34, 64, โˆ’175) ํ’€์ถฉ์ „ ์ƒํƒœ
3 (9, 0, 103) ์œ„์น˜ ํํ•ฉ (wheelbase ์žฌ๋ณด์ •), ฮธ๋งŒ ์ž”์ฐจ

๋ฐฐํ„ฐ๋ฆฌ ์ถฉ์ „ ์ƒํƒœ๊ฐ€ ๊ฒฐ๊ณผ๋ฅผ ๋‹ค๋ฅด๊ฒŒ ๋งŒ๋“œ๋Š” ์ฃผ์›์ธ์ž…๋‹ˆ๋‹ค. ๊ฒฝ๊ธฐ ์ง์ „ ์ถฉ์ „ ๊ด€๋ฆฌ์™€ wheelbase ์žฌ๋ณด์ •์œผ๋กœ ํํ•ฉ ์˜ค์ฐจ๋ฅผ ์ตœ์†Œํ™”ํ–ˆ์œผ๋ฉฐ, ์ž”์—ฌ ฮธ ์˜ค์ฐจ๋Š” IMU ๋ถ€์žฌ์— ๊ธฐ์ธํ•˜๋ฏ€๋กœ EKF ์ธก์ • ๊ฐฑ์‹ ์œผ๋กœ ์–ต์ œํ•ฉ๋‹ˆ๋‹ค.

8.3 ์•Œ๋ ค์ง„ ์ด์Šˆ์™€ ๋Œ€์‘

์ด์Šˆ ๋Œ€์‘
์ขŒ์šฐ ๋ชจํ„ฐ ๋น„๋Œ€์นญ(ํผ) ์ขŒ/์šฐ ๋ถ„๋ฆฌ substep(20/25) + V_TRIM ๋…ธ๋ธŒ + ํ๋ฃจํ”„, ๋ฌผ๋ฆฌ ์ ๊ฒ€(๋จธ๋ฆฌ์นด๋ฝ ์ œ๊ฑฐ)
IMU(์ž์ด๋กœ) ๋ถ€์žฌ cardinal heading ๋ชจ๋ธ + EKF ์ธก์ • ๊ฐฑ์‹ ์œผ๋กœ ํ—ค๋”ฉ ๋ณด๊ฐ•

8.4 ์ฃผํ–‰ ์˜์ƒ ๋ฐ ์†Œ์Šค์ฝ”๋“œ

๊ตฌ๋ถ„ ๋งํฌ
1ํšŒ์ฐจ ์ฃผํ–‰ (3 ๊ตฌ๊ฐ„) https://youtube.com/shorts/K_pr-sQFMR8
2ํšŒ์ฐจ ์ฃผํ–‰ (2 ๊ตฌ๊ฐ„) https://youtube.com/shorts/Clsc8AX5SYY
3ํšŒ์ฐจ ์ฃผํ–‰ (1 ๊ตฌ๊ฐ„) https://youtube.com/shorts/vPwYhzRY5yc
์—ฐ์Šต ์ฃผํ–‰ (์™„์ฃผ) https://youtube.com/shorts/dIfozfXyMbs
GitHub https://github.com/CNU-Embedded-Programming/Embedded-Challenge

9. ํ”„๋กœ์ ํŠธ ๊ตฌ์กฐ ๋ฐ ๋นŒ๋“œ

Embedded-Challenge/
โ”œโ”€โ”€ Drivers/                  # STM32F4xx HAL, CMSIS, BSP
โ”œโ”€โ”€ Middlewares/              # FreeRTOS (CMSIS-OS)
โ”œโ”€โ”€ docs/                     # ์ด๋ฏธ์ง€ (uBrain, ๋งต)
โ””โ”€โ”€ project/RTOS/
    โ”œโ”€โ”€ Inc/                  # main.h, HAL conf, IT ํ—ค๋”
    โ”œโ”€โ”€ Src/                  # main.c (์ „์ฒด ๋กœ์ง), it/msp/system
    โ”œโ”€โ”€ MDK-ARM/              # Keil uVision ํ”„๋กœ์ ํŠธ (Project.uvprojx) โ† ์‚ฌ์šฉ
    โ”œโ”€โ”€ EWARM/                # IAR ํ”„๋กœ์ ํŠธ
    โ””โ”€โ”€ TrueSTUDIO/           # TrueSTUDIO ํ”„๋กœ์ ํŠธ

๋นŒ๋“œ ๋ฐ ์‹คํ–‰

  1. Keil MDK-ARM uVision์—์„œ project/RTOS/MDK-ARM/Project.uvprojx๋ฅผ ์—ฝ๋‹ˆ๋‹ค.
  2. ํ”„๋กœ์ ํŠธ๋ฅผ ๋นŒ๋“œํ•œ ๋’ค STM32F429 ๋ณด๋“œ์— ๋‹ค์šด๋กœ๋“œ/ํ”Œ๋ž˜์‹œํ•ฉ๋‹ˆ๋‹ค.
  3. ์ „์›๊ณผ ๋ชจํ„ฐ ๋“œ๋ผ์ด๋ฒ„๋ฅผ ์—ฐ๊ฒฐํ•˜๊ณ , ์ฃผํ–‰ ํ™˜๊ฒฝ์— ๋งž๊ฒŒ ์„ผ์„œ ์ž„๊ณ„๊ฐ’ยทํšŒ์ „ substep์„ ํ™•์ธํ•ฉ๋‹ˆ๋‹ค.
  4. ํšŒ์ „/IR/์ง์ง„ ์บ˜๋ฆฌ๋ธŒ๋ ˆ์ด์…˜์ด ํ•„์š”ํ•˜๋ฉด main.c์˜ ๋นŒ๋“œ ์Šค์œ„์น˜(CALIB_PIVOT, CALIB_IR, CALIB_ODOM)๋ฅผ ๋ณ€๊ฒฝ ํ›„ ์žฌ๋นŒ๋“œํ•ฉ๋‹ˆ๋‹ค.

About

No description, website, or topics provided.

Resources

Stars

2 stars

Watchers

0 watching

Forks

Releases

Packages

Contributors

Languages