MPU6050 제프 로우버그 씨의 라이브러리를 사용해서, MPU6050_DMP6 코드를 조금 수정해서 별도로 헤더 파일(MPU6050_setup.h)로 빼서 메인 코드를 단순화 함.
그리고 스탭 모터 함수를 직접 작성해서 MyStepper.h 헤드파일로 빼서 메인코드를 단순화 시킴.
이 코드에서 핵심은 6축 센서 중 yaw 값을 360도 방식으로 읽어서, 그값만큼 스탭 모터를 회전시킴.
// for esp32
#include "MPU6050_setup.h" // always first in the headers
#include <LiquidCrystal_I2C.h>
LiquidCrystal_I2C lcd(0x27, 20, 4);
#include "MyStepper.h"
#define IN1 19
#define IN2 18
#define IN3 5
#define IN4 17
MyStepper myStepper(IN1, IN2, IN3, IN4);
void setup() {
MPU6050_setup(); // always first in the setup()
lcd.init();
lcd.backlight();
}
float prev_yaw = 0.0;
unsigned long t = 0;
const unsigned int checkInterval = 30;
const unsigned int ignoreChangeNoise = 80; // 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();
myStepper.dstep_coor(round(yaw));
}
}
// MyStepper.h
#define dstep(x) dstep_coor(x) // coordinate step
#define dstep_abs(x) dstep_coor(x)
class MyStepper {
public:
MyStepper() {
IN1 = 19;
IN2 = 18;
IN3 = 5;
IN4 = 17;
pinMode(IN1, OUTPUT);
pinMode(IN2, OUTPUT);
pinMode(IN3, OUTPUT);
pinMode(IN4, OUTPUT);
}
MyStepper(uint8_t IN1, uint8_t IN2, uint8_t IN3, uint8_t IN4) {
this->IN1 = IN1;
this->IN2 = IN2;
this->IN3 = IN3;
this->IN4 = IN4;
pinMode(IN1, OUTPUT);
pinMode(IN2, OUTPUT);
pinMode(IN3, OUTPUT);
pinMode(IN4, OUTPUT);
}
uint8_t IN1;
uint8_t IN2;
uint8_t IN3;
uint8_t IN4;
const unsigned int stepsPerRevolution = 512; // 8 * 512 = 4096
const float degreePerStep = 360.0 / 512.0; // 1 step 당 이동 각도: 0.703125
int cur_pos = 0;
int prev_pos = 0;
unsigned int speed = 1000; // max: 1000
unsigned long t = 0;
bool switching_sequence[8][4] = { {0, 0, 0, 1}, {0, 0, 1, 1},
{0, 0, 1, 0}, {0, 1, 1, 0}, {0, 1, 0, 0}, {1, 1, 0, 0}, {1, 0, 0, 0}, {1, 0, 0, 1} };
void steps_8_clockwise() {
for (int i = 7; i >= 0; ) {
if (millis() - t >= round(1000.0/speed)) { // delay(1000/speed);
t = millis();
digitalWrite(IN1, switching_sequence[i][0]);
digitalWrite(IN2, switching_sequence[i][1]);
digitalWrite(IN3, switching_sequence[i][2]);
digitalWrite(IN4, switching_sequence[i][3]);
i--;
}
}
}
void steps_8_counterclockwise() {
for (int i = 0; i < 8; ) {
if (millis() - t >= 1000/speed) {
t = millis();
digitalWrite(IN1, switching_sequence[i][0]);
digitalWrite(IN2, switching_sequence[i][1]);
digitalWrite(IN3, switching_sequence[i][2]);
digitalWrite(IN4, switching_sequence[i][3]);
i++;
}
}
}
// relative dstep
void dstep_rel(int target) {
int steps_ = abs(round(target / degreePerStep));
if (target > 0) {
for (int i = 0; i < steps_; i++) {
steps_8_clockwise();
}
} else if (target < 0) {
for (int i = 0; i < steps_; i++) {
steps_8_counterclockwise();
}
}
prev_pos = cur_pos;
cur_pos = cur_pos + target;
if (cur_pos < 0) {
cur_pos += 360;
} else if (cur_pos >= 360) {
cur_pos = cur_pos % 360;
}
}
// coordinate dstep
void dstep_coor(int target) {
if (target < 0 || target >= 360) { // degree must be greater than 0
return;
} else {
if ( abs(target - cur_pos) > 180 ) {
if (target - cur_pos > 0) {
//Serial.println(target - cur_pos - 360);
dstep_rel(target - cur_pos - 360); // 시계 반대방향 cur 30 -> target 270
} else {
//Serial.println(target - cur_pos + 360);
dstep_rel(target - cur_pos + 360); // 시계 방향 cur 270 -> target 30
}
} else {
//Serial.println(target - cur_pos);
dstep_rel(target - cur_pos);
}
}
}
};// 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://www.youtube.com/shorts/tzvcGYZoEp0