[브레드보드 IO 11편]📐 아두이노로 인버티드 펜듈럼(도립진자: Inverted Pendulum) [단계 1-A]: MPU-6050

[민한식]

 민선생의 전자공방 ・ 2026. 6. 25. 14:54

URL 복사  통계 

본문 기타 기능

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

📐 인버티드 펜듈럼 — 단계 1-A
MPU-6050 캘리브레이션 & 정확한 각도 출력
— 하사점에서 정확히 0°를 출력하기까지 —

MPU-6050 · atan2 각도 · 자이로 드리프트 · 영점 캘리브레이션 · 폴링 방식
아두이노 프로젝트 · 민선생의 전자공방

■ 프롤로그

안녕하세요, 민선생입니다. 지난 10편에서 인버티드 펜듈럼 프로젝트의 전체 하드웨어 구성과 9편에 걸친 추진 로드맵을 소개했습니다. 이번 11편부터는 실제 코드를 단계별로 작성하고 검증해 나갑니다.

오늘의 목표는 인버티드 펜듈럼 제어의 첫 번째 핵심인 MPU-6050 센서에서 정확한 각도를 출력하는 것입니다. 단순히 각도를 읽는 것이 아니라, 전원을 켤 때마다 하사점(막대가 아래로 늘어진 상태)에서 정확히 0°가 출력되도록 자동 영점 조정까지 완성합니다.

접기/펴기

인버티드펜듈럼, 단계 1-A MPU6050 캘리브레이션 & 각도 출력, 비상정지 리밋스위치 동작 테스트 영상

LCD에는 모든 변화를 표시하지 않고, 각도 데이터를 4번 계산할 동안 1번 갱신하는 주기로 표시됩니다. 따라서 영상에서 LCD가 실제 움직임보다 한박자 늦게 보이는 것은 정상적인 동작입니다. 😊

MPU6050 데이터 갱신 주기 : 50ms (초당 20회)

LCD 데이터 갱신 주기 : 200ms (초당 5회)

LCD는 MPU6050이 4번 계산하는 동안 1번 갱신합니다. 따라서 영상에서 LCD가 실제 움직임보다 한박자 늦게 보이는 것은 정상적인 동작입니다.

이렇게 하는 이유는 사람이 읽을 수 있는 최소한의 속도로 제공하고, 초당 5번 갱신함으로써 I2C 통신의 불필요한 부하를 줄이기 위해서입니다.

📌 이전 편 참조

10편 인버티드 펜듈럼 하드웨어 구성 & 9편 프로젝트 로드맵

8편 MPU-6050 Roll·Pitch 자세 감지 → atan2 각도 계산, I2C 레지스터 읽기

9편 DRV8825 + 스텝모터 Non-blocking 제어 → 폴링 방식 타이머 구조 기초

1편 I2C 통신 기초 → Wire 라이브러리, beginTransmission / requestFrom

🎯 이번 편에서 배우는 것

① MPU-6050 센서의 내부 구조와 레지스터 설정 방법

② 가속도 센서로 각도를 계산하는 atan2 수식의 원리

③ 자이로 센서의 드리프트 문제와 오프셋 캘리브레이션

④ MPU-6050 부착 오차를 보정하는 각도 영점 캘리브레이션

⑤ INT 핀 없이 폴링 방식으로 데이터를 읽는 방법

⑥ 비상정지 리밋스위치 SW 디바운싱 처리

⑦ 시리얼 플로터로 각도·각속도를 실시간 확인하는 방법

1. MPU-6050 센서 기초

1-1. MPU-6050이란?

MPU-6050은 InvenSense(현 TDK)가 개발한 6축 IMU(Inertial Measurement Unit) 센서입니다. 하나의 칩 안에 3축 가속도계와 3축 자이로스코프를 내장하고 있어 물체의 기울기와 회전 속도를 동시에 측정할 수 있습니다.

내장 센서	측정 대상	이 프로젝트에서의 역할
가속도계	X·Y·Z축 선형 가속도 (중력 포함)	중력 방향으로 기울기(각도) 계산 / 장기 안정·진동에 취약
자이로스코프	X·Y·Z축 각속도 (°/s)	막대 쓰러지는 속도 측정 / 빠른 반응·시간 누적 오차(드리프트) 발생

💡 핵심 포인트

두 센서를 융합하는 이유

가속도만 사용 : 중력으로 각도는 알 수 있지만 진동·충격에 노이즈가 심함

자이로만 사용 : 각속도 적분 → 각도, 그러나 시간이 지나면 오차 누적 (드리프트)

→ 이번 11편에서는 먼저 가속도계 각도만 사용

→ 12편에서 상보필터로 융합하면 두 문제 동시 해결!

1-2. 이 프로젝트에서 Roll 값을 사용하는 이유

MPU-6050이 펜듈럼 회전축에 직접 장착된 모습

MPU-6050이 펜듈럼 회전축에 직접 장착된 모습

