[브레드보드 IO 13편], 🏁 아두이노로 인버티드 펜듈럼(도립진자) [단계 2-A] 스텝모터 한계 실측 — 오토 호밍 · 속도

[민한식]

민선생의 전자공방 ・ 2026. 7. 2. 14:30

URL 복사 통계

본문 기타 기능

브레드보드 활용 IO 인터페이스 13편

🏁 인버티드 펜듈럼 — 단계 2-A
모터와 기구의 한계에 도전하다
— 오토 호밍 · 속도 한계 실측 · 관성탈조 예방 —

Non-blocking 스텝펄스 · 오토 호밍 · 7단계 속도 테스트 · 상태 머신
아두이노 프로젝트 · 민선생의 전자공방

■ 프롤로그

안녕하세요, 민선생입니다. 지난 12편에서 상보필터로 부드럽고 정확한 각도를 완성했습니다. 이번 13편에서는 드디어 스텝모터를 움직입니다!

사실 이 단계에서 지난 방학 때 스윙업을 시도했다가 모터 탈조로 인버티드 펜듈럼을 직립해보려다 못했습니다. 그래서 이번에는 같은 실수를 반복하지 않기 위해, 마침 블로그를 시작했으니 한단계 한단계 기초를 다지며 해보고 있습니다. 모터와 LM가이드 등 물리시스템의 원활한 구동을 확인하고 다음 단계로 넘어가는것이 타당할 듯 합니다.

그래서 호밍과 중립이동의 단계 2를 두 편으로 나누어 진행합니다. 이번 13편은 스텝모터, 스텝모터 드라이버 구동 테스트, 펄스 구동 최적화의 물리기구 구동시스템 테스트(2-A)단계, 다음 14편은 MPU6050 초기화와 모터 호밍, 중립이동의 전체 통합(2-B)단계로 진행 하려 합니다.

🎯 왜 2단계로 나누는가

13편의 2단계 전략 — 실패에서 배운 설계

2-A (이번 편) : 모터 단독 테스트

MPU-6050 없이 순수 모터 동작만 집중 검증

호밍으로 레일 길이 실측 → 중앙 이동

속도를 단계별로 올리며 하드웨어 한계 실측

관성탈조 예방 코드 적용 및 검증

2-B (다음 편) : 전체 통합

MPU-6050 초기화 + 10초 필터 워밍업

안정화 완료 후 자동 호밍 → 중립 대기

→ 모터가 완벽히 검증된 상태에서 센서를 통합해야

문제 발생 시 원인을 즉시 특정할 수 있습니다!

인버티트펜듈럼 호밍 - 중앙이동 - 7단계 속도 테스트(왕복) - 원점

📌 이전 편 참조

12편 단계 1-B — 상보필터 (좌표계의 중심 확보 완료)

11편 단계 1-A — MPU-6050 캘리브레이션

10편 하드웨어 구성 & 전체 로드맵

9편 DRV8825 + 스텝모터 Non-blocking 제어 → 이번 편의 기술 기반!

🎯 이번 편에서 해 볼 것

① 스텝모터 탈조의 원인 복습 — 속도와 토크의 관계

② 오토 호밍 알고리즘 — CNC·3D 프린터와 같은 원리

③ Non-blocking 스텝 펄스 생성 (micros() 기반)

④ 상태 머신(State Machine) 설계 — 이 프로젝트의 뼈대

⑤ 7단계 속도 테스트로 하드웨어 한계 실측 (25→170mm/s)

⑥ 관성탈조 예방 — 속도 클램핑 + 방향전환 드웰

⑦ 탈조를 수치로 검증하는 방법

회로 구성은 이전편과 동일합니다. 인버티드 펜듈럼 프로젝트 하드웨어는 아래의 회도와 같이 만들어 놓았습니다. 인버티드펜듈럼이 직립할때 까지 모든 과정에서 회로도와 배선은 변경 없이 지금의 배선을 유지하고, 단계별 기능이 추가되고 코드 업그레이드와 단계별 동작 구현 중심으로 진행하려 합니다.

Inverted Pendulum(역진자) 브레드보드 회로 배선도(배선도 툴 Wokwi)

실제 제어부 — Arduino R4 · DRV8825 드라이버 · 브레드보드 배선

1. 스텝모터 탈조 — 복습과 실전

1-1. 탈조란 무엇인가 (9편 복습)

9편에서 다뤘던 내용을 실전 관점에서 다시 정리합니다. 스텝모터는 펄스 하나당 정확히 한 스텝씩 회전하는 모터입니다. 그런데 모터가 감당할 수 없는 조건이 되면 펄스를 받고도 회전하지 못하고 '드르륵' 하고 위치를 놓칩니다. 이것이 탈조(Step Loss)입니다.

⚠️ 탈조의 3가지 원인

탈조가 발생하는 3가지 상황

① 속도 한계 초과

스텝모터는 속도가 올라갈수록 토크가 급격히 감소

→ 한계 속도를 넘으면 회전자가 자기장을 못 따라감

② 급격한 방향 전환 (관성탈조) ← 이번 편의 핵심!

고속 이동 중 방향을 반전하면

카트+펜듈럼의 관성이 반대 방향 토크를 요구

→ 순간적으로 토크 부족 → 슬립 발생

③ 펄스 타이밍 붕괴

lcd.clear() 같은 블로킹 코드가 CPU를 점유

→ 펄스 간격이 깨져 부드러운 회전 불가

