카테고리 없음
(esp32) GY521 MPU6050, MG996R 서보 모터, LiquidCrystal display 연동
미친토끼
2025. 5. 24. 07:52
스텝 모터 연동에 비해 서보 모터 연동은 한결 간단하다. 아두이노에서 사용하던 Servo 라이브러리는 작동하지 않기 때문에 ESP32Servo 라이브러리를 설치한 다음, MPU6050에서 얻은 yaw값 (-90~90)에 90을 더해서 0~180 범주에서 정수로 출력하면 된다. 모터의 가변저항을 읽어 복잡한 설정을 할까 하다가, 관두었다. 이제 PID와 밸런싱 로봇을 공부해야겠다.
먼저 MG996R 서버 모터를 esp32에서 돌리는 테스트 코드.
#include <ESP32Servo.h>
Servo servo;
int servoPin = 4;
void setup() {
Serial.begin(115200);
servo.attach(servoPin, 360, 2600); // for MG996R initailization for complete 180 degree rotation
servo.write(90);
}
void loop() {
for (int angle = 0; angle <= 180; angle++) {
servo.write(angle);
Serial.print("angle: ");
Serial.print(angle);
delay(15);
}
delay(500);
for (int angle = 180; angle > 0; angle--) {
servo.write(angle);
Serial.print("angle: ");
Serial.print(angle);
delay(15);
}
delay(1000);
}
아래 코드는 MPU6050의 yaw, pitch, roll 값을 읽어와서 그 값을 디스플레이에 보여주며 서보 모터를 그만큼 돌린다.
필자가 커스텀한 MPU6050_setup.h를 include해야 하며, LiquidCrystal_I2C 라이브러리도 사용한다.
// for esp32
#include "MPU6050_setup.h" // always first in the headers
#include <LiquidCrystal_I2C.h>
LiquidCrystal_I2C lcd(0x27, 20, 4);
#include <ESP32Servo.h>
Servo servo;
int servoPin = 4;
int angle = 90; // default angle
void setup() {
MPU6050_setup(); // always first in the setup()
servo.attach(servoPin, 360, 2600); // for complete 180 degree ratation
servo.write(90);
delay(1000);
lcd.init();
lcd.backlight();
}
float prev_yaw = 0.0;
unsigned long t = 0;
const unsigned int checkInterval = 30;
const unsigned int ignoreChangeNoise = 30; // ignored degree change. sudden a lot degree change in a very short time can be a noise.
void loop() {
mpuDmpGet();
Serial.print("ypr\t");
float yaw = ypr[0] * 180 / M_PI; // 라디안을 360도 각도로 변환
//if (yaw < 0) yaw = 360 + yaw; // 마이너스 값이면 360을 더해서 양각으로 만듦
//갑작스런 수치 변화를 노이즈로 간주하고 무시
if (abs(yaw - prev_yaw) < ignoreChangeNoise) {
prev_yaw = yaw; // yaw값이 정상이라면 갱신
} else if (abs(yaw - prev_yaw - 360) < ignoreChangeNoise) { // 360 라인을 넘어서는 바람에 yaw값이 커진 것이라면; 반시계 방향
prev_yaw = yaw; // yaw값이 정상이라면 갱신
} else if (abs(yaw - prev_yaw + 360) < ignoreChangeNoise){ // 360 라인을 넘어서는 바람에 yaw값이 커진 것이라면; 시계 방향
prev_yaw = yaw;
} else {
yaw = prev_yaw; // 비정상적인 경우에는 갱신하지 않음, 이전값 고수
}
Serial.print(yaw);
Serial.print("\t");
float pitch = ypr[1] * 180 / M_PI; // 라디안을 360도 각도로 변환
//if (pitch < 0) pitch = 360.0 + pitch;
Serial.print(pitch);
Serial.print("\t");
float roll = ypr[2] * 180 / M_PI; // 라디안을 360도 각도로 변환
//if (roll < 0) roll = 360.0 + roll;
Serial.print(roll);
Serial.println();
if (millis() % 1000 == 0) {
lcd.clear(); // too often clear() makes us read the screen difficult
}
lcd.setCursor(0, 0);
lcd.print("yaw/pitch/roll");
lcd.setCursor(0, 1);
lcd.print(yaw);
lcd.setCursor(0, 2);
lcd.print(pitch);
lcd.setCursor(0, 3);
lcd.print(roll);
if (millis() - t > checkInterval) {
t = millis();
servo.write(round(yaw + 90));
delay(20);
}
}
// MPU6050_setup.h
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#include "Wire.h"
MPU6050 mpu;
#define INTERRUPT_PIN 23
// MPU control/status vars
bool dmpReady = false; // set true if DMP init was successful
uint8_t mpuIntStatus; // holds atctual interrupt status byte from MPU
uint8_t devStatus; // return status after each device operation (0 = success, !0 = error)
uint16_t packetSize; // expected DMP packet size (default is 42 bytes)
uint16_t fifoCount; // count of all bytes currently in FIFO
uint8_t fifoBuffer[64]; // FIFO storage buffer
// orientation/motion vars
Quaternion q; // [w, x, y, z] quanternion container
VectorFloat gravity; // [x, y, z] gravity vector
float equler[3]; // [psi, theta, phi] Euler angle container
float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector
// INTERRUPT DETECTION ROUTINE
volatile bool mpuInterrupt = false; // indicates wheter MPU interrupt pin has gone high
void dmpDataReady() {
mpuInterrupt = true;
}
// INITIAL SETUP
void MPU6050_setup() {
Wire.begin();
Wire.setClock(400000);
Serial.begin(115200);
Serial.println("Initializing I2C devices...");
mpu.initialize();
pinMode(INTERRUPT_PIN, INPUT);
Serial.println(mpu.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");
Serial.println("Initializing DMP...");
devStatus = mpu.dmpInitialize();
// supply your own gyro offsets here, scaled for min sensitivity
// mpu.setXGyroOffset(220);
// mpu.setYGyroOffset(76);
// mpu.setZGyroOffset(-85);
// mpu.setZAccelOffset(1788); // 1688 factory default for my test chip
// make sure it worked (returns 0 if so)
if (devStatus == 0) {
// Calibration Time: generate offsets and calibrate our MPU6050
mpu.CalibrateAccel(6);
mpu.CalibrateGyro(6);
mpu.PrintActiveOffsets();
// turn on the DMP, now that it's ready
Serial.println("Enabling DMP...");
mpu.setDMPEnabled(true);
// enable Arduino interrupt detection
Serial.print("Enabling interrupt detection (Arduino external interrupt ");
//Serial.print(digitalPinToInterrupt(INTERRUPT_PIN));
Serial.println(")...");
attachInterrupt(INTERRUPT_PIN, dmpDataReady, RISING);
mpuIntStatus = mpu.getIntStatus();
// set our DMP ready flag so the main loop() function knows it's okay to use it
Serial.println("DMP ready! Waiting for first interrupt...");
dmpReady = true;
// get expected DMP packet size for later comparison
packetSize = mpu.dmpGetFIFOPacketSize();
} else {
// ERROR!
// 1 = inital memory load failed
// 2 = DMP configuration updates failed
// (if it's going to break, usually the code will be 1)
Serial.print("DMP Initailization failed (code ");
Serial.print(devStatus);
Serial.println(")");
}
}
void mpuDmpGet() {
// if programming failed, don't try to do anything
if (!dmpReady) return;
// read a packet from FIFO
if (mpu.dmpGetCurrentFIFOPacket(fifoBuffer)) { // Get the Lastest Packet // uint8_t fifoBuffer[64]; // FIFO storage buffer
// display Euler angles in degress
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
}
}
https://youtube.com/shorts/l_kf0tIhh8E?si=JmYZBRUJ47Le9Yjw