MPU-6050은 카트 위의 펜듈럼 회전축에 직접 장착되어 있습니다. 막대가 기울면 센서도 함께 기울기 때문에, 센서의 기울기가 곧 막대의 기울기입니다. 막대는 한 축(회전축)을 중심으로만 기울어지는데, 이 방향의 기울기가 바로 Roll 값입니다.

📐 각도 정의 (전 단계 공통 기준)

각도 정의 — 이 프로젝트 전체에서 공통 적용

하사점 (막대가 아래로 자연스럽게 늘어진 상태) = 0°

상사점 (막대가 위로 똑바로 서 있는 상태) = ±180°

오른쪽으로 기울어짐 = + 방향

왼쪽으로 기울어짐 = - 방향

Roll 계산 수식 : angle = atan2(AY, AZ) × (180 / π)

1-3. 두 가지 문제 — 드리프트와 부착 오차

문제 ① 자이로 드리프트

자이로스코프는 정지해 있어도 미세한 전기적 노이즈 때문에 0이 아닌 아주 작은 값을 출력합니다. 이것을 각속도로 착각해 적분하면 시간이 지날수록 각도 오차가 한 방향으로 계속 증가합니다. 이를 드리프트(Drift)라고 합니다.

예시 : 자이로 오프셋이 0.1°/s 라면

10초 후 → 1° 오차 / 60초 후 → 6° 오차 / 5분 후 → 30° 오차 (제어 불가!)

해결 : 전원 ON 시 정지 상태에서 500회 샘플 평균 → 오프셋 측정 → 매번 차감

문제 ② MPU-6050 부착 오차

센서를 회전축에 장착할 때 정확히 수직으로 붙지 않을 수 있습니다. 조금만 기울어도 막대가 하사점으로 늘어져 있을 때 atan2 결과가 0°가 아닌 값(예: 2.3°)이 됩니다.

예시 : 센서가 3° 기울어져 장착된 경우

하사점 실제 = 0° → 센서 출력 = -3° / 90° 기울임 실제 = 90° → 출력 = 87°

해결 : 전원 ON 시 하사점 상태에서 200회 샘플 평균 → angleOffset 저장

accelAngle = rawAngle - angleOffset → 하사점에서 정확히 0°

2. 회로 구성

2-1. 회로도

브레드보드 회로도 (Wokwi) — MPU-6050 + LCD + DRV8825 + 리밋스위치

2-2. 핀 배선표

아두이노 핀	연결 부품	설정	설명
A4 (SDA)	MPU-6050 SDA + LCD SDA	I2C 데이터	0x68·0x27 같은 버스 공유
A5 (SCL)	MPU-6050 SCL + LCD SCL	I2C 클럭	400kHz Fast Mode
D3	리밋스위치 ×2 병렬	INPUT_PULLUP 평시LOW·터치HIGH	비상정지시 디바운싱 20ms
D4	DRV8825 DIR	OUTPUT	이 단계 미사용
D5	DRV8825 STEP	OUTPUT	이 단계 미사용
D6	DRV8825 EN	OUTPUT·HIGH	모터 비활성 유지 (Active LOW →HIGH=OFF)

Arduino R4 · DRV8825 드라이버 · 브레드보드

MPU-6050 I2C 주소 설정

AD0 핀 = GND (또는 미연결) → 주소 0x68 ← 이 프로젝트 사용

AD0 핀 = 3.3V → 주소 0x69 (두 개 연결 시 구분용)

LCD 주소 0x27, MPU-6050 주소 0x68 — 서로 다르므로 같은 버스(A4/A5)에 공존 가능

Wire.setClock(400000) → Fast Mode 400kHz 설정 권장

3. 단계 1-A 코드 상세 설명

코드를 5개 부분으로 나누어 각 부분의 원리와 의미를 자세히 살펴보겠습니다.

3-1. MPU-6050 초기화 — initMPU6050()

MPU-6050은 전원을 켜면 슬립(Sleep) 모드로 시작합니다. 사용하려면 슬립 모드를 해제하고 측정 범위와 필터를 설정해야 합니다. 이 모든 것을 레지스터에 값을 쓰는 방식으로 합니다.

코드 ① — MPU-6050 초기화 (레지스터 직접 설정)