→ 9편과 10편에서 다룬 lcd.clear() 금지의 이유

1-2. DRV8825와 1/8 마이크로스텝 (핵심 설정 복습)

9편에서는 1/16 마이크로스텝을 사용했지만, 이 프로젝트에서는 1/8 마이크로스텝을 사용합니다. 토크와 부드러움의 균형을 고려한 선택입니다.

M0	M1	M2	마이크로스텝	1회전 스텝수
L	L	L	Full (1/1)	200
H	H	L	1/8 ✅ 이 프로젝트	1,600
L	L	H	1/16 (9편에서 사용)	3,200
H	H	H	1/32 (최고 부드러움)	6,400

💡 핵심 포인트

이 프로젝트의 이동 분해능 계산

1회전 = 200 × 8 = 1,600 스텝

GT2 풀리 20T × 2mm 피치 → 1회전 = 40mm 이동

→ 1 스텝 = 40 ÷ 1,600 = 0.025mm

속도 → 스텝 간격 변환

스텝 간격(μs) = 25,000 ÷ 속도(mm/s)

예) 100mm/s → 250μs, 170mm/s → 147μs

2. 오토 호밍 알고리즘

2-1. 왜 호밍이 필요한가

전원을 켠 순간, 아두이노는 카트가 레일의 어디에 있는지 전혀 알 수 없습니다. 3D 프린터를 켜면 노즐이 먼저 구석으로 이동해 '삑' 하고 원점을 잡는 것을 보셨을 겁니다. 그것이 바로 오토 호밍(Auto-Homing)입니다.

호밍이 확보하는 것

① 절대 좌표계의 원점 (position = 0)

② 레일의 실제 이동 가능 거리 (maxPos)

③ 정확한 중앙 위치 (centerPos = maxPos / 2)

→ 이후 모든 이동은 '중앙 기준 몇 mm' 로 정확하게 제어 가능

→ 12편의 민선생 Tip을 기억하시나요?

상보필터 = 좌표계의 중심 / 오토 호밍 = 기구적 중심!

2-2. 호밍 알고리즘 5단계

리밋스위치 2개가 D3 하나에 병렬로 연결되어 있어 어느 쪽 스위치가 터치됐는지 구분할 수 없습니다. 그래서 방향을 코드로 고정하는 방식을 사용합니다.

📐 호밍 알고리즘

① CW 방향 저속(50mm/s) 이동 → 리밋 터치 감지

호밍은 항상 저속! (안전 + 정밀한 터치 위치)

② Back-off : 반대 방향 100 step (2.5mm) 후퇴

스위치에서 떨어진 위치를 기준점으로 사용

→ 이 위치를 position = 0 으로 확정

③ CCW 방향 저속 이동 → 반대쪽 리밋 터치 감지

④ Back-off 100 step → 이 위치가 maxPos (레일 길이 실측!)

⑤ 중앙(maxPos / 2)으로 이동 → 호밍 완료

✅ 호밍 실측 데이터

실측 결과 — 우리 하드웨어의 레일 길이

maxPos = 13,545 스텝 = 338.6 mm

centerPos = 6,772 스텝 = 169.3 mm

→ 설계값(LM가이드 420mm)에서 카트 폭과 Back-off를 제외한

실제 이동 가능 거리가 정확하게 실측되었습니다!

3. Non-blocking 스텝 펄스 — 이번 편의 핵심 기술

3-1. 왜 delay()를 쓰면 안 되는가

9편의 테스트 코드에서는 delayMicroseconds()로 펄스를 만들었습니다. 모터만 돌린다면 문제없지만, 앞으로 MPU-6050 읽기·필터 계산·PID 연산이 동시에 돌아가야 합니다. delay()는 그 시간 동안 CPU를 완전히 멈추므로 절대 사용할 수 없습니다.

코드 ① — Non-blocking 스텝 펄스

// ★ Non-blocking 스텝 펄스 생성 — 이 프로젝트 전체의 심장

void updateStepper() {

if (!moving) return; // 이동 중이 아니면 즉시 복귀

unsigned long nowUs = micros();

if (nowUs - lastStepUs >= stepInterval) { // 스텝 시간이 됐나?

lastStepUs = nowUs;

// 스텝 펄스 1개 출력 (DRV8825 최소 펄스폭 1.9μs 보장)

digitalWrite(PIN_STEP, HIGH);

delayMicroseconds(3);

digitalWrite(PIN_STEP, LOW);

currentPos += moveDir; // 위치 카운트 (핵심!)

if (currentPos == targetPos) moving = false; // 목표 도달

}

}

💡 핵심 포인트

이 함수의 설계 철학

loop()가 매우 빠르게 반복되면서 이 함수를 계속 호출

→ 스텝 시간이 되면 펄스 1개만 쏘고 즉시 복귀

→ 나머지 시간에 다른 작업(센서·LCD·PID) 가능

currentPos 변수가 곧 카트의 절대 위치!

→ 엔코더 없이도 소프트웨어로 위치를 정확히 추적

→ 단, 탈조가 발생하면 이 값과 실제 위치가 어긋남

→ 그래서 탈조 검증이 중요합니다 (5장에서 설명)

3-2. 상태 머신 — 12편에서 예고한 설계 기법

12편 마지막에 예고했던 상태 머신(State Machine)을 드디어 적용합니다. 복잡한 동작 순서를 상태로 나누어 관리하면 코드가 명확해지고 디버깅이 쉬워집니다.

코드 ② — 상태 머신 구조