bool initMPU6050() {

// ① I2C 버스에서 MPU-6050 존재 확인 (연결 오류 조기 감지)

Wire.beginTransmission(MPU_ADDR); // 0x68

if (Wire.endTransmission() != 0) return false;

// ② 슬립 모드 해제 (레지스터 0x6B에 0x00 쓰기)

Wire.beginTransmission(MPU_ADDR);

Wire.write(0x6B); Wire.write(0x00); // SLEEP 비트 = 0

Wire.endTransmission(); delay(100);

// ③ 샘플레이트 : 1kHz / (1+7) = 125Hz (레지스터 0x19)

Wire.beginTransmission(MPU_ADDR);

Wire.write(0x19); Wire.write(0x07);

Wire.endTransmission();

// ④ DLPF 저역통과필터 = 5 → 대역폭 10Hz (진동 노이즈 억제)

Wire.beginTransmission(MPU_ADDR);

Wire.write(0x1A); Wire.write(0x05);

Wire.endTransmission();

// ⑤ 자이로 풀스케일 ±250°/s → 1LSB = 1/131 °/s (가장 민감)

Wire.beginTransmission(MPU_ADDR);

Wire.write(0x1B); Wire.write(0x00); // FS_SEL=0

Wire.endTransmission();

// ⑥ 가속도 풀스케일 ±2g → 1LSB = 1/16384 g (기본값)

Wire.beginTransmission(MPU_ADDR);

Wire.write(0x1C); Wire.write(0x00); // AFS_SEL=0

Wire.endTransmission();

delay(50);

Serial.println("MPU6050 init OK");

return true;

}

레지스터	설정값	의미
0x6B PWR_MGMT	0x00	슬립 해제 — 이것을 안 하면 센서가 동작하지 않음
0x19 SMPLRT_DIV	0x07	샘플레이트 = 1kHz/(1+7) = 125Hz — 8ms마다 새 데이터
0x1A CONFIG	0x05	DLPF 대역폭 10Hz — 진동·충격 노이즈 차단
0x1B GYRO_CONFIG	0x00	±250°/s — 가장 민감, 1LSB=1/131 °/s
0x1C ACCEL_CONFIG	0x00	±2g — 중력 측정에 충분, 1LSB=1/16384 g

3-2. 자이로 드리프트 캘리브레이션 — calibrateGyro()

전원을 켠 직후 자동으로 실행됩니다. 이 동안 기구에 절대 손을 대면 안 됩니다. LCD에 'DO NOT MOVE!!!' 경고가 표시됩니다.

코드 ② — 자이로 드리프트 오프셋 캘리브레이션

void calibrateGyro() {

lcdPrint4("=== CALIBRATING ===", "MPU6050 Gyro Offset",

"DO NOT MOVE !!!", "Please wait...");

delay(CALIB_DELAY_MS); // 2초 대기 — 사용자 손 뗄 시간

long sumGy = 0;

int16_t gy = 0;

int failCount = 0;

for (int i = 0; i < CALIB_SAMPLES; i++) { // 500회 반복

if (readGyroRaw(gy)) {

sumGy += gy; // 원시 LSB값 누적

} else {

failCount++; // 읽기 실패 카운트

}

delay(2); // 2ms 간격 = 500Hz 샘플링

}

// 500회 평균 = 정지 상태의 자이로 출력 = 드리프트 오프셋

gyroOffsetY = (float)sumGy / (float)(CALIB_SAMPLES - failCount);

// 이후 calcGyroRate()에서 매번 이 값을 차감하여 드리프트 제거

}

⚠️ 주의사항

캘리브레이션 중 움직이면 안 되는 이유

정지 상태의 자이로 출력 = 드리프트 오프셋 (예: 2.1 LSB)

움직임이 있으면 실제 각속도(예: 500 LSB)가 섞여 오프셋이 완전히 잘못됨

실패 증상 : 막대를 가만히 놔둬도 accelAngle이 천천히 흘러가는 현상

해결 : 전원 ON 후 LCD 'DO NOT MOVE!!!' 표시 동안 2초 기다리기

3-3. 각도 영점 캘리브레이션 — calibrateAngleOffset()

자이로 캘리브레이션 직후 실행됩니다. 이 단계에서는 펜듈럼이 자연스럽게 하사점(아래)에 매달려 있어야 합니다. 이 상태의 각도를 측정해 이후 모든 각도에서 차감합니다.

코드 ③ — 각도 영점 캘리브레이션

void calibrateAngleOffset() {

lcdPrint4("=== ANGLE ZERO ====", "Hang pendulum DOWN",

"DO NOT MOVE !!!", "Please wait...");

delay(CALIB_DELAY_MS); // 2초 대기

const int ANG_SAMPLES = 200;

double sumAngle = 0.0;

int failCount = 0;

for (int i = 0; i < ANG_SAMPLES; i++) {

int16_t ay = 0, az = 0;

if (readAccelRaw(ay, az)) {

// 오프셋 미적용 원시 각도 계산

float raw = atan2f((float)ay, (float)az) * (180.0f / PI);

sumAngle += raw;

} else {

failCount++;

}

delay(5); // 5ms 간격

}

// 200회 평균 = 하사점의 원시 각도 = 이것이 부착 오프셋

angleOffset = (float)(sumAngle / (double)(ANG_SAMPLES - failCount));

// 이후 calcAccelAngle()에서 rawAngle - angleOffset 수행

// → 하사점에서 accelAngle = 0.00°

}

3-4. 데이터 읽기 & 각도 계산

MPU-6050의 데이터를 읽을 때 INT 핀 인터럽트를 사용하지 않고 직접 읽는 폴링(Polling) 방식을 사용합니다. 코드가 단순하고 Wire 라이브러리 충돌 문제가 없어 안정적입니다.

코드 ④ — 데이터 읽기 & 각도·각속도 계산

// 가속도 원시값 읽기 (AY, AZ — Roll 계산에 필요한 것만)

bool readAccelRaw(int16_t &ay, int16_t &az) {

Wire.beginTransmission(MPU_ADDR);

Wire.write(0x3B); // 가속도 레지스터 시작 주소

if (Wire.endTransmission(false) != 0) return false; // repeated start

if (Wire.requestFrom(MPU_ADDR, 6, true) != 6) return false;

// 레지스터 순서 : AX_H AX_L AY_H AY_L AZ_H AZ_L

int16_t ax = (Wire.read()<<8) | Wire.read(); (void)ax; // AX 미사용

ay = (Wire.read()<<8) | Wire.read(); // AY ← Roll에 사용

az = (Wire.read()<<8) | Wire.read(); // AZ ← Roll에 사용

return true;

}

// 각도 계산 (영점 보정 + Angle Wrapping 포함)

float calcAccelAngle(int16_t ay, int16_t az) {

float raw = atan2f((float)ay, (float)az) * (180.0f / PI); // -180~+180°

float angle = raw - angleOffset; // 영점 오프셋 차감

// Angle Wrapping : 오프셋 차감 후 범위 벗어나면 보정

while (angle > 180.0f) angle -= 360.0f;

while (angle < -180.0f) angle += 360.0f;

return angle;

}

// 각속도 계산 (드리프트 오프셋 차감)

float calcGyroRate(int16_t gy) {

return ((float)gy - gyroOffsetY) / 131.0f; // LSB → °/s

}

📐 atan2 수식 원리

atan2(AY, AZ) 수식 원리

막대 수직(하사점) : AZ = +g, AY = 0 → atan2(0, g) = 0°

오른쪽 90° 기울임 : AZ = 0, AY = +g → atan2(g, 0) = +90°

상사점(뒤집음) : AZ = -g, AY = 0 → atan2(0, -g) = ±180°

atan2(Y, X)는 두 값의 비율로 정확한 사분면을 포함한 각도를 반환

결과 범위 : -π ~ +π 라디안 → ×(180/π)하면 -180° ~ +180°

3-5. 비상정지 — checkEmergency() & loop() 구조

코드 ⑤ — 비상정지 & loop() Non-blocking 구조

void checkEmergency() {

static bool lastState = LOW; // static : 함수 종료 후도 값 유지

static unsigned long lastMs = 0;

bool cur = digitalRead(PIN_LIMIT);

if (cur == HIGH && lastState == LOW) { // LOW → HIGH 상승 엣지 감지

if (millis() - lastMs > 20) { // 20ms 디바운싱

lastMs = millis(); lastState = HIGH; emergencyStop = true;

digitalWrite(PIN_EN, HIGH); // 모터 Enable 차단 (방어적)

Serial.println("!!! EMERGENCY !!!");

}

}

if (cur == LOW) lastState = LOW; // 스위치 릴리즈 감지

}

void loop() {

checkEmergency(); // [0순위] 항상 최초 실행

if (emergencyStop) return; // 비상정지 시 아무것도 안 함

unsigned long nowMs = millis();

if (nowMs - lastPrintMs >= 50) { // [1순위] 50ms마다 MPU 읽기

lastPrintMs = nowMs;

int16_t ay=0, az=0, gy=0;

if (!readAccelRaw(ay,az) || !readGyroRaw(gy)) return;

float accelAngle = calcAccelAngle(ay, az);

float gyroRate = calcGyroRate(gy);

// 시리얼 플로터 출력 (레이블:값 → 두 선 동시 표시)

Serial.print("accelAngle:"); Serial.print(accelAngle, 2);

Serial.print(" gyroRate:"); Serial.println(gyroRate, 2);

// [2순위] LCD 200ms마다 (clear() 사용 금지!)

static unsigned long lastLcd = 0;

if (nowMs - lastLcd >= 200) {

lastLcd = nowMs;

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

snprintf(b2, sizeof(b2), "Angle:%7.2f deg ", accelAngle);

snprintf(b3, sizeof(b3), "Gyro :%7.2f d/s ", gyroRate);

snprintf(b4, sizeof(b4), "AY:%-6d AZ:%-5d", ay, az);

lcd.setCursor(0,0); lcd.print("Stage 1-A Monitor ");

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

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

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

}

}

}

⚠️ 주의사항

lcd.clear()를 사용하지 않는 이유 — 탈조의 원인!

lcd.clear() 실행 시 I2C 통신으로 LCD 전체를 지움 → CPU 약 15~25ms 점유

이 단계에서는 모터가 없어 문제없어 보이지만...

스윙업 단계에서 사용하면 25ms 동안 스텝 펄스 50개가 사라짐 → 탈조!

setCursor()로 특정 위치에 덮어쓰면 깜박임도 없고 블로킹도 최소화됩니다.

접기/펴기