// 상태 머신 정의 — 전체 동작 흐름이 한눈에!

enum SysState {

ST_HOMING_CW, // ① CW 이동 → 리밋 찾기

ST_BACKOFF_1, // ② Back-off → pos = 0 확정

ST_HOMING_CCW, // ③ CCW 이동 → 반대 리밋 찾기

ST_BACKOFF_2, // ④ Back-off → maxPos 확정

ST_MOVE_CENTER, // ⑤ 중앙 이동

ST_PAUSE, // 단계 사이 2초 대기

ST_TEST, // ⑥ 7단계 속도 왕복 테스트

ST_VERIFY, // ⑦ 탈조 검증 (재호밍)

ST_DONE, // 완료

ST_ERROR // 에러 (리밋 미감지)

};

// loop()는 switch문으로 현재 상태의 일만 처리

void loop() {

switch (state) {

case ST_HOMING_CW: ... break;

case ST_BACKOFF_1: ... break;

// 각 상태는 조건 충족 시 다음 상태로 전환

}

}

상태 머신의 장점 — 이후 단계까지 이어지는 뼈대

2-B에서는 이 앞에 ST_WARMING_UP(필터 10초 수렴)만 추가

스윙업 단계에서는 ST_SWINGUP, ST_BALANCE 추가

→ 기존 상태는 그대로, 새 상태만 끼워 넣는 확장 구조!

4. 속도 한계 실측 — 모터와 기구의 한계에 도전

4-1. 테스트 설계

이론상 한계가 아니라 우리 하드웨어의 실제 한계를 알아야 합니다. 중앙에서 ±80mm 왕복하며 속도를 7단계로 올려가는 테스트를 설계했습니다.

단계	속도	스텝 간격	실측 결과
1	25mm/s	1000μs	✅ 정확 복귀 (pos=6772)
2	50mm/s	500μs	✅ 정확 복귀
3	80mm/s	312μs	✅ 정확 복귀
4	110mm/s	227μs	✅ 정확 복귀
5	130mm/s	192μs	✅ 정확 복귀
6	150mm/s	166μs	✅ 정확 복귀
7	170mm/s	147μs	✅ 정확 복귀 — 한계 확정!
—	190mm/s	131μs	❌ 정역전환 시 슬립 발생 (별도 테스트)

✅ 확정 속도 체계

실측으로 확정된 속도 체계 (이후 전 단계 공통 기준!)

하드웨어 한계 : 170 mm/s (190에서 정역전환 슬립)

스윙업 최대 속도 : 130 mm/s (한계의 75% 안전 마진)

중앙 이동 속도 : 80 mm/s

호밍 속도 : 50 mm/s (정밀·안전)

★ 이 값들이 스윙업(3단계)·PID(4단계) 코드의

속도 상한으로 그대로 사용됩니다!

4-2. 흥미로운 발견 — 등속 한계와 반전 한계는 다르다

테스트 중 중요한 사실을 발견했습니다. 등속으로 달리는 것과 방향을 바꾸는 것은 전혀 다른 부하 조건이라는 점입니다.

💡 등속 한계 ≠ 반전 한계

실측 관찰 결과

등속 주행 : 190mm/s 이상도 회전 자체는 가능

정역전환(반전) : 190mm/s에서 슬립 발생!

왜 반전이 더 어려운가?

고속 이동 중인 카트 + 펜듈럼에는 큰 관성이 있음

→ 방향 반전 순간, 모터는 이 관성을 거슬러

반대 방향 토크를 즉시 만들어야 함

→ 순간 토크 요구량이 등속 주행의 몇 배로 급증

→ 토크 부족 → 슬립 (관성탈조)

★ 스윙업과 PID는 방향 전환이 핵심 동작!

따라서 반전 한계(170)을 기준으로 잡았습니다.

5. 관성탈조 예방 — 2중 안전장치

실측으로 한계를 알았으니, 이제 코드가 그 한계를 절대 넘지 않도록 안전장치를 설계합니다. 핵심은 PID 제어에 지장을 주지 않으면서 하드웨어를 보호하는 것입니다.

5-1. 안전장치 ① — 속도 클램핑

코드 ③ — 속도 클램핑

// ★ 속도 클램핑 — 관성탈조 예방 ①

// 어떤 속도 요청이 와도 하드웨어 한계(170mm/s)를 넘지 않음

// 이후 PID 단계에서 출력이 폭주해도 이 함수가 하드웨어를 보호!

#define MIN_INTERVAL_US 147 // 170mm/s의 스텝 간격

unsigned long clampInterval(unsigned long interval) {

if (interval < MIN_INTERVAL_US) return MIN_INTERVAL_US;

return interval;

}

// 사용 예 : 모든 이동 시작 시 자동 적용

void startMove(long target, unsigned long interval) {

stepInterval = clampInterval(interval); // ← 여기!

...

}

💡 핵심 포인트

PID 제어에 지장이 없는 이유

클램핑은 단순히 '상한값'만 제한

→ PID가 계산한 출력이 한계 이내면 그대로 통과

→ 한계를 넘는 순간에만 최대값으로 잘라줌

→ 제어 반응성에 전혀 영향 없음, 하드웨어만 보호

5-2. 안전장치 ② — 방향전환 드웰 (Dwell)

190mm/s에서 확인된 반전 슬립을 방지하기 위해, 방향이 바뀔 때만 1ms의 짧은 대기를 삽입합니다. 관성이 정리될 시간을 주는 것입니다.