인버티드펜듈럼, 단계 1-A MPU6050 캘리브레이션 & 각도 출력, 비상정지 리밋스위치 동작 테스트 영상

LCD에는 모든 변화를 표시하지 않고, 각도 데이터를 4번 계산할 동안 1번 갱신하는 주기로 표시됩니다. 따라서 영상에서 LCD가 실제 움직임보다 한박자 늦게 보이는 것은 정상적인 동작입니다. 😊

MPU6050 데이터 갱신 주기 : 50ms (초당 20회)

LCD 데이터 갱신 주기 : 200ms (초당 5회)

LCD는 MPU6050이 4번 계산하는 동안 1번 갱신합니다. 따라서 영상에서 LCD가 실제 움직임보다 한박자 늦게 보이는 것은 정상적인 동작입니다.

이렇게 하는 이유는 사람이 읽을 수 있는 최소한의 속도로 제공하고, 초당 5번 갱신함으로써 I2C 통신의 불필요한 부하를 줄이기 위해서입니다.

4. 완성 코드 전체

실제 하드웨어에서 동작 확인을 마친 단계 1-A 전체 코드입니다.

/*

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

* Inverted Pendulum — 단계 1-A

* MPU6050 캘리브레이션 & 원시 각도 출력

*

* 목표

* ① 자이로 오프셋 캘리브레이션 (500회 평균)

* ② 가속도 각도 오프셋 캘리브레이션 → 하사점 = 정확히 0°

* ③ 가속도 센서 atan2 기반 각도 출력

* ④ 하사점 = 0°, 상사점 = ±180° 매핑 확인

* ⑤ 시리얼 플로터로 accelAngle / gyroRate 동시 확인

*

* 각도 영점 원리

* MPU6050 부착 위치 오차 → 하사점에서 atan2 ≠ 0°

* 캘리브 시 angleOffset = 평균(rawAngle) 저장

* 이후 : accelAngle = rawAngle - angleOffset → 하사점 = 0°

*

* 핀 배선 (확정)

* MPU6050 SDA → A4, SCL → A5

* D3 : 리밋스위치 (이 단계 비상정지만)

* D4 : DIR, D5 : STEP, D6 : EN (이 단계 미사용)

*

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

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

*/

#include <Wire.h>

#include <LiquidCrystal_I2C.h>

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

#define MPU_ADDR 0x68

#define PIN_LIMIT 3 // 리밋스위치 (INPUT_PULLUP, 평시LOW·터치HIGH)

#define PIN_EN 6 // 모터 EN (이 단계 미사용)

#define PIN_STEP 5 // 이 단계 미사용

#define PIN_DIR 4 // 이 단계 미사용

// ── MPU6050 레지스터 ────────────────────────────

#define REG_PWR_MGT 0x6B

#define REG_SMPLRT 0x19

#define REG_CONFIG 0x1A

#define REG_GYRO_CFG 0x1B

#define REG_ACCEL_CFG 0x1C

#define REG_ACCEL_OUT 0x3B

// ── 캘리브레이션 설정 ───────────────────────────

#define CALIB_SAMPLES 500 // 자이로 오프셋 샘플 수

#define CALIB_DELAY_MS 2000 // 캘리브 시작 전 대기 (손 뗄 시간)

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

LiquidCrystal_I2C lcd(0x27, 20, 4);

// ── 전역 변수 ───────────────────────────────────

float gyroOffsetY = 0.0f; // 자이로 Y축 드리프트 오프셋

float angleOffset = 0.0f; // 각도 영점 오프셋 (하사점 = 0° 보정용)

unsigned long lastPrintMs = 0;

const unsigned long PRINT_INTERVAL = 50; // 50ms = 초당 20회

bool emergencyStop = false;

// ── 함수 선언 ───────────────────────────────────

bool initMPU6050();

void calibrateGyro();

void calibrateAngleOffset();

bool readAccelRaw(int16_t &ay, int16_t &az);

bool readGyroRaw(int16_t &gy);

float calcAccelAngle(int16_t ay, int16_t az);

float calcGyroRate(int16_t gy);

void checkEmergency();

void lcdPrint2(const char* l1, const char* l2);

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

const char* l3, const char* l4);

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

// MPU6050 초기화

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

bool initMPU6050() {

Wire.beginTransmission(MPU_ADDR);

if (Wire.endTransmission() != 0) {

Serial.println("ERROR: MPU6050 not found on I2C");

return false;

}

// 슬립 모드 해제

Wire.beginTransmission(MPU_ADDR);

Wire.write(REG_PWR_MGT); Wire.write(0x00);

if (Wire.endTransmission() != 0) return false;

delay(100);

// 샘플레이트 125Hz

Wire.beginTransmission(MPU_ADDR);

Wire.write(REG_SMPLRT); Wire.write(0x07);

Wire.endTransmission();

// DLPF 대역폭 10Hz (진동 노이즈 억제)

Wire.beginTransmission(MPU_ADDR);

Wire.write(REG_CONFIG); Wire.write(0x05);

Wire.endTransmission();

// 자이로 ±250°/s (LSB = 131)

Wire.beginTransmission(MPU_ADDR);

Wire.write(REG_GYRO_CFG); Wire.write(0x00);

Wire.endTransmission();

// 가속도 ±2g (LSB = 16384)

Wire.beginTransmission(MPU_ADDR);

Wire.write(REG_ACCEL_CFG); Wire.write(0x00);

Wire.endTransmission();

delay(50);

Serial.println("MPU6050 init OK");

return true;

}

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

// 자이로 드리프트 오프셋 캘리브레이션

// ★ 실행 중 절대 움직이면 안 됨!

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

void calibrateGyro() {

lcdPrint4("=== CALIBRATING ===",

"MPU6050 Gyro Offset",

"DO NOT MOVE !!!",

"Please wait...");

Serial.println("=== Gyro Calibration Start ===");

Serial.println("DO NOT MOVE the pendulum!");

delay(CALIB_DELAY_MS);

long sumGy = 0;

int16_t gy = 0;

int fail = 0;

for (int i = 0; i < CALIB_SAMPLES; i++) {

if (readGyroRaw(gy)) sumGy += gy;

else fail++;

delay(2);

if ((i + 1) % 50 == 0) {

Serial.print("Calibrating... ");

Serial.print(i + 1); Serial.print("/");

Serial.println(CALIB_SAMPLES);

}

}

gyroOffsetY = (float)sumGy / (float)(CALIB_SAMPLES - fail);

Serial.print("Gyro Offset (raw LSB): "); Serial.println(gyroOffsetY, 4);

Serial.print("Gyro Offset (deg/s) : "); Serial.println(gyroOffsetY / 131.0f, 4);

Serial.println("=== Gyro Calib Done ===");

char buf[21];

snprintf(buf, sizeof(buf), "Offset:%.4f d/s", gyroOffsetY / 131.0f);

lcdPrint4("=== CALIB DONE ===", buf, "Starting...", "");

delay(1500);

}

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

// 가속도 각도 영점 캘리브레이션

// ★ 펜듈럼이 하사점(아래)에 자유롭게 매달린 상태여야 함

// ★ calibrateGyro() 완료 후 호출

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

void calibrateAngleOffset() {

lcdPrint4("=== ANGLE ZERO ====",

"Hang pendulum DOWN",

"DO NOT MOVE !!!",

"Please wait...");

Serial.println("=== Angle Offset Calibration ===");

Serial.println("Pendulum must hang freely at BOTTOM.");

Serial.println("DO NOT MOVE!");

delay(CALIB_DELAY_MS);

const int ANG_SAMPLES = 200;

double sumAngle = 0.0;

int fail = 0;

for (int i = 0; i < ANG_SAMPLES; i++) {

int16_t ay = 0, az = 0;

if (readAccelRaw(ay, az)) {

sumAngle += atan2f((float)ay, (float)az) * (180.0f / PI);

} else {

fail++;

}

delay(5);

}

angleOffset = (float)(sumAngle / (double)(ANG_SAMPLES - fail));

Serial.print("Angle Offset: "); Serial.print(angleOffset, 4);

Serial.println(" deg (homing offset for down=0 deg)");

Serial.println("=== Angle Offset Done ===");

char b1[21];

snprintf(b1, sizeof(b1), "Offset:%+.2f deg", angleOffset);

lcdPrint4("=== ZERO DONE ====", b1, "Down=0 Up=+-180", "Starting...");

delay(1500);

}

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

// 가속도 원시값 읽기 (폴링 방식, AY·AZ만)

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

bool readAccelRaw(int16_t &ay, int16_t &az) {

Wire.beginTransmission(MPU_ADDR);

Wire.write(REG_ACCEL_OUT);

if (Wire.endTransmission(false) != 0) return false;

if (Wire.requestFrom(MPU_ADDR, 6, true) != 6) return false;

int16_t ax = (Wire.read() << 8) | Wire.read(); (void)ax;

ay = (Wire.read() << 8) | Wire.read();

az = (Wire.read() << 8) | Wire.read();

return true;

}

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

// 자이로 원시값 읽기 (폴링 방식, GY만)

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

bool readGyroRaw(int16_t &gy) {

Wire.beginTransmission(MPU_ADDR);

Wire.write(0x43);

if (Wire.endTransmission(false) != 0) return false;

if (Wire.requestFrom(MPU_ADDR, 4, true) != 4) return false;

int16_t gx = (Wire.read() << 8) | Wire.read(); (void)gx;

gy = (Wire.read() << 8) | Wire.read();

return true;

}

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

// 가속도 → 각도 변환 (영점 보정 + Angle Wrapping)

// 하사점 = 0°, 상사점 = ±180°

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

float calcAccelAngle(int16_t ay, int16_t az) {

float raw = atan2f((float)ay, (float)az) * (180.0f / PI);

float angle = raw - angleOffset;

// Angle Wrapping : -180° ~ +180° 유지

while (angle > 180.0f) angle -= 360.0f;

while (angle < -180.0f) angle += 360.0f;

return angle;

}

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