코드 ④ — 방향전환 드웰

// ★ 방향전환 드웰 — 관성탈조 예방 ②

// 방향 반전 시 1ms 대기 → 관성 정리 시간 확보

// PID 튜닝 시 문제가 되면 0으로 변경하여 즉시 비활성화 가능

#define DWELL_US 1000 // 1ms (0 = 비활성화)

void startMove(long target, unsigned long interval) {

...

int newDir = (target > currentPos) ? +1 : -1;

// 직전 이동과 반대 방향이면 드웰 적용

if (lastDir != 0 && newDir != lastDir && DWELL_US > 0) {

delayMicroseconds(DWELL_US); // 1ms 관성 정리

}

moveDir = newDir;

lastDir = newDir;

digitalWrite(PIN_DIR, (newDir > 0) ? DIR_CCW : DIR_CW);

delayMicroseconds(5); // DIR 셋업 타임 (650ns 이상)

...

}

설계 판단 — 가감속을 뺀 이유

드웰 1ms가 PID에 지장 없는 이유

PID 제어 주기 = 5ms (5,000μs)

드웰 시간 = 1ms (1,000μs)

→ 방향이 바뀌는 순간에만 발생하며 제어 주기 내에 흡수됨

→ 만약 직립 제어에서 미세 떨림이 생기면

DWELL_US = 0 한 줄로 즉시 끌 수 있게 설계

※ 사다리꼴 가감속은 적용하지 않음

→ 가감속은 PID 출력 반응을 지연시켜 제어에 불리

→ 클램핑 + 드웰만으로 충분히 안전 확보

6. 탈조를 수치로 검증하는 방법

탈조는 눈과 귀로도 감지할 수 있지만('틱' 소리, 떨림), 이 코드는 탈조를 수치로 검증합니다. 원리는 간단하면서도 강력합니다.

📐 탈조 검증 알고리즘

탈조 수치 검증 원리

① 7단계 왕복 테스트 완료 후

② 저속으로 다시 CW 리밋까지 이동 (재호밍)

③ 이때 currentPos를 확인

이론값 : Back-off 위치인 -100 step에서 리밋 터치

실제값과 이론값의 차이 = 그동안 누적된 탈조량!

왕복 중 currentPos가 매번 6772로 정확히 복귀

+ 재호밍 오차가 허용치 이내

= 전 구간 탈조 없음이 수치로 증명됨

💡 스위치 반복 오차와 위치 관리 철학

리밋스위치 반복 오차에 대하여

재호밍 오차에는 탈조 외에 리밋스위치의 기계적

반복 오차(터치 위치가 매번 조금씩 다름)가 포함됩니다.

실측 : 속도를 2배 이상 올려도 오차가 크게 늘지 않음

→ 오차의 대부분은 스위치 반복 오차로 판단

→ 진짜 탈조라면 속도에 비례해 급증했을 것

★ 그래서 위치 기준은 스위치가 아닌

소프트웨어 카운트(currentPos)로 관리합니다.

스위치는 초기 원점 확보와 비상정지 역할만!

시리얼 모니터 — 호밍 결과 및 7단계 테스트 로그 화면

7. 완성 코드 전체 (코딩 작업에 AI의 도움을 받았습니다.)

실제 하드웨어에서 7단계 속도 테스트를 모두 통과한 단계 2-A 최종 코드입니다.

/*

* ================================================================

* Inverted Pendulum — 단계 2-A (모터 단독 테스트) 최종본

* 오토 호밍 + 중앙 정렬 + 7단계 속도 검증 + 관성탈조 예방

*

* 실측 확정 사양 (테스트로 검증 완료)

* 하드웨어 한계 : 170mm/s (190mm/s에서 정역전환 슬립 발생)

* 스윙업 최대 : 130mm/s (한계의 75% 안전 마진)

* 호밍 속도 : 50mm/s

* 중앙 이동 : 80mm/s

*

* 관성탈조 예방 (이번 최종본 추가)

* ① 속도 클램핑 : 모든 이동 요청을 170mm/s 이하로 제한

* ② 방향전환 드웰 : 방향 반전 시 1ms 대기 (관성 정리)

* → DWELL_US = 0 으로 바꾸면 즉시 비활성화 가능

* ③ 가감속 미적용 : PID 응답성 우선 설계

*

* 속도 → 스텝 간격 (0.025mm/step, 1/8 마이크로스텝)

* 25mm/s→1000μs 80mm/s→312μs 130mm/s→192μs

* 50mm/s→ 500μs 110mm/s→227μs 150mm/s→166μs

* 170mm/s→147μs

*

* 작성 : 민선생의 전자공방 2026

* ================================================================

*/

#include <Wire.h>

#include <LiquidCrystal_I2C.h>

// ── 핀 정의 ─────────────────────────────────────

#define PIN_LIMIT 3

#define PIN_DIR 4

#define PIN_STEP 5

#define PIN_EN 6

// ── 방향 정의 ───────────────────────────────────

#define DIR_INVERT 0

#define DIR_CW (DIR_INVERT ? LOW : HIGH)

#define DIR_CCW (DIR_INVERT ? HIGH : LOW)

// ── 기구 사양 ───────────────────────────────────

#define MM_PER_STEP 0.025f

#define STEPS_PER_MM 40

// ── ★ 확정 속도 체계 (실측 기반) ─────────────────

#define LIMIT_MMS 170.0f // 하드웨어 한계 (실측!)

#define MIN_INTERVAL_US 147 // 170mm/s의 스텝 간격 — 클램핑 기준

#define SWING_MAX_MMS 130.0f // 스윙업 최대 (75% 마진) — 이후 단계 사용

#define SPEED_HOMING_US 500 // 호밍 50mm/s

#define SPEED_CENTER_US 312 // 중앙 이동 80mm/s

// ── ★ 관성탈조 예방 : 방향전환 드웰 ──────────────

// 방향 반전 시 관성 정리를 위한 대기 시간 (μs)

// PID 튜닝 시 문제가 되면 0으로 변경하여 비활성화

#define DWELL_US 1000 // 1ms

// ── 테스트 속도 테이블 (7단계 최종 검증) ──────────

#define NUM_SPEEDS 7

const float speedMmS[NUM_SPEEDS] = {25, 50, 80, 110, 130, 150, 170};

const unsigned long speedIntUs[NUM_SPEEDS] = {1000, 500, 312, 227, 192, 166, 147};

// ── 호밍·테스트 설정 ────────────────────────────

#define BACKOFF_STEPS 100

#define TEST_DIST_MM 80

#define TEST_DIST_STEPS (TEST_DIST_MM * STEPS_PER_MM) // 3200

#define MAX_HOMING_STEPS 20000

#define VERIFY_TOL_STEPS 100 // 리밋스위치 반복오차 허용 (2.5mm)

// ── LCD ─────────────────────────────────────────

LiquidCrystal_I2C lcd(0x27, 20, 4);

// ── 상태 머신 ───────────────────────────────────

enum SysState {

ST_HOMING_CW, ST_BACKOFF_1, ST_HOMING_CCW, ST_BACKOFF_2,

ST_MOVE_CENTER, ST_PAUSE, ST_TEST, ST_VERIFY, ST_DONE, ST_ERROR

};

SysState state, nextState;

// ── 모터 제어 변수 ──────────────────────────────

long currentPos = 0;

long targetPos = 0;

long maxPos = 0;

long centerPos = 0;

int moveDir = 0; // +1 / -1

int lastDir = 0; // 직전 이동 방향 (드웰 판단용)

bool moving = false;

unsigned long stepInterval = SPEED_HOMING_US;

unsigned long lastStepUs = 0;

long homingStepCount = 0;

// ── 테스트 변수 ─────────────────────────────────

int speedIndex = 0;

int testPhase = 0;

unsigned long pauseStartMs = 0;

unsigned long moveStartMs = 0; // ★ 이동시간 실측용

// ════════════════════════════════════════════════

// 리밋스위치 (노이즈 필터)

// ════════════════════════════════════════════════

bool limitTouched() {

if (digitalRead(PIN_LIMIT) == HIGH) {

delayMicroseconds(500);

if (digitalRead(PIN_LIMIT) == HIGH) return true;

}

return false;

}

// ════════════════════════════════════════════════

// 모터 Enable (Active LOW)

// ════════════════════════════════════════════════

void motorEnable(bool on) {

digitalWrite(PIN_EN, on ? LOW : HIGH);

if (on) delay(10);

}

// ════════════════════════════════════════════════

// ★ 속도 클램핑 — 관성탈조 예방 ①

// 어떤 요청이 와도 하드웨어 한계(170mm/s)를 넘지 않음

// 이후 PID 단계에서도 이 함수를 그대로 사용!

// ════════════════════════════════════════════════

unsigned long clampInterval(unsigned long interval) {

if (interval < MIN_INTERVAL_US) return MIN_INTERVAL_US;

return interval;

}

// ════════════════════════════════════════════════

// 이동 시작 — 방향전환 드웰 포함 (관성탈조 예방 ②)

// ════════════════════════════════════════════════

void startMove(long target, unsigned long interval) {

targetPos = target;

stepInterval = clampInterval(interval); // ★ 클램핑 적용

int newDir;

if (target > currentPos) newDir = +1;

else if (target < currentPos) newDir = -1;

else { moving = false; return; }

// ★ 방향전환 드웰 : 직전과 반대 방향이면 1ms 대기

if (lastDir != 0 && newDir != lastDir && DWELL_US > 0) {

delayMicroseconds(DWELL_US);

}

moveDir = newDir;

lastDir = newDir;

digitalWrite(PIN_DIR, (newDir > 0) ? DIR_CCW : DIR_CW);

delayMicroseconds(5); // DIR 셋업 타임

lastStepUs = micros();

moveStartMs = millis(); // ★ 이동시간 측정 시작

moving = true;

}

// ════════════════════════════════════════════════

// ★ Non-blocking 스텝 펄스 (핵심)

// ════════════════════════════════════════════════

void updateStepper() {

if (!moving) return;

unsigned long nowUs = micros();

if (nowUs - lastStepUs >= stepInterval) {

lastStepUs = nowUs;

digitalWrite(PIN_STEP, HIGH);

delayMicroseconds(3);

digitalWrite(PIN_STEP, LOW);

currentPos += moveDir;

if (currentPos == targetPos) moving = false;

}

}

// ════════════════════════════════════════════════

// 호밍 전용 무제한 이동

// ════════════════════════════════════════════════

void startHomingMove(int dir, unsigned long interval) {

moveDir = dir;

lastDir = dir;

stepInterval = clampInterval(interval);

digitalWrite(PIN_DIR, (dir > 0) ? DIR_CCW : DIR_CW);

delayMicroseconds(5);

homingStepCount = 0;

lastStepUs = micros();

moving = true;

}