// 자이로 → 각속도 변환 (드리프트 보정)

// ±250°/s → 1LSB = 1/131 °/s

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

float calcGyroRate(int16_t gy) {

return ((float)gy - gyroOffsetY) / 131.0f;

}

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

// 비상정지 확인 (리밋스위치 폴링 + 디바운싱 20ms)

// 평시 LOW, 터치 HIGH

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

void checkEmergency() {

static bool lastState = LOW;

static unsigned long lastMs = 0;

bool cur = digitalRead(PIN_LIMIT);

if (cur == HIGH && lastState == LOW) {

if (millis() - lastMs > 20) {

lastMs = millis();

lastState = HIGH;

emergencyStop = true;

digitalWrite(PIN_EN, HIGH); // 모터 전류 차단 (방어적)

Serial.println("!!! EMERGENCY: Limit switch triggered !!!");

lcdPrint2("!!! EMERGENCY !!!", "Limit SW triggered");

}

}

if (cur == LOW) lastState = LOW;

}

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

// LCD 헬퍼

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

void lcdPrint2(const char* l1, const char* l2) {

lcd.clear();

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

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

}

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

const char* l3, const char* l4) {

lcd.clear();

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);

}

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

// setup()

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

void setup() {

Serial.begin(115200);

Wire.begin();

Wire.setClock(400000); // I2C Fast Mode 400kHz

lcd.init();

lcd.backlight();

lcdPrint2("IP Stage 1-A", "Initializing...");

// 핀 초기화

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

pinMode(PIN_EN, OUTPUT);

pinMode(PIN_STEP, OUTPUT);

pinMode(PIN_DIR, OUTPUT);

digitalWrite(PIN_EN, HIGH); // 모터 비활성

digitalWrite(PIN_STEP, LOW);

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

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

Serial.println(" MPU6050 Calib & Angle Output");

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

// ① MPU6050 초기화

if (!initMPU6050()) {

lcdPrint2("MPU6050 ERROR!", "Check wiring A4/A5");

Serial.println("HALT: MPU6050 init failed.");

while (true) delay(1000);

}

// ② 자이로 드리프트 오프셋 캘리브레이션 (500회 평균)

calibrateGyro();

// ③ 각도 영점 캘리브레이션 (200회 평균 → 하사점 = 0°)

calibrateAngleOffset();

// 준비 완료

lcdPrint4(" Stage 1-A Monitor",

"Rotate pendulum!",

"Check SerialPlotter",

"115200bps");

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

Serial.println("Rotate pendulum by hand.");

Serial.println("Format: accelAngle:xx.xx gyroRate:xx.xx");

Serial.println("");

}

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

// loop() — Non-blocking 구조

// [0순위] 비상정지 확인

// [1순위] 50ms마다 MPU 읽기 + 시리얼 플로터 출력

// [2순위] 200ms마다 LCD 갱신 (setCursor 덮어쓰기)

// ★ delay() 없음 ★ lcd.clear() 없음

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

void loop() {

checkEmergency();

if (emergencyStop) return;

unsigned long nowMs = millis();

if (nowMs - lastPrintMs >= PRINT_INTERVAL) { // 50ms

lastPrintMs = nowMs;

int16_t ay = 0, az = 0, gy = 0;

if (!readAccelRaw(ay, az) || !readGyroRaw(gy)) {

Serial.println("WARNING: MPU6050 read failed");

return;

}

float accelAngle = calcAccelAngle(ay, az); // -180° ~ +180°

float gyroRate = calcGyroRate(gy); // °/s

// 시리얼 플로터 출력 (레이블:값 형식 → 두 선 동시 표시)

Serial.print("accelAngle:"); Serial.print(accelAngle, 2);

Serial.print(" gyroRate:"); Serial.println(gyroRate, 2);

// LCD 갱신 200ms마다 (clear() 사용 금지!)

static unsigned long lastLcdMs = 0;

if (nowMs - lastLcdMs >= 200) {

lastLcdMs = nowMs;

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

snprintf(b1, sizeof(b1), "Stage 1-A Monitor ");

snprintf(b2, sizeof(b2), "Angle:%7.2f deg ", accelAngle);

snprintf(b3, sizeof(b3), "Gyro :%7.2f d/s ", gyroRate);

snprintf(b4, sizeof(b4), "AY:%-6d AZ:%-6d", ay, az);

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

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

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

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

}

}

}

/*

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

* 검증 체크리스트

* □ 시리얼 모니터 115200bps → "MPU6050 init OK" 확인

* □ "Gyro Offset" 값 출력 확인

* □ "Angle Offset" 값 출력 확인

* □ 시리얼 플로터 → 하사점에서 accelAngle ≒ 0.00° 확인

* □ 한 바퀴 회전 → -180°~+180° 연속 출력 (튐 없음)

* □ 90° 기울임 → ±90° 근접

* □ 정지 1분 → gyroRate ±2°/s 이내

* □ 리밋스위치 터치 → "EMERGENCY" 출력 확인

* ★ 모두 통과 → 12편 단계 1-B 진행!

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

*/