void updateHomingStepper() {

if (!moving) return;

unsigned long nowUs = micros();

if (nowUs - lastStepUs >= stepInterval) {

lastStepUs = nowUs;

digitalWrite(PIN_STEP, HIGH);

delayMicroseconds(3);

digitalWrite(PIN_STEP, LOW);

currentPos += moveDir;

homingStepCount++;

}

}

// ════════════════════════════════════════════════

// LCD (★ 모터 정지 중에만!)

// ════════════════════════════════════════════════

void lcdShow(const char* l1, const char* l2,

const char* l3, const char* l4) {

lcd.setCursor(0, 0); lcd.print(l1);

lcd.setCursor(0, 1); lcd.print(l2);

lcd.setCursor(0, 2); lcd.print(l3);

lcd.setCursor(0, 3); lcd.print(l4);

}

void enterPause(SysState next) {

nextState = next;

pauseStartMs = millis();

state = ST_PAUSE;

}

// ════════════════════════════════════════════════

// 속도 테스트 1회 시작

// ════════════════════════════════════════════════

void startSpeedTest(int idx) {

char b1[21], b2[21], b4[21];

snprintf(b1, sizeof(b1), "TEST %d/7 ", idx + 1);

snprintf(b2, sizeof(b2), "Speed:%5.1f mm/s ", speedMmS[idx]);

snprintf(b4, sizeof(b4), "Limit:170 Max:130 ");

lcd.clear();

lcdShow(b1, b2, "+-80mm round trip ", b4);

Serial.print("[TEST ");

Serial.print(idx + 1);

Serial.print("/7] ");

Serial.print(speedMmS[idx], 1);

Serial.print("mm/s (");

Serial.print(speedIntUs[idx]);

Serial.println("us/step) start");

testPhase = 0;

startMove(centerPos + TEST_DIST_STEPS, speedIntUs[idx]);

state = ST_TEST;

}

// ════════════════════════════════════════════════

// setup()

// ════════════════════════════════════════════════

void setup() {

Serial.begin(115200);

Wire.begin();

Wire.setClock(400000);

pinMode(PIN_LIMIT, INPUT_PULLUP); // ★ 필수!

pinMode(PIN_DIR, OUTPUT);

pinMode(PIN_STEP, OUTPUT);

pinMode(PIN_EN, OUTPUT);

digitalWrite(PIN_STEP, LOW);

motorEnable(false);

lcd.init();

lcd.backlight();

lcd.clear();

lcdShow("IP Stage2-A Final ",

"Homing+Speed Verify",

"Limit:170mm/s ",

"Starting in 3sec...");

Serial.println("========================================");

Serial.println(" Inverted Pendulum Stage 2-A FINAL");

Serial.println(" Motor Test with Anti-Skip Protection");

Serial.println("========================================");

Serial.println("HW Limit : 170mm/s (measured)");

Serial.println("Swing Max : 130mm/s (75% margin)");

Serial.println("Anti-skip : clamp + 1ms dwell on reverse");

Serial.println("Test speeds : 25/50/80/110/130/150/170 mm/s");

Serial.println("");

delay(3000);

motorEnable(true);

lcd.clear();

lcdShow("=== HOMING CW === ",

"Moving to limit... ",

"50mm/s ",

" ");

Serial.println("[HOMING] CW direction, searching limit...");

startHomingMove(-1, SPEED_HOMING_US);

state = ST_HOMING_CW;

}

// ════════════════════════════════════════════════

// loop() — 상태 머신

// ════════════════════════════════════════════════