5. 동작 확인 & 검증

5-1. 전원 ON 후 순서

순서	LCD 표시	시리얼 출력 내용	소요 시간
①	IP Stage 1-A Initializing...	MPU6050 init OK	약 0.2초
②	=== CALIBRATING === DO NOT MOVE!!!	=== Gyro Calibration Start === Calib 50/500 ... 500/500	약 3~4초
③	=== CALIB DONE === Offset: x.xx	Gyro Offset LSB: x.xxxx	약 1.5초
④	=== ANGLE ZERO ==== Hang pendulum DOWN	=== Angle Offset Calib === Angle Offset: x.xxxx deg	약 3초
⑤	=== ZERO DONE ==== AngleOfs:+x.xx deg	After offset: down=0, up=+-180	약 1.5초
⑥	Stage 1-A Monitor Angle: 0.xx deg Gyro: 0.xx d/s	accelAngle:0.xx gyroRate:0.xx (시리얼 플로터 파형 시작)	이후 계속

5-2. 동작 사진 & 영상 삽입 위치

접기/펴기

인버티드펜듈럼, 단계 1-A MPU6050 캘리브레이션 & 각도 출력, 비상정지 리밋스위치 동작 테스트 영상

Arduino IDE 시리얼 모니터 출력 화면

5-3. 검증 체크리스트

✅ 동작 확인 체크리스트

□ 시리얼 모니터 115200bps → 'MPU6050 init OK' 출력 확인

□ 자이로 캘리브 완료 → 'Gyro Offset LSB' 값 출력 확인

□ 각도 영점 완료 → 'Angle Offset' 값 출력 확인

□ 시리얼 플로터 열기 (Tools → Serial Plotter)

□ 펜듈럼 하사점에 가만히 두기 → accelAngle ≒ 0.00° 확인 ← 핵심!

□ 손으로 한 바퀴 천천히 회전 → -180°~+180° 연속 출력 (튐 없음)

□ gyroRate → 회전 중 변하고 정지 시 0 근접

□ 90° 기울임 → accelAngle ≒ ±90° 확인

□ 정지 1분 → gyroRate ±2°/s 이내 유지

□ 리밋스위치 누르기 → 'EMERGENCY' 출력 + LCD 변경 확인

★ 모두 통과 → 12편 단계 1-B (상보필터 + Angle Wrapping) 진행!

6. 마무리

이번 11편에서는 인버티드 펜듈럼 제어의 첫 번째 핵심인 MPU-6050 각도 출력을 완성했습니다. 단순히 센서 값을 읽는 것이 아니라 자이로 드리프트 보정과 각도 영점 보정까지 구현하여, 전원을 켤 때마다 하사점에서 정확히 0°가 출력되게 했습니다.

이 두 가지 캘리브레이션은 MPU-6050을 사용하는 모든 제어 시스템에서 필수적인 과정입니다. 드론, 로봇팔 등 어떤 프로젝트에서도 그대로 적용할 수 있습니다.

이번 편에서 완성한 것

✅ MPU-6050 레지스터 직접 설정 (Wire 라이브러리만 사용)

✅ 자이로 드리프트 오프셋 캘리브레이션 (500회 평균)

✅ 가속도 각도 영점 캘리브레이션 (200회 평균) → 하사점 = 0°

✅ atan2 기반 Roll 각도 계산 + Angle Wrapping

✅ 리밋스위치 SW 디바운싱 비상정지

✅ Non-blocking loop() 구조 (delay() 없음, lcd.clear() 없음)

✅ 시리얼 플로터 실시간 파형 확인

✅ 실제 하드웨어 동작 확인 완료!

▶ 다음 편 예고

▶ 다음 편 예고 — 12편 : 단계 1-B 상보 필터 + Angle Wrapping

이번 편은 가속도 센서만 사용했습니다.

12편에서는 자이로와 가속도를 융합하는 상보 필터를 적용합니다.

• 상보필터 : angle = 0.98×(angle + gyro×dt) + 0.02×accelAngle

• dt = micros() 실측 → 정밀한 타이밍

• 빠른 스윙업 중에도 드리프트 없는 부드러운 각도 출력

• 시리얼 플로터에서 CF 출력 vs 가속도 단독 출력 비교

민선생의 전자공방 · 브레드보드 활용 IO 인터페이스 11편 · 2026

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

• 10편: [인버티드 펜듈럼 로드맵 & 하드웨어 구성] 바로가기

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

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

blog.naver.com

태그#아두이노#아두이노R4#인버티드펜듈럼#역진자#MPU6050#자이로센서#가속도센서#캘리브레이션#영점조정#드리프트보정#논블로킹#폴링방식#스텝모터#제어공학#아두이노프로젝트#include#define 태그수정