void loop() {

switch (state) {

case ST_HOMING_CW:

updateHomingStepper();

if (limitTouched()) {

moving = false;

Serial.println("[HOMING] CW limit touched!");

startMove(currentPos + BACKOFF_STEPS, SPEED_HOMING_US);

state = ST_BACKOFF_1;

}

if (homingStepCount > MAX_HOMING_STEPS) state = ST_ERROR;

break;

case ST_BACKOFF_1:

updateStepper();

if (!moving) {

currentPos = 0;

Serial.println("[HOMING] Back-off done. Position = 0");

lcd.clear();

lcdShow("=== HOMING CCW === ",

"Moving to limit... ",

"Left pos = 0 ",

" ");

startHomingMove(+1, SPEED_HOMING_US);

state = ST_HOMING_CCW;

}

break;

case ST_HOMING_CCW:

updateHomingStepper();

if (limitTouched()) {

moving = false;

Serial.println("[HOMING] CCW limit touched!");

startMove(currentPos - BACKOFF_STEPS, SPEED_HOMING_US);

state = ST_BACKOFF_2;

}

if (homingStepCount > MAX_HOMING_STEPS) state = ST_ERROR;

break;

case ST_BACKOFF_2:

updateStepper();

if (!moving) {

maxPos = currentPos;

centerPos = maxPos / 2;

Serial.print("[HOMING] maxPos = ");

Serial.print(maxPos);

Serial.print(" steps = ");

Serial.print(maxPos * MM_PER_STEP, 1);

Serial.println(" mm");

Serial.print("[HOMING] centerPos = ");

Serial.println(centerPos);

char b3[21];

snprintf(b3, sizeof(b3), "Rail:%5.0fmm ",

maxPos * MM_PER_STEP);

lcd.clear();

lcdShow("=== HOMING DONE ===",

"Moving to center...",

b3,

"80mm/s ");

startMove(centerPos, SPEED_CENTER_US);

state = ST_MOVE_CENTER;

}

break;

case ST_MOVE_CENTER:

updateStepper();

if (!moving) {

Serial.println("[CENTER] Arrived at center.");

Serial.println("");

Serial.println("=== 7-Speed Final Verify Start ===");

speedIndex = 0;

lcd.clear();

lcdShow("=== AT CENTER === ",

"Final verify ready ",

"Test 1 in 2sec ",

"25mm/s ");

enterPause(ST_TEST);

}

break;

case ST_PAUSE:

if (millis() - pauseStartMs >= 2000) {

startSpeedTest(speedIndex);

}

break;

case ST_TEST:

updateStepper();

if (!moving) {

if (testPhase == 0) {

testPhase = 1;

startMove(centerPos - TEST_DIST_STEPS, speedIntUs[speedIndex]);

} else if (testPhase == 1) {

testPhase = 2;

startMove(centerPos, speedIntUs[speedIndex]);

} else {

// ★ 왕복 완료 — 이동시간 실측 포함 로그

unsigned long elapsed = millis() - pauseStartMs;

// 이론 시간 : 320mm ÷ 속도 (왕복 4구간 = 80×4)

float theoreticalS = (TEST_DIST_MM * 4.0f) / speedMmS[speedIndex];

Serial.print("[TEST ");

Serial.print(speedIndex + 1);

Serial.print("/7] Done. pos=");

Serial.print(currentPos);

Serial.print(" (expect ");

Serial.print(centerPos);

Serial.print(") theoretical move time=");

Serial.print(theoreticalS, 2);

Serial.println("s");

speedIndex++;

if (speedIndex < NUM_SPEEDS) {

char b1[21], b2[21];

snprintf(b1, sizeof(b1), "Test %d done ", speedIndex);

snprintf(b2, sizeof(b2), "Next:%5.1f mm/s ",

speedMmS[speedIndex]);

lcd.clear();

lcdShow(b1, "Listen for skip! ", b2, "in 2sec... ");

enterPause(ST_TEST);

} else {

lcd.clear();

lcdShow("All 7 tests DONE ",

"Verifying... ",

"Re-homing CW 50mm/s",

" ");

Serial.println("");

Serial.println("[VERIFY] Re-homing to check step loss...");

startHomingMove(-1, SPEED_HOMING_US);

state = ST_VERIFY;

}

}

}

break;

case ST_VERIFY:

updateHomingStepper();

if (limitTouched()) {

moving = false;

motorEnable(false);

long expected = -BACKOFF_STEPS;

long error = currentPos - expected;

float errorMm = error * MM_PER_STEP;

Serial.println("");

Serial.println("========================================");

Serial.println(" FINAL VERIFY RESULT");

Serial.print(" Expected : "); Serial.println(expected);

Serial.print(" Actual : "); Serial.println(currentPos);

Serial.print(" Diff : ");

Serial.print(error);

Serial.print(" steps (");

Serial.print(errorMm, 2);

Serial.println(" mm)");

char b2[21], b3[21];

snprintf(b2, sizeof(b2), "Diff:%+5ld steps ", error);

snprintf(b3, sizeof(b3), " %+7.2f mm ", errorMm);

lcd.clear();

if (abs(error) <= VERIFY_TOL_STEPS) {

Serial.println(" RESULT : PASS!");

Serial.println(" (diff = limit switch repeatability)");

Serial.println("");

Serial.println(" >> CONFIRMED SPEED SYSTEM <<");

Serial.println(" HW Limit : 170 mm/s");

Serial.println(" Swing Max : 130 mm/s");

Serial.println(" Homing : 50 mm/s");

Serial.println(" >> Ready for Stage 2-B (MPU+Homing) <<");

Serial.println("========================================");

lcdShow("== FINAL PASS! == ",

b2, b3,

"Go to Stage 2-B! ");

} else {

Serial.println(" RESULT : Step loss over tolerance!");

Serial.println("========================================");

lcdShow("== CHECK NEEDED ==",

b2, b3,

"See serial monitor ");

}

state = ST_DONE;

}

if (homingStepCount > MAX_HOMING_STEPS) state = ST_ERROR;

break;

case ST_DONE:

break;

case ST_ERROR:

moving = false;

motorEnable(false);

lcd.clear();

lcdShow("!!! ERROR !!! ",

"Limit not found ",

"Check limit switch ",

"wiring on D3 ");

Serial.println("!!! ERROR: Limit not detected within 500mm");

state = ST_DONE;

break;

default:

break;

}

}

/*

* ================================================================

* 검증 체크리스트 (2-A 최종본)

*

* □ 호밍 → maxPos ≒ 341mm (13600~13700 step) 확인

* □ 중앙 이동 (80mm/s) 확인

* □ 7단계 (25→170mm/s) 왕복 모두 pos = center 복귀

* □ 170mm/s에서도 '틱' 소리 없음 (드웰 효과 확인)

* □ 최종 Diff가 ±100 step (2.5mm) 이내 → PASS

* □ LCD "FINAL PASS!" 확인

*

* 확정되는 것

* ★ 하드웨어 한계 170mm/s + 클램핑 함수 → 이후 전 단계 재사용

* ★ 방향전환 드웰 1ms → PID 단계에서 문제 시 DWELL_US=0

* ★ Non-blocking 스텝 구조 → 2-B에서 그대로 사용

*

* PASS → 2-B : MPU6050 초기화 + 10초 워밍업 + 자동 호밍!

* ================================================================

*/

8. 동작 확인 & 실측 결과

8-1. 실측 테스트 결과 종합

항목	실측값	의미
레일 실측 길이	13,545 step = 338.6mm	호밍으로 자동 측정
중앙 위치	6,772 step = 169.3mm	이후 기준 좌표
7단계 왕복 위치	전부 6,772 정확 복귀	이동 중 탈조 0건!
하드웨어 한계	170mm/s	정역전환 기준 실측
반전 슬립 발생점	190mm/s	등속은 가능, 반전 시 슬립
확정 스윙업 최대	130mm/s	한계의 75% 안전 마진

펜듈럼 호밍 → 중앙 이동 → 7단계 속도 왕복 테스트 전체 과정 영상

8-2. 검증 체크리스트

✅ 동작 확인 체크리스트

□ 전원 ON 3초 후 CW 저속 호밍 시작

□ 리밋 터치 → 2.5mm Back-off → Position = 0

□ 반대편 호밍 → maxPos 실측 (약 13,500 step 부근)

□ 중앙 이동 (80mm/s) 완료

□ 7단계 속도 테스트 자동 진행 (25→170mm/s)

□ 각 왕복 후 pos = centerPos 정확 복귀 확인 ← 핵심!

□ 170mm/s에서도 '틱' 소리 없이 부드러운 동작

□ 최종 재호밍 검증 통과

★ 모두 통과 → 2-B (MPU6050 통합 + 자동 호밍) 진행!

9. 마무리

이번 2-A에서는 지난 방학의 모터탈조를 반면교사 삼아, 모터와 기구의 한계를 실측으로 확정했습니다. 이론값이 아닌 우리 하드웨어의 진짜 한계(170mm/s)를 찾았고, 그 한계를 코드가 절대 넘지 않도록 클램핑과 드웰이라는 2중 안전장치를 만들었습니다.

특히 등속 한계와 반전 한계가 다르다는 발견은 스윙업과 PID 설계에 직접 반영될 중요한 실측 데이터입니다. 이렇게 단계별로 검증하며 쌓아 올린 기반 위에서는 이후 문제가 생겨도 원인을 즉시 특정할 수 있습니다.

이번 편에서 완성한 것

✅ Non-blocking 스텝 펄스 생성 (micros() 기반) 첫 적용

✅ 상태 머신 설계 — 이후 전 단계의 뼈대

✅ 오토 호밍 — 레일 길이 실측 (338.6mm)

✅ 7단계 속도 테스트 — 하드웨어 한계 170mm/s 확정

✅ 관성탈조 예방 — 클램핑 + 방향전환 드웰

✅ 탈조 수치 검증 방법 확립

✅ 확정 속도 체계 : 170 / 130 / 80 / 50 mm/s

▶ 다음 편 예고

▶ 다음 편 예고 — 14편 [단계2-B] : MPU6050 센터 초기화 및 워밍업 & 자동 호밍 전체 통합

이제 검증된 모터에 12편의 상보필터를 통합합니다!

• MPU-6050 초기화 + 캘리브레이션

• 상보필터 10초 워밍업 — LCD 카운트다운 표시

• STATE_WARMING_UP → STATE_HOMING → STATE_READY

• 필터 안정화 완료 후 호밍 자동 시작

• 중립 위치 이동 후 스윙업 대기 상태 진입

12편에서 예고했던 상태 머신 3단계가 드디어 완성됩니다!

민선생의 전자공방 · 브레드보드 활용 IO 인터페이스 13편(2-A) · 2026

[함께 읽으면 좋은 브레드보드 IO 시리즈]

[브레드보드 IO 10편] 🤖 아두이노 R4로 세우는 인버티드 펜듈럼(역진자)— 하드웨어 구성 & 프로젝트 로드맵 —

■ 프롤로그 안녕하세요, 민선생입니다. 지금까지 브레드보드 활용 IO 인터페이스 시리즈를 통해 I2C ...

blog.naver.com

[브레드보드 IO 11편]📐 인버티드 펜듈럼 (단계 1-A): MPU-6050 캘리브레이션과 정확한 각도 출력

■ 프롤로그 안녕하세요, 민선생입니다. 지난 10편에서 인버티드 펜듈럼 프로젝트의 전체 하드웨어 구성과 ...

blog.naver.com

• 12편: 인버티드 펜듈럼(단계1-A): 상보필터로 완성되는 정확한 각도제어 바로가기

[브레드보드 IO 12편] 🤖 인버티드 펜듈럼 (단계 1-B): 상보 필터(Complementary Filter)로 완성하는 정확한 각도 제어

■ 프롤로그 안녕하세요, 민선생입니다. 지난 11편에서 MPU-6050 가속도 센서만으로 각도를 출력하고 영...

blog.naver.com

[다음편]

[브레드보드 IO 14편] 🤝 인버티드 펜듈럼 [단계 2-B] MPU6050 + 스텝모터 통합! 전원 ON 20초, 자율 초기화 시스템 완성

■ 프롤로그 안녕하세요, 민선생입니다. 지난 13편(2-A)에서 모터와 기구의 한계를 실측으로 확정했습니다...

blog.naver.com

태그#인버티드펜듈럼#아두이노프로젝트#스텝모터제어#DRV8825#오토호밍#탈조방지#전자공방#하드웨어설계#공학입문#아두이노R4#상태머신#하드웨어실측#민선생의전자공방#define#include