깜지로 쓰려는 블로그

요 며칠새 블랜더도 손에 안잡히고 

센서 다루는것도 손에 안잡혀서 계속 놀고 있었다.

mpu6050 3개에서 ypr 가져오도록 하긴했는데

평평한 곳에 같은 방향으로 놓아도 yaw값이 다 다르게 나와서

mpu6050에 가속조 자이로는 없어도 지자기가 없다길래

mpu9250이라도 새로 사서 해야하나 좌절하고 있었다.

오랜만에 imu 가지고 트래킹하는걸 찾다가

내가 생각한거랑 비슷한걸 한 사람을 찾았다.

이 사람의 경우 블랜더에서 하는걸 보여주는데

계속 센서 위치를 어떻게 놓는게 좋을까 고민하던중에 보게되었다.

https://www.youtube.com/watch?v=F1IdRtIDdIs

더불어 소스코드도 같이 올려줬는데

이 친구는 ypr이 아닌 쿼터니언을 뽑아오도록 하더라

근데 찝찝한게 내 mpu6050은 같은 방향을 보도록 해도 yaw가 다르던데

쿼터니언으로 안한게 문제였을까?

https://github.com/T-K-233/Dual-MPU6050-Motion-Sync/blob/main/sensors/Arduino_dual_MPU6050_joint_capture/Arduino_dual_MPU6050_joint_capture.ino

mpu6050_dmp 코드를 보면

오일러, 쿼터니언, ypr, 가속도 등 출력해내도록 있던데

ypr이나 오일러각이랑 비슷한거 아닌가?

https://github.com/jrowberg/i2cdevlib/blob/master/Arduino/MPU6050/examples/MPU6050_DMP6/MPU6050_DMP6.ino

싶어서 구글링 좀하면서 해매다가

이글도 처음에 대충 지나가면서  이해가 안갔는데

https://taeyoung96.github.io/mathematics/Euler/

이 글이랑 영상 몇개보고

이해갔다.

roll pitch yaw는 고정된 초기 축에 따라서 회전학 각도고

오일러각은 회전하면서 회전기준축도 이동한다.

위 국내 글에 그림을 잘보면 이 내용이 있는데 제대로 안봐서 못보고 지나갓엇다.

아무튼 내 mpu6050도 쿼터니언으로 뽑으면 좀 나으려나?

안그래도 이전에 만들면서도 오프셋 고쳐야할탠대 싶었는데

위 영상 만든사람도 오프셋을 고쳐서 하라고한다.

그런데 보다보니까

내가 갖고잇는 dmp 코드랑 뭔가 조금 다르더라.

  // supply your own gyro offsets here, scaled for min sensitivity
  mpu68.setXGyroOffset(0);
  mpu68.setYGyroOffset(0);
  mpu68.setZGyroOffset(0);
  mpu68.setXAccelOffset(0);
  mpu68.setYAccelOffset(0);
  mpu68.setZAccelOffset(0);
  
  mpu69.setXGyroOffset(0);
  mpu69.setYGyroOffset(0);
  mpu69.setZGyroOffset(0);
  mpu69.setXAccelOffset(0);
  mpu69.setYAccelOffset(0);
  mpu69.setZAccelOffset(0);

  
  // Calibration Time: generate offsets and calibrate our MPU6050
  mpu68.CalibrateAccel(6);
  mpu68.CalibrateGyro(6);
  
  mpu69.CalibrateAccel(6);
  mpu69.CalibrateGyro(6);

이게 2019년도에 업데이트된 최신 코드같은데

https://github.com/jrowberg/i2cdevlib/blob/master/Arduino/MPU6050/examples/MPU6050_DMP6_using_DMP_V6.12/MPU6050_DMP6_using_DMP_V6.12.ino

내가 사용한 mpu6050 라이브러리는

2013년에 만들어진 아주 오래된 코드를 기반으로 하고있었다 --..

// I2C device class (I2Cdev) demonstration Arduino sketch for MPU6050 class using DMP (MotionApps v2.0)
// 6/21/2012 by Jeff Rowberg <jeff@rowberg.net>
// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib
//
// Changelog:
//      2013-05-08 - added seamless Fastwire support
//                 - added note about gyro calibration
//      2012-06-21 - added note about Arduino 1.0.1 + Leonardo compatibility error
//      2012-06-20 - improved FIFO overflow handling and simplified read process
//      2012-06-19 - completely rearranged DMP initialization code and simplification
//      2012-06-13 - pull gyro and accel data from FIFO packet instead of reading directly
//      2012-06-09 - fix broken FIFO read sequence and change interrupt detection to RISING
//      2012-06-05 - add gravity-compensated initial reference frame acceleration output
//                 - add 3D math helper file to DMP6 example sketch
//                 - add Euler output and Yaw/Pitch/Roll output formats
//      2012-06-04 - remove accel offset clearing for better results (thanks Sungon Lee)
//      2012-06-01 - fixed gyro sensitivity to be 2000 deg/sec instead of 250
//      2012-05-30 - basic DMP initialization working

/* ============================================

최신 코드

체인지로그

// I2C device class (I2Cdev) demonstration Arduino sketch for MPU6050 class using DMP (MotionApps v6.12)
// 6/21/2012 by Jeff Rowberg <jeff@rowberg.net>
// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib
//
// Changelog:
//      2019-07-10 - Uses the new version of the DMP Firmware V6.12
//                 - Note: I believe the Teapot demo is broken with this versin as
//                 - the fifo buffer structure has changed
//      2016-04-18 - Eliminated a potential infinite loop
//      2013-05-08 - added seamless Fastwire support
//                 - added note about gyro calibration
//      2012-06-21 - added note about Arduino 1.0.1 + Leonardo compatibility error
//      2012-06-20 - improved FIFO overflow handling and simplified read process
//      2012-06-19 - completely rearranged DMP initialization code and simplification
//      2012-06-13 - pull gyro and accel data from FIFO packet instead of reading directly
//      2012-06-09 - fix broken FIFO read sequence and change interrupt detection to RISING
//      2012-06-05 - add gravity-compensated initial reference frame acceleration output
//                 - add 3D math helper file to DMP6 example sketch
//                 - add Euler output and Yaw/Pitch/Roll output formats
//      2012-06-04 - remove accel offset clearing for better results (thanks Sungon Lee)
//      2012-06-01 - fixed gyro sensitivity to be 2000 deg/sec instead of 250
//      2012-05-30 - basic DMP initialization working

/* ============================================

3/6 작성

---------------------------------

3/12 작성

계속 개으름 피우다가 다시 시작

이 mpu6050 ypr 추출 코드 작성한 사람 깃헙을 보면

https://github.com/jrowberg/i2cdevlib/tree/master/Arduino/MPU6050

모션앱뒤에 20, 41, 612가 붙어있는데

이게 뭔가 보니

DMP 펌웨어 버전을 말하는듯하다.

https://invensense.tdk.com/products/motion-tracking/6-axis/mpu-6050/

// I2C device class (I2Cdev) demonstration Arduino sketch for MPU6050 class using DMP (MotionApps v6.12)
// 6/21/2012 by Jeff Rowberg <jeff@rowberg.net>
// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib
//
// Changelog:
//      2019-07-10 - Uses the new version of the DMP Firmware V6.12
//                 - Note: I believe the Teapot demo is broken with this versin as
//                 - the fifo buffer structure has changed

일단 이 예제코드로 동작시켜봄

https://github.com/jrowberg/i2cdevlib/blob/master/Arduino/MPU6050/examples/MPU6050_DMP6_using_DMP_V6.12/MPU6050_DMP6_using_DMP_V6.12.ino

#include "I2Cdev.h"

#include "MPU6050_6Axis_MotionApps612.h"
//#include "MPU6050.h" // not necessary if using MotionApps include file

#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include "Wire.h"
#endif


MPU6050 mpu;

#define OUTPUT_READABLE_YAWPITCHROLL



#define INTERRUPT_PIN 2  // use pin 2 on Arduino Uno & most boards
#define LED_PIN 13 // (Arduino is 13, Teensy is 11, Teensy++ is 6)
bool blinkState = false;

// MPU control/status vars
bool dmpReady = false;  // set true if DMP init was successful
uint8_t mpuIntStatus;   // holds actual 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]         quaternion container
VectorInt16 aa;         // [x, y, z]            accel sensor measurements
VectorInt16 gy;         // [x, y, z]            gyro sensor measurements
VectorInt16 aaReal;     // [x, y, z]            gravity-free accel sensor measurements
VectorInt16 aaWorld;    // [x, y, z]            world-frame accel sensor measurements
VectorFloat gravity;    // [x, y, z]            gravity vector
float euler[3];         // [psi, theta, phi]    Euler angle container
float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector

// packet structure for InvenSense teapot demo
uint8_t teapotPacket[14] = { '$', 0x02, 0, 0, 0, 0, 0, 0, 0, 0, 0x00, 0x00, '\r', '\n' };



// ================================================================
// ===               INTERRUPT DETECTION ROUTINE                ===
// ================================================================

volatile bool mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high
void dmpDataReady() {
  mpuInterrupt = true;
}



// ================================================================
// ===                      INITIAL SETUP                       ===
// ================================================================

void setup() {
  // join I2C bus (I2Cdev library doesn't do this automatically)
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
  Wire.begin();
  Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having compilation difficulties
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
  Fastwire::setup(400, true);
#endif

  // initialize serial communication
  // (115200 chosen because it is required for Teapot Demo output, but it's
  // really up to you depending on your project)
  Serial.begin(115200);
  while (!Serial); // wait for Leonardo enumeration, others continue immediately

  // NOTE: 8MHz or slower host processors, like the Teensy @ 3.3V or Arduino
  // Pro Mini running at 3.3V, cannot handle this baud rate reliably due to
  // the baud timing being too misaligned with processor ticks. You must use
  // 38400 or slower in these cases, or use some kind of external separate
  // crystal solution for the UART timer.

  // initialize device
  Serial.println(F("Initializing I2C devices..."));
  mpu.initialize();
  pinMode(INTERRUPT_PIN, INPUT);

  // verify connection
  Serial.println(F("Testing device connections..."));
  Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));

  // wait for ready
  Serial.println(F("\nSend any character to begin DMP programming and demo: "));
  while (Serial.available() && Serial.read()); // empty buffer
  while (!Serial.available());                 // wait for data
  while (Serial.available() && Serial.read()); // empty buffer again

  // load and configure the DMP
  Serial.println(F("Initializing DMP..."));
  devStatus = mpu.dmpInitialize();

  // supply your own gyro offsets here, scaled for min sensitivity
  mpu.setXGyroOffset(51);
  mpu.setYGyroOffset(8);
  mpu.setZGyroOffset(21);
  mpu.setXAccelOffset(1150);
  mpu.setYAccelOffset(-50);
  mpu.setZAccelOffset(1060);
  // 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);
    Serial.println();
    mpu.PrintActiveOffsets();
    // turn on the DMP, now that it's ready
    Serial.println(F("Enabling DMP..."));
    mpu.setDMPEnabled(true);

    // enable Arduino interrupt detection
    Serial.print(F("Enabling interrupt detection (Arduino external interrupt "));
    Serial.print(digitalPinToInterrupt(INTERRUPT_PIN));
    Serial.println(F(")..."));
    attachInterrupt(digitalPinToInterrupt(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(F("DMP ready! Waiting for first interrupt..."));
    dmpReady = true;

    // get expected DMP packet size for later comparison
    packetSize = mpu.dmpGetFIFOPacketSize();
  } else {
    // ERROR!
    // 1 = initial memory load failed
    // 2 = DMP configuration updates failed
    // (if it's going to break, usually the code will be 1)
    Serial.print(F("DMP Initialization failed (code "));
    Serial.print(devStatus);
    Serial.println(F(")"));
  }

  // configure LED for output
  pinMode(LED_PIN, OUTPUT);
}



// ================================================================
// ===                    MAIN PROGRAM LOOP                     ===
// ================================================================

void loop() {
  // if programming failed, don't try to do anything
  if (!dmpReady) return;
  // read a packet from FIFO
  if (mpu.dmpGetCurrentFIFOPacket(fifoBuffer)) { // Get the Latest packet 

#ifdef OUTPUT_READABLE_QUATERNION
    // display quaternion values in easy matrix form: w x y z
    mpu.dmpGetQuaternion(&q, fifoBuffer);
    Serial.print("quat\t");
    Serial.print(q.w);
    Serial.print("\t");
    Serial.print(q.x);
    Serial.print("\t");
    Serial.print(q.y);
    Serial.print("\t");
    Serial.println(q.z);
#endif

#ifdef OUTPUT_READABLE_EULER
    // display Euler angles in degrees
    mpu.dmpGetQuaternion(&q, fifoBuffer);
    mpu.dmpGetEuler(euler, &q);
    Serial.print("euler\t");
    Serial.print(euler[0] * 180 / M_PI);
    Serial.print("\t");
    Serial.print(euler[1] * 180 / M_PI);
    Serial.print("\t");
    Serial.println(euler[2] * 180 / M_PI);
#endif

#ifdef OUTPUT_READABLE_YAWPITCHROLL
    // display Euler angles in degrees
    mpu.dmpGetQuaternion(&q, fifoBuffer);
    mpu.dmpGetGravity(&gravity, &q);
    mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
    Serial.print("ypr\t");
    Serial.print(ypr[0] * 180 / M_PI);
    Serial.print("\t");
    Serial.print(ypr[1] * 180 / M_PI);
    Serial.print("\t");
    Serial.print(ypr[2] * 180 / M_PI);
    /*
      mpu.dmpGetAccel(&aa, fifoBuffer);
      Serial.print("\tRaw Accl XYZ\t");
      Serial.print(aa.x);
      Serial.print("\t");
      Serial.print(aa.y);
      Serial.print("\t");
      Serial.print(aa.z);
      mpu.dmpGetGyro(&gy, fifoBuffer);
      Serial.print("\tRaw Gyro XYZ\t");
      Serial.print(gy.x);
      Serial.print("\t");
      Serial.print(gy.y);
      Serial.print("\t");
      Serial.print(gy.z);
    */
    Serial.println();

#endif

#ifdef OUTPUT_READABLE_REALACCEL
    // display real acceleration, adjusted to remove gravity
    mpu.dmpGetQuaternion(&q, fifoBuffer);
    mpu.dmpGetAccel(&aa, fifoBuffer);
    mpu.dmpGetGravity(&gravity, &q);
    mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
    Serial.print("areal\t");
    Serial.print(aaReal.x);
    Serial.print("\t");
    Serial.print(aaReal.y);
    Serial.print("\t");
    Serial.println(aaReal.z);
#endif

#ifdef OUTPUT_READABLE_WORLDACCEL
    // display initial world-frame acceleration, adjusted to remove gravity
    // and rotated based on known orientation from quaternion
    mpu.dmpGetQuaternion(&q, fifoBuffer);
    mpu.dmpGetAccel(&aa, fifoBuffer);
    mpu.dmpGetGravity(&gravity, &q);
    mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
    mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q);
    Serial.print("aworld\t");
    Serial.print(aaWorld.x);
    Serial.print("\t");
    Serial.print(aaWorld.y);
    Serial.print("\t");
    Serial.println(aaWorld.z);
#endif

#ifdef OUTPUT_TEAPOT
    // display quaternion values in InvenSense Teapot demo format:
    teapotPacket[2] = fifoBuffer[0];
    teapotPacket[3] = fifoBuffer[1];
    teapotPacket[4] = fifoBuffer[4];
    teapotPacket[5] = fifoBuffer[5];
    teapotPacket[6] = fifoBuffer[8];
    teapotPacket[7] = fifoBuffer[9];
    teapotPacket[8] = fifoBuffer[12];
    teapotPacket[9] = fifoBuffer[13];
    Serial.write(teapotPacket, 14);
    teapotPacket[11]++; // packetCount, loops at 0xFF on purpose
#endif

    // blink LED to indicate activity
    blinkState = !blinkState;
    digitalWrite(LED_PIN, blinkState);
  }
}

잘 나오기는 한데 euler에서는 중력을 안쓰고 ypr 계산시에 중력을 사용하는 이유가 뭘까

실제 코드 dmpgetypr 들어가면 gravity 사용해서 계산하긴한다.

#ifdef OUTPUT_READABLE_QUATERNION
    // display quaternion values in easy matrix form: w x y z
    mpu.dmpGetQuaternion(&q, fifoBuffer);
    Serial.print("quat\t");
    Serial.print(q.w);
    Serial.print("\t");
    Serial.print(q.x);
    Serial.print("\t");
    Serial.print(q.y);
    Serial.print("\t");
    Serial.println(q.z);
#endif

#ifdef OUTPUT_READABLE_EULER
    // display Euler angles in degrees
    mpu.dmpGetQuaternion(&q, fifoBuffer);
    mpu.dmpGetEuler(euler, &q);
    Serial.print("euler\t");
    Serial.print(euler[0] * 180 / M_PI);
    Serial.print("\t");
    Serial.print(euler[1] * 180 / M_PI);
    Serial.print("\t");
    Serial.println(euler[2] * 180 / M_PI);
#endif

#ifdef OUTPUT_READABLE_YAWPITCHROLL
    // display Euler angles in degrees
    mpu.dmpGetQuaternion(&q, fifoBuffer);
    mpu.dmpGetGravity(&gravity, &q);
    mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);

uint8_t MPU6050::dmpGetQuaternion(int16_t *data, const uint8_t* packet) {
    // TODO: accommodate different arrangements of sent data (ONLY default supported now)
    if (packet == 0) packet = dmpPacketBuffer;
    data[0] = ((packet[0] << 8) | packet[1]);
    data[1] = ((packet[4] << 8) | packet[5]);
    data[2] = ((packet[8] << 8) | packet[9]);
    data[3] = ((packet[12] << 8) | packet[13]);
    return 0;
}
uint8_t MPU6050::dmpGetQuaternion(Quaternion *q, const uint8_t* packet) {
    // TODO: accommodate different arrangements of sent data (ONLY default supported now)
    int16_t qI[4];
    uint8_t status = dmpGetQuaternion(qI, packet);
    if (status == 0) {
        q -> w = (float)qI[0] / 16384.0f;
        q -> x = (float)qI[1] / 16384.0f;
        q -> y = (float)qI[2] / 16384.0f;
        q -> z = (float)qI[3] / 16384.0f;
        return 0;
    }
    return status; // int16 return value, indicates error if this line is reached
}


uint8_t MPU6050::dmpGetEuler(float *data, Quaternion *q) {
    data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1);   // psi
    data[1] = -asin(2*q -> x*q -> z + 2*q -> w*q -> y);                              // theta
    data[2] = atan2(2*q -> y*q -> z - 2*q -> w*q -> x, 2*q -> w*q -> w + 2*q -> z*q -> z - 1);   // phi
    return 0;
}

#ifdef USE_OLD_DMPGETYAWPITCHROLL
uint8_t MPU6050::dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity) {
    // yaw: (about Z axis)
    data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1);
    // pitch: (nose up/down, about Y axis)
    data[1] = atan(gravity -> x / sqrt(gravity -> y*gravity -> y + gravity -> z*gravity -> z));
    // roll: (tilt left/right, about X axis)
    data[2] = atan(gravity -> y / sqrt(gravity -> x*gravity -> x + gravity -> z*gravity -> z));
    return 0;
}
#else 
uint8_t MPU6050::dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity) {
    // yaw: (about Z axis)
    data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1);
    // pitch: (nose up/down, about Y axis)
    data[1] = atan2(gravity -> x , sqrt(gravity -> y*gravity -> y + gravity -> z*gravity -> z));
    // roll: (tilt left/right, about X axis)
    data[2] = atan2(gravity -> y , gravity -> z);
    if (gravity -> z < 0) {
        if(data[1] > 0) {
            data[1] = PI - data[1]; 
        } else { 
            data[1] = -PI - data[1];
        }
    }
    return 0;
}
#endif

위 계산에 대한 해설은 다음 링크에 있다.

https://oduerr.github.io/gesture/ypr_calculations.html

ypr 계산시 왜 gravity 백터를 사용하는지 궁금해서 구글링하다

국내에 mpu 사용한 글들 몇가지 찾아봄

https://blog.naver.com/ysahn2k/221410391235

https://m.blog.naver.com/yuninjae1234/220935189584

ypr 대신 그냥 쿼터니언과 오일러만 출력시켜보니

초기 시점 기준으로 회전한 만큼 각이 바뀐다.

중력벡터를 사용하면서 실제 중력 기준으로 각도를 변환해주는 거구나.

중력벡터를 안쓰고 하면 초기 위치 기준으로 값들이 출력된다.

왜 맨 위에 블렌더에서 실험한 사람은 yaw pitch roll을 안쓰고 바로 쿼터니언을 사용한것인지 이제 이해가 간다.

아래는 필요없는 부분 제거해서 좀더 간략하게 줄인 코드

#include "I2Cdev.h"

#include "MPU6050_6Axis_MotionApps612.h"
//#include "MPU6050.h" // not necessary if using MotionApps include file

#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include "Wire.h"
#endif


MPU6050 mpu;

//#define OUTPUT_READABLE_YAWPITCHROLL
#define OUTPUT_READABLE_EULER

// MPU control/status vars
bool dmpReady = false;  // set true if DMP init was successful
uint8_t mpuIntStatus;   // holds actual 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]         quaternion container
VectorInt16 aa;         // [x, y, z]            accel sensor measurements
VectorInt16 gy;         // [x, y, z]            gyro sensor measurements
VectorInt16 aaReal;     // [x, y, z]            gravity-free accel sensor measurements
VectorInt16 aaWorld;    // [x, y, z]            world-frame accel sensor measurements
VectorFloat gravity;    // [x, y, z]            gravity vector
float euler[3];         // [psi, theta, phi]    Euler angle container
float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector


// ================================================================
// ===                      INITIAL SETUP                       ===
// ================================================================

void setup() {


#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
  Wire.begin();
  Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having compilation difficulties
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
  Fastwire::setup(400, true);
#endif

  Serial.begin(115200);
  while (!Serial); // wait for Leonardo enumeration, others continue immediately


  // initialize device
  Serial.println(F("Initializing I2C devices..."));
  mpu.initialize();

  // verify connection
  Serial.println(F("Testing device connections..."));
  Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));

  // load and configure the DMP
  Serial.println(F("Initializing DMP..."));
  devStatus = mpu.dmpInitialize();

  // supply your own gyro offsets here, scaled for min sensitivity
  mpu.setXGyroOffset(51);
  mpu.setYGyroOffset(8);
  mpu.setZGyroOffset(21);
  mpu.setXAccelOffset(1150);
  mpu.setYAccelOffset(-50);
  mpu.setZAccelOffset(1060);
  // 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);
    Serial.println();
    mpu.PrintActiveOffsets();
    // turn on the DMP, now that it's ready
    Serial.println(F("Enabling DMP..."));
    mpu.setDMPEnabled(true);

    mpuIntStatus = mpu.getIntStatus();

    // set our DMP Ready flag so the main loop() function knows it's okay to use it
    Serial.println(F("DMP ready! Waiting for first interrupt..."));
    dmpReady = true;

    // get expected DMP packet size for later comparison
    packetSize = mpu.dmpGetFIFOPacketSize();
  } else {
    Serial.print(F("DMP Initialization failed (code "));
    Serial.print(devStatus);
    Serial.println(F(")"));
  }

}



// ================================================================
// ===                    MAIN PROGRAM LOOP                     ===
// ================================================================

void loop() {
  // if programming failed, don't try to do anything
  if (!dmpReady) return;
  // read a packet from FIFO
  if (mpu.dmpGetCurrentFIFOPacket(fifoBuffer)) { // Get the Latest packet 

#ifdef OUTPUT_READABLE_QUATERNION
    // display quaternion values in easy matrix form: w x y z
    mpu.dmpGetQuaternion(&q, fifoBuffer);
    Serial.print("quat\t");
    Serial.print(q.w);
    Serial.print("\t");
    Serial.print(q.x);
    Serial.print("\t");
    Serial.print(q.y);
    Serial.print("\t");
    Serial.println(q.z);
#endif

#ifdef OUTPUT_READABLE_EULER
    // display Euler angles in degrees
    mpu.dmpGetQuaternion(&q, fifoBuffer);
    mpu.dmpGetEuler(euler, &q);
    Serial.print("euler\t");
    Serial.print(euler[0] * 180 / M_PI);
    Serial.print("\t");
    Serial.print(euler[1] * 180 / M_PI);
    Serial.print("\t");
    Serial.println(euler[2] * 180 / M_PI);
#endif

#ifdef OUTPUT_READABLE_YAWPITCHROLL
    // display Euler angles in degrees
    mpu.dmpGetQuaternion(&q, fifoBuffer);
    mpu.dmpGetGravity(&gravity, &q);
    mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
    Serial.print("ypr\t");
    Serial.print(ypr[0] * 180 / M_PI);
    Serial.print("\t");
    Serial.print(ypr[1] * 180 / M_PI);
    Serial.print("\t");
    Serial.print(ypr[2] * 180 / M_PI);
    Serial.println();

#endif
  }
}

지난글에 esp32 mpu6050 보조베터리 블루투스로 

ypr 여러개 보내도록 했고

언리얼에서 동작시키게 하려하는데 계속 개으름피우고 놀고 있었다.

근데 데이터 가져와서 쓰자니

어떻게 분할시킬까가 고민이었는데

구분해서 보려고 대충

핀:yrp/핀:ypr/핀:ypr  \n

이런식으로 출력되게 만들었지만

언리얼에서 구분하기 귀찬을거같아서

ypr:ypr:ypr/

각 mpy간에는 :로 구분하고

다음 데이터와는 /로 구분하는 식으로수정해서 다루려고함.

esp32 업로드 코드

마지막에 있는 구분자 코드만 수정

#include "I2Cdev.h"
#include <map>

#include "MPU6050_6Axis_MotionApps20.h"
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
    #include "Wire.h"
#endif

MPU6050 mpu;

#define OUTPUT_READABLE_YAWPITCHROLL

// MPU control/status vars
bool dmpReady = false;  // set true if DMP init was successful
uint8_t mpuIntStatus;   // holds actual 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]         quaternion container
VectorInt16 aa;         // [x, y, z]            accel sensor measurements
VectorInt16 aaReal;     // [x, y, z]            gravity-free accel sensor measurements
VectorInt16 aaWorld;    // [x, y, z]            world-frame accel sensor measurements
VectorFloat gravity;    // [x, y, z]            gravity vector
float euler[3];         // [psi, theta, phi]    Euler angle container
float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector

volatile bool mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high
void dmpDataReady() {
    mpuInterrupt = true;
}


#include "BluetoothSerial.h"
String device_name = "ESP32-BT-Slave";

// Check if Bluetooth is available
#if !defined(CONFIG_BT_ENABLED) || !defined(CONFIG_BLUEDROID_ENABLED)
  #error Bluetooth is not enabled! Please run `make menuconfig` to and enable it
#endif

// Check Serial Port Profile
#if !defined(CONFIG_BT_SPP_ENABLED)
  #error Serial Port Profile for Bluetooth is not available or not enabled. It is only available for the ESP32 chip.
#endif

BluetoothSerial BTSerial;
int mpuPinNums[] = {32, 33, 25};
int mpuNum = sizeof(mpuPinNums) / sizeof(int);

struct YPR
{
    int ypr[3];
};
std::map<int, YPR> yprMap;


void InitMPU(int pinNum);
void GetData(int pinNum);
void SendData();

void setup() {
  //Serial.begin(115200);
  BTSerial.begin(device_name); //Bluetooth device name

  YPR zeroYpr = {0, 0, 0};
  for(int i = 0; i < mpuNum; i++)
    yprMap.insert(std::pair<int,YPR>(mpuPinNums[i], zeroYpr));

  BTSerial.print("start init mpuNum : ");
  BTSerial.println(mpuNum);

  for (int i = 0; i < mpuNum;i++)
  {
    BTSerial.print("set pin output : ");
    BTSerial.println(mpuPinNums[i]);
    pinMode(mpuPinNums[i], OUTPUT);

  }

  // join I2C bus (I2Cdev library doesn't do this automatically)
  #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
      Wire.begin();
      Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having compilation difficulties
  #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
      Fastwire::setup(400, true);
  #endif

  for (int i = 0; i < mpuNum;i++)
    InitMPU(mpuPinNums[i]);
}

void loop() {
  for (int i = 0; i < mpuNum;i++)
    GetData(mpuPinNums[i]);

  SendData();
}




void InitMPU(int pinNum)
{
  for (int i = 0; i < mpuNum; i++)
  {
    if (mpuPinNums[i] == pinNum)
      digitalWrite(mpuPinNums[i], LOW);  //set selected mpu6050 addr 0x68
    else
      digitalWrite(mpuPinNums[i], HIGH);  //set other mpu6050 addr 0x69
  }

  // initialize device
  mpu.initialize();

  // verify connection
  BTSerial.println(F("Testing device connections..."));
  BTSerial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));

  // load and configure the DMP
  BTSerial.println(F("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) {
      // turn on the DMP, now that it's ready
      BTSerial.println(F("Enabling DMP..."));
      mpu.setDMPEnabled(true);

      // enable Arduino interrupt detection
      BTSerial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
      attachInterrupt(0, dmpDataReady, RISING);
      //attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), dmpDataReady, RISING);

      mpuIntStatus = mpu.getIntStatus();

      // set our DMP Ready flag so the main loop() function knows it's okay to use it
      BTSerial.println(F("DMP ready! Waiting for first interrupt..."));
      dmpReady = true;

      // get expected DMP packet size for later comparison
      packetSize = mpu.dmpGetFIFOPacketSize();
  } else {
      BTSerial.print(F("DMP Initialization failed (code "));
      BTSerial.print(devStatus);
      BTSerial.println(F(")"));
  }
}


void GetData(int pinNum)
{
  for (int i = 0; i < mpuNum; i++)
  {
    if (mpuPinNums[i] == pinNum)
      digitalWrite(mpuPinNums[i], LOW);  //set selected mpu6050 addr 0x68
    else
      digitalWrite(mpuPinNums[i], HIGH);  //set other mpu6050 addr 0x69
  }

  // if programming failed, don't try to do anything
  if (!dmpReady) return;

  mpuIntStatus = mpu.getIntStatus();

  // get current FIFO count
  fifoCount = mpu.getFIFOCount();

  // check for overflow (this should never happen unless our code is too inefficient)
  if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
      // reset so we can continue cleanly
      mpu.resetFIFO();
  // otherwise, check for DMP data ready interrupt (this should happen frequently)
  } else if (mpuIntStatus & 0x02) {
    // wait for correct available data length, should be a VERY short wait
    while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();

    // read a packet from FIFO
    mpu.getFIFOBytes(fifoBuffer, packetSize);
    
    // track FIFO count here in case there is > 1 packet available
    fifoCount -= packetSize;

    #ifdef OUTPUT_READABLE_YAWPITCHROLL
      // display Euler angles in degrees
      mpu.dmpGetQuaternion(&q, fifoBuffer);
      mpu.dmpGetGravity(&gravity, &q);
      mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
      
      YPR tmpYpr;
      tmpYpr.ypr[0] = int(ypr[0] * 180/M_PI);
      tmpYpr.ypr[1] = int(ypr[1] * 180/M_PI);
      tmpYpr.ypr[2] = int(ypr[2] * 180/M_PI);

      yprMap[pinNum] = tmpYpr;
    #endif
  }
}


void SendData()
{
  for (int i = 0; i < mpuNum; i++)
  {
    BTSerial.print(yprMap[mpuPinNums[i]].ypr[0]);
    BTSerial.print(",");
    BTSerial.print(yprMap[mpuPinNums[i]].ypr[1]);
    BTSerial.print(",");
    BTSerial.print(yprMap[mpuPinNums[i]].ypr[2]);
    if(i == mpuNum - 1)
      BTSerial.print("/");
    else
      BTSerial.print(":");
  }
}

생각한데로 잘 나오긴하는듯하다 

언리얼로 ㄱㄱ

이전에 언리얼 제어한다고 썻던 블루프린트 파일인데

맨 위에잇는 set actor rotation으로 액터 회전을 시켰엇다.

지금은 mpu6050 3개로 3개의 ypr 데이터를 동시에 가져오고 있으니

따로 회전시키려고

bp_model 3개를 만들어서 다루려고함.

블루투스 시리얼 데이터를

/로 구분시켜놨는데

ypr 데이터가 3개가 있어 길어졋으므로

이전에는 30개씩 잘라썻다면 중간에 어중간하게 잘려서 잘못 만들어질수 있으니

이번엔 길이를 넉넉하게 170씩 자르도록 수정

일단 firstyprstring을 그대로 출력시키면

값이 잘 나오긴한다.

* esp32를 쓰면서 com포트는 12, 보드레이트는 115200으로 변경

초기화 루틴에서

actor 가져오도록하고

YPR 문자열 가져온뒤에

:로 분할해서

액터 로테이션 설정해주도록 수정하면

보드레이트를 115200으로 해서그런가

빠르게 동작해서 그런지 보간을 안줘도 꽤 부드럽게 회전한다

이전 글에서 

MPU6050 YPR 가져와서 블루투스로 통신하는걸 했는데

우노보드때와 다르게

보조베터리 전원을 사용해도 정상적으로 데이터 송신하는걸 확인할수 있었다.

이번 글을 작성하면서 MPU6050 여러개 연결해서 해보려고하는데

AD0로 주소를 두가지만 선택할수 있다보니 이전에 사용한 방법을 그대로하려고한다.

아래 핀맵을 보면

GPIO 번호랑 흰색 박스에 번호가 따로있어서

GPIO 번호를 쓰면 되는지 햇갈리는데

다음 링크를 참고해보니 GPIO 번호를 사용해보면 될듯하다.

https://blog.naver.com/mapes_khkim/221905932214

근데 ESP32 38핀 짜리 GPIO는 번호가 순서대로 가느네 아니라 뒤죽박죽 되있어서 좀햇갈리긴한다.

아쉬운데로 파워포인트로 대충 선 그렸는데

이런 느낌으로 배선하고

(mpu6050 vcc, gnd 제외)

이전 코드 참고해서 만들었는데

#include "I2Cdev.h"

#include "MPU6050_6Axis_MotionApps20.h"
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
    #include "Wire.h"
#endif

// class default I2C address is 0x68
// specific I2C addresses may be passed as a parameter here
// AD0 low = 0x68 (default for SparkFun breakout and InvenSense evaluation board)
// AD0 high = 0x69
MPU6050 mpu;
//MPU6050 mpu(0x69); // <-- use for AD0 high

#define OUTPUT_READABLE_YAWPITCHROLL

// MPU control/status vars
bool dmpReady = false;  // set true if DMP init was successful
uint8_t mpuIntStatus;   // holds actual 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]         quaternion container
VectorInt16 aa;         // [x, y, z]            accel sensor measurements
VectorInt16 aaReal;     // [x, y, z]            gravity-free accel sensor measurements
VectorInt16 aaWorld;    // [x, y, z]            world-frame accel sensor measurements
VectorFloat gravity;    // [x, y, z]            gravity vector
float euler[3];         // [psi, theta, phi]    Euler angle container
float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector

volatile bool mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high
void dmpDataReady() {
    mpuInterrupt = true;
}


#include "BluetoothSerial.h"
String device_name = "ESP32-BT-Slave";

// Check if Bluetooth is available
#if !defined(CONFIG_BT_ENABLED) || !defined(CONFIG_BLUEDROID_ENABLED)
  #error Bluetooth is not enabled! Please run `make menuconfig` to and enable it
#endif

// Check Serial Port Profile
#if !defined(CONFIG_BT_SPP_ENABLED)
  #error Serial Port Profile for Bluetooth is not available or not enabled. It is only available for the ESP32 chip.
#endif

BluetoothSerial BTSerial;
int mpuPinNums[] = {36, 39, 34};
int mpuNum = sizeof(mpuPinNums) / sizeof(int);

void InitMPU(int pinNum);
void SendData(int pinNum);


void setup() {
  Serial.begin(115200);
  BTSerial.begin(device_name); //Bluetooth device name

  for (int i = 0; i < mpuNum;i++)
    pinMode(mpuPinNums[i], OUTPUT);

  // join I2C bus (I2Cdev library doesn't do this automatically)
  #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
      Wire.begin();
      Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having compilation difficulties
  #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
      Fastwire::setup(400, true);
  #endif

  for (int i = 0; i < mpuNum;i++)
    InitMPU(mpuPinNums[i]);
}

void loop() {
  for (int i = 0; i < mpuNum;i++)
    SendData(mpuPinNums[i]);

  Serial.println("");


}




void InitMPU(int pinNum)
{
  for (int i = 0; i < mpuNum; i++)
  {
    if (mpuPinNums[i] == pinNum)
      digitalWrite(mpuPinNums[i], LOW);  //set selected mpu6050 addr 0x68
    else
      digitalWrite(mpuPinNums[i], HIGH);  //set other mpu6050 addr 0x69
  }
  Serial.print("start init pin ");
  Serial.println(pinNum);

  // initialize device
  mpu.initialize();

  // verify connection
  Serial.println(F("Testing device connections..."));
  Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));

  // load and configure the DMP
  Serial.println(F("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) {
      // turn on the DMP, now that it's ready
      Serial.println(F("Enabling DMP..."));
      mpu.setDMPEnabled(true);

      // enable Arduino interrupt detection
      Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
      attachInterrupt(0, dmpDataReady, RISING);
      //attachInterrupt(digitalPinToInterrupt(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(F("DMP ready! Waiting for first interrupt..."));
      dmpReady = true;

      // get expected DMP packet size for later comparison
      packetSize = mpu.dmpGetFIFOPacketSize();
  } else {
      Serial.print(F("DMP Initialization failed (code "));
      Serial.print(devStatus);
      Serial.println(F(")"));
  }
}


void SendData(int pinNum)
{
  for (int i = 0; i < mpuNum; i++)
  {
    if (mpuPinNums[i] == pinNum)
      digitalWrite(mpuPinNums[i], LOW);  //set selected mpu6050 addr 0x68
    else
      digitalWrite(mpuPinNums[i], HIGH);  //set other mpu6050 addr 0x69
  }

  // if programming failed, don't try to do anything
  if (!dmpReady) return;

  mpuIntStatus = mpu.getIntStatus();

  // get current FIFO count
  fifoCount = mpu.getFIFOCount();

  // check for overflow (this should never happen unless our code is too inefficient)
  if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
      // reset so we can continue cleanly
      mpu.resetFIFO();
  // otherwise, check for DMP data ready interrupt (this should happen frequently)
  } else if (mpuIntStatus & 0x02) {
    // wait for correct available data length, should be a VERY short wait
    while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();

    // read a packet from FIFO
    mpu.getFIFOBytes(fifoBuffer, packetSize);
    
    // track FIFO count here in case there is > 1 packet available
    fifoCount -= packetSize;

    #ifdef OUTPUT_READABLE_YAWPITCHROLL
        // display Euler angles in degrees
        mpu.dmpGetQuaternion(&q, fifoBuffer);
        mpu.dmpGetGravity(&gravity, &q);
        mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
        Serial.print(pinNum);
        Serial.print(":");
        Serial.print(int(ypr[0] * 180/M_PI));
        Serial.print(",");
        Serial.print(int(ypr[1] * 180/M_PI));
        Serial.print(",");
        Serial.print(int(ypr[2] * 180/M_PI));
        Serial.print("/");
    #endif
  }
}

GPIO를 인풋모드로밖에 못쓴다면서

데이터가 가끔 조금 나왔다가 말고

제대로 안나온다.

왜이런가 싶어 구글링해보니 이쪽 핀들은 인풋으로 밖에 못쓴다고하네

문제 고치고 시리얼로 출력해보면

#include "I2Cdev.h"

#include "MPU6050_6Axis_MotionApps20.h"
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
    #include "Wire.h"
#endif

// class default I2C address is 0x68
// specific I2C addresses may be passed as a parameter here
// AD0 low = 0x68 (default for SparkFun breakout and InvenSense evaluation board)
// AD0 high = 0x69
MPU6050 mpu;
//MPU6050 mpu(0x69); // <-- use for AD0 high

#define OUTPUT_READABLE_YAWPITCHROLL

// MPU control/status vars
bool dmpReady = false;  // set true if DMP init was successful
uint8_t mpuIntStatus;   // holds actual 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]         quaternion container
VectorInt16 aa;         // [x, y, z]            accel sensor measurements
VectorInt16 aaReal;     // [x, y, z]            gravity-free accel sensor measurements
VectorInt16 aaWorld;    // [x, y, z]            world-frame accel sensor measurements
VectorFloat gravity;    // [x, y, z]            gravity vector
float euler[3];         // [psi, theta, phi]    Euler angle container
float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector

volatile bool mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high
void dmpDataReady() {
    mpuInterrupt = true;
}


#include "BluetoothSerial.h"
String device_name = "ESP32-BT-Slave";

// Check if Bluetooth is available
#if !defined(CONFIG_BT_ENABLED) || !defined(CONFIG_BLUEDROID_ENABLED)
  #error Bluetooth is not enabled! Please run `make menuconfig` to and enable it
#endif

// Check Serial Port Profile
#if !defined(CONFIG_BT_SPP_ENABLED)
  #error Serial Port Profile for Bluetooth is not available or not enabled. It is only available for the ESP32 chip.
#endif

BluetoothSerial BTSerial;
int mpuPinNums[] = {32, 33, 25};
int mpuNum = sizeof(mpuPinNums) / sizeof(int);

void InitMPU(int pinNum);
void SendData(int pinNum);


void setup() {
  Serial.begin(115200);
  BTSerial.begin(device_name); //Bluetooth device name

  Serial.print("start init mpuNum : ");
  Serial.println(mpuNum);

  for (int i = 0; i < mpuNum;i++)
  {
    Serial.print("set pin output : ");
    Serial.println(mpuPinNums[i]);
    pinMode(mpuPinNums[i], OUTPUT);

  }

  // join I2C bus (I2Cdev library doesn't do this automatically)
  #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
      Wire.begin();
      Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having compilation difficulties
  #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
      Fastwire::setup(400, true);
  #endif

  for (int i = 0; i < mpuNum;i++)
    InitMPU(mpuPinNums[i]);
}

void loop() {
  for (int i = 0; i < mpuNum;i++)
    SendData(mpuPinNums[i]);

  Serial.println("");


}




void InitMPU(int pinNum)
{
  for (int i = 0; i < mpuNum; i++)
  {
    if (mpuPinNums[i] == pinNum)
      digitalWrite(mpuPinNums[i], LOW);  //set selected mpu6050 addr 0x68
    else
      digitalWrite(mpuPinNums[i], HIGH);  //set other mpu6050 addr 0x69
  }

  // initialize device
  mpu.initialize();

  // verify connection
  Serial.println(F("Testing device connections..."));
  Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));

  // load and configure the DMP
  Serial.println(F("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) {
      // turn on the DMP, now that it's ready
      Serial.println(F("Enabling DMP..."));
      mpu.setDMPEnabled(true);

      // enable Arduino interrupt detection
      Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
      attachInterrupt(0, dmpDataReady, RISING);
      //attachInterrupt(digitalPinToInterrupt(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(F("DMP ready! Waiting for first interrupt..."));
      dmpReady = true;

      // get expected DMP packet size for later comparison
      packetSize = mpu.dmpGetFIFOPacketSize();
  } else {
      Serial.print(F("DMP Initialization failed (code "));
      Serial.print(devStatus);
      Serial.println(F(")"));
  }
}


void SendData(int pinNum)
{
  for (int i = 0; i < mpuNum; i++)
  {
    if (mpuPinNums[i] == pinNum)
      digitalWrite(mpuPinNums[i], LOW);  //set selected mpu6050 addr 0x68
    else
      digitalWrite(mpuPinNums[i], HIGH);  //set other mpu6050 addr 0x69
  }

  // if programming failed, don't try to do anything
  if (!dmpReady) return;

  mpuIntStatus = mpu.getIntStatus();

  // get current FIFO count
  fifoCount = mpu.getFIFOCount();

  // check for overflow (this should never happen unless our code is too inefficient)
  if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
      // reset so we can continue cleanly
      mpu.resetFIFO();
  // otherwise, check for DMP data ready interrupt (this should happen frequently)
  } else if (mpuIntStatus & 0x02) {
    // wait for correct available data length, should be a VERY short wait
    while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();

    // read a packet from FIFO
    mpu.getFIFOBytes(fifoBuffer, packetSize);
    
    // track FIFO count here in case there is > 1 packet available
    fifoCount -= packetSize;

    #ifdef OUTPUT_READABLE_YAWPITCHROLL
        // display Euler angles in degrees
        mpu.dmpGetQuaternion(&q, fifoBuffer);
        mpu.dmpGetGravity(&gravity, &q);
        mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
        Serial.print(pinNum);
        Serial.print(":");
        Serial.print(int(ypr[0] * 180/M_PI));
        Serial.print(",");
        Serial.print(int(ypr[1] * 180/M_PI));
        Serial.print(",");
        Serial.print(int(ypr[2] * 180/M_PI));
        Serial.print("/");
    #endif
  }
}

잘 나오기는한데 뭔가 좀 이상하다.

그냥 대충 전역 딕셔너리 이용해서 쓰고싶은데

구글링으로 esp32에서 map 사용하는 내용 찾았다.

이걸 쓰기전에 먼저 \n이 왜 자주발생하는지부터 찾아서 해결하고 넘어가야겠다.

https://techtutorialsx.com/2021/11/02/esp32-the-cpp-map-container/

중간에 핀번호랑 인터럽트 상태 등 출력하도록 잠깐추가

  // get current FIFO count
  fifoCount = mpu.getFIFOCount();

  Serial.print(pinNum);
  Serial.print("pin dmpReady : ");
  Serial.print(", after mpuIntStatus : ");
  Serial.print(mpuIntStatus);
  Serial.print(", fifocount : ");
  Serial.println(fifoCount);

  // check for overflow (this should never happen unless our code is too inefficient)
  if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
      // reset so we can continue cleanly
      mpu.resetFIFO();

지금은또 지저분하긴하지만 멀정하게 잘나오는듯하다.

지우고봐도 값은 정상적인거같은데

왜이렇게 println이 많이 적용된건진 모르겟다.

이렇게 많이 시키진않았다만

대충 값은 잘들어오니 그냥 맵에 저장하도록 수정

map 사용하려고하는데 value 자리에 int[3]이 잘안된다.

아래 링크에서 배열 대신 구조체 쓰는내용을 봐서 수정

https://stackoverflow.com/questions/2582529/how-can-i-use-an-array-as-map-value

struct YPR
{
    int ypr[3];
};
std::map<int, YPR> yprMap;


void InitMPU(int pinNum);
void SendData(int pinNum);

void setup() {
  Serial.begin(115200);
  BTSerial.begin(device_name); //Bluetooth device name

  YPR zeroYpr = {0, 0, 0};
  for(int i = 0; i < mpuNum; i++)
    yprMap.insert(std::pair<int,YPR>(mpuPinNums[i], zeroYpr));

블루투스로 보내도록 전체코드 정리

#include "I2Cdev.h"
#include <map>

#include "MPU6050_6Axis_MotionApps20.h"
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
    #include "Wire.h"
#endif

MPU6050 mpu;

#define OUTPUT_READABLE_YAWPITCHROLL

// MPU control/status vars
bool dmpReady = false;  // set true if DMP init was successful
uint8_t mpuIntStatus;   // holds actual 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]         quaternion container
VectorInt16 aa;         // [x, y, z]            accel sensor measurements
VectorInt16 aaReal;     // [x, y, z]            gravity-free accel sensor measurements
VectorInt16 aaWorld;    // [x, y, z]            world-frame accel sensor measurements
VectorFloat gravity;    // [x, y, z]            gravity vector
float euler[3];         // [psi, theta, phi]    Euler angle container
float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector

volatile bool mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high
void dmpDataReady() {
    mpuInterrupt = true;
}


#include "BluetoothSerial.h"
String device_name = "ESP32-BT-Slave";

// Check if Bluetooth is available
#if !defined(CONFIG_BT_ENABLED) || !defined(CONFIG_BLUEDROID_ENABLED)
  #error Bluetooth is not enabled! Please run `make menuconfig` to and enable it
#endif

// Check Serial Port Profile
#if !defined(CONFIG_BT_SPP_ENABLED)
  #error Serial Port Profile for Bluetooth is not available or not enabled. It is only available for the ESP32 chip.
#endif

BluetoothSerial BTSerial;
int mpuPinNums[] = {32, 33, 25};
int mpuNum = sizeof(mpuPinNums) / sizeof(int);

struct YPR
{
    int ypr[3];
};
std::map<int, YPR> yprMap;


void InitMPU(int pinNum);
void GetData(int pinNum);
void SendData();

void setup() {
  //Serial.begin(115200);
  BTSerial.begin(device_name); //Bluetooth device name

  YPR zeroYpr = {0, 0, 0};
  for(int i = 0; i < mpuNum; i++)
    yprMap.insert(std::pair<int,YPR>(mpuPinNums[i], zeroYpr));

  BTSerial.print("start init mpuNum : ");
  BTSerial.println(mpuNum);

  for (int i = 0; i < mpuNum;i++)
  {
    BTSerial.print("set pin output : ");
    BTSerial.println(mpuPinNums[i]);
    pinMode(mpuPinNums[i], OUTPUT);

  }

  // join I2C bus (I2Cdev library doesn't do this automatically)
  #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
      Wire.begin();
      Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having compilation difficulties
  #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
      Fastwire::setup(400, true);
  #endif

  for (int i = 0; i < mpuNum;i++)
    InitMPU(mpuPinNums[i]);
}

void loop() {
  for (int i = 0; i < mpuNum;i++)
    GetData(mpuPinNums[i]);

  SendData();
}




void InitMPU(int pinNum)
{
  for (int i = 0; i < mpuNum; i++)
  {
    if (mpuPinNums[i] == pinNum)
      digitalWrite(mpuPinNums[i], LOW);  //set selected mpu6050 addr 0x68
    else
      digitalWrite(mpuPinNums[i], HIGH);  //set other mpu6050 addr 0x69
  }

  // initialize device
  mpu.initialize();

  // verify connection
  BTSerial.println(F("Testing device connections..."));
  BTSerial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));

  // load and configure the DMP
  BTSerial.println(F("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) {
      // turn on the DMP, now that it's ready
      BTSerial.println(F("Enabling DMP..."));
      mpu.setDMPEnabled(true);

      // enable Arduino interrupt detection
      BTSerial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
      attachInterrupt(0, dmpDataReady, RISING);
      //attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), dmpDataReady, RISING);

      mpuIntStatus = mpu.getIntStatus();

      // set our DMP Ready flag so the main loop() function knows it's okay to use it
      BTSerial.println(F("DMP ready! Waiting for first interrupt..."));
      dmpReady = true;

      // get expected DMP packet size for later comparison
      packetSize = mpu.dmpGetFIFOPacketSize();
  } else {
      BTSerial.print(F("DMP Initialization failed (code "));
      BTSerial.print(devStatus);
      BTSerial.println(F(")"));
  }
}


void GetData(int pinNum)
{
  for (int i = 0; i < mpuNum; i++)
  {
    if (mpuPinNums[i] == pinNum)
      digitalWrite(mpuPinNums[i], LOW);  //set selected mpu6050 addr 0x68
    else
      digitalWrite(mpuPinNums[i], HIGH);  //set other mpu6050 addr 0x69
  }

  // if programming failed, don't try to do anything
  if (!dmpReady) return;

  mpuIntStatus = mpu.getIntStatus();

  // get current FIFO count
  fifoCount = mpu.getFIFOCount();

  // check for overflow (this should never happen unless our code is too inefficient)
  if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
      // reset so we can continue cleanly
      mpu.resetFIFO();
  // otherwise, check for DMP data ready interrupt (this should happen frequently)
  } else if (mpuIntStatus & 0x02) {
    // wait for correct available data length, should be a VERY short wait
    while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();

    // read a packet from FIFO
    mpu.getFIFOBytes(fifoBuffer, packetSize);
    
    // track FIFO count here in case there is > 1 packet available
    fifoCount -= packetSize;

    #ifdef OUTPUT_READABLE_YAWPITCHROLL
      // display Euler angles in degrees
      mpu.dmpGetQuaternion(&q, fifoBuffer);
      mpu.dmpGetGravity(&gravity, &q);
      mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
      
      YPR tmpYpr;
      tmpYpr.ypr[0] = int(ypr[0] * 180/M_PI);
      tmpYpr.ypr[1] = int(ypr[1] * 180/M_PI);
      tmpYpr.ypr[2] = int(ypr[2] * 180/M_PI);

      yprMap[pinNum] = tmpYpr;
    #endif
  }
}


void SendData()
{
  for (int i = 0; i < mpuNum; i++)
  {
    BTSerial.print(mpuPinNums[i]);
    BTSerial.print(":");
    BTSerial.print(yprMap[mpuPinNums[i]].ypr[0]);
    BTSerial.print(",");
    BTSerial.print(yprMap[mpuPinNums[i]].ypr[1]);
    BTSerial.print(",");
    BTSerial.print(yprMap[mpuPinNums[i]].ypr[2]);
    if(i == mpuNum - 1)
      BTSerial.println("");
    else
      BTSerial.print("/");
  }
}

esp32 보조베터리 전원으로 mpu6050 ypr 송신하기

좌측 중간 오른쪽

32    33    25

순서인데

중간(초록불)에 있는 mpu6050은 이번에 알리에서 산게아닌 전에 있던거라 그런지

y 값이 혼자 다르다.

어쨋든 보조베터리 블루투스로 전체 데이터 빠르게 송신하니 됫지

일단 이전 글에서 ESP32로 어떻게 블루투스 통신하는지는 살펴봤다.

이제 드뎌 MPU6050달아서 좀해보려고하는데

ESP32가 버전이 여러개있으면서 핀맵이 조금씩 다른것같다.

내가갖고있는건 아래거랑 비슷하지만 살짝다른데

핀 내용이 뒷면에 있어서 빵판에 꽂으니 뭐가 뭔핀인지 모르겟다.

저사진 보고 따라가도 되나싶기도하고

구매한 사이트에서 확인하니 맞는것같다.

잠깐 찾아보니

이 글에서 esp32 gpio 설명을 잘 적어주긴했는데

30핀 기준인것같아 내꺼랑 맞는지는 좀봐야겟다.

https://mmirann.github.io/iot/arduino/2021/02/08/ESP32_GPIO.html

ESP32 30핀짜리 핀맵

ESP32 30핀짜리에는 GPIO21, GPIO22가 I2C 통신을 위해 사용되는데

앞서 찾은 38핀 보드 핀아웃에는 i2c 핀 내용이 안보여서

다른 38핀 핀아웃도 찾아봄

GPIO 21, 22가 맞나부다.

일단 위 핀맵 참고해서

I2C 통신할수 있도록 연결했다.

3V3은 빨간색

GND 파랑,어두운색

SCL 주황

SDA 노랑

esp32에서 mpu6050어떻게 쓸까 찾아보다가

찾은 글을 보니

내가 썻던 mpu6050 코드랑 거의 비슷해서 기존걸 사용하려햇는데

https://dev.to/shilleh/measure-angles-with-mpu6050-and-esp32-part-2-3d-animation-1cmm

아두이노에서 사용할때랑 다르게

이 에러가 발생했다.

esp32는 avr이 아니니 뜨는게 당연하긴한데

구글링하니 어캐고치는지 찾음

그리고 아까 진행하다가

i2c 무슨 에러낫엇는데 캡처를 안하고 넘어가서 뭐엿는지 기억안난다.

여기서 시킨데로 i2cdev만 새로 다운받은걸로 교체

했더니 그냥 넘어가졋다.

https://blog.naver.com/PostView.nhn?blogId=heennavi1004&logNo=222342645959&parentCategoryNo=&categoryNo=86&viewDate=&isShowPopularPosts=false&from=postView

mpu6050 코드 업로드시키고 리셋했더니

자꾸 dmp 초기화 실패가 나온다.

원인 찾아보려고하는데

공급전압 문제라는 사람도있긴하지만

i2c scanner로 한번 스캐닝해보라는 글찾앗다.

대충 esp32 i2c scanner 찾아서 확인

https://wise-self.tistory.com/84

i2c가 정상적으로 주소 찾긴했다.

내가 뭘 실수했을까 다시보다보니

setup에서 i2c 초기화 부분을 제대로 안넣어뒀더라--

  // join I2C bus (I2Cdev library doesn't do this automatically)
  #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
      Wire.begin();
      Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having compilation difficulties
  #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
      Fastwire::setup(400, true);
  #endif

다시 업로드하고 실행시켯는데

dmp 초기화는 잘된것같지만 자세 출력이 안된다..

코드 중간에 mpu interrupt 기다리는 내용이 있어

여기서 막힌것으로 보인다.

  // wait for MPU interrupt or extra packet(s) available
  while (!mpuInterrupt && fifoCount < packetSize) {
  }

  // reset interrupt flag and get INT_STATUS byte
  mpuInterrupt = false;

이 부분을 지우고 나면 잘 동작

어짜피 인터럽트 안써도 대충 원하는대로 동작하므로

대충 FIFO가 가득차서 리프레시 할떄 뺴고는 잘나온다.

#include "I2Cdev.h"

#include "MPU6050_6Axis_MotionApps20.h"
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
    #include "Wire.h"
#endif

// class default I2C address is 0x68
// specific I2C addresses may be passed as a parameter here
// AD0 low = 0x68 (default for SparkFun breakout and InvenSense evaluation board)
// AD0 high = 0x69
MPU6050 mpu;
//MPU6050 mpu(0x69); // <-- use for AD0 high

#define OUTPUT_READABLE_YAWPITCHROLL

// MPU control/status vars
bool dmpReady = false;  // set true if DMP init was successful
uint8_t mpuIntStatus;   // holds actual 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]         quaternion container
VectorInt16 aa;         // [x, y, z]            accel sensor measurements
VectorInt16 aaReal;     // [x, y, z]            gravity-free accel sensor measurements
VectorInt16 aaWorld;    // [x, y, z]            world-frame accel sensor measurements
VectorFloat gravity;    // [x, y, z]            gravity vector
float euler[3];         // [psi, theta, phi]    Euler angle container
float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector

// packet structure for InvenSense teapot demo
uint8_t teapotPacket[14] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' };


volatile bool mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high
void dmpDataReady() {
    mpuInterrupt = true;
}







#include "BluetoothSerial.h"

String device_name = "ESP32-BT-Slave";

// Check if Bluetooth is available
#if !defined(CONFIG_BT_ENABLED) || !defined(CONFIG_BLUEDROID_ENABLED)
  #error Bluetooth is not enabled! Please run `make menuconfig` to and enable it
#endif

// Check Serial Port Profile
#if !defined(CONFIG_BT_SPP_ENABLED)
  #error Serial Port Profile for Bluetooth is not available or not enabled. It is only available for the ESP32 chip.
#endif

BluetoothSerial BTSerial;

void setup() {
  // join I2C bus (I2Cdev library doesn't do this automatically)
  #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
      Wire.begin();
      Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having compilation difficulties
  #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
      Fastwire::setup(400, true);
  #endif


  BTSerial.begin(device_name); //Bluetooth device name
  Serial.begin(115200);



  // initialize device
  mpu.initialize();

  // verify connection
  Serial.println(F("Testing device connections..."));
  Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));

  // load and configure the DMP
  Serial.println(F("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) {
      // turn on the DMP, now that it's ready
      Serial.println(F("Enabling DMP..."));
      mpu.setDMPEnabled(true);

      // enable Arduino interrupt detection
      Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
      attachInterrupt(0, dmpDataReady, RISING);
      //attachInterrupt(digitalPinToInterrupt(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(F("DMP ready! Waiting for first interrupt..."));
      dmpReady = true;

      // get expected DMP packet size for later comparison
      packetSize = mpu.dmpGetFIFOPacketSize();
  } else {
      Serial.print(F("DMP Initialization failed (code "));
      Serial.print(devStatus);
      Serial.println(F(")"));
  }





}

void loop() {


  // if programming failed, don't try to do anything
  if (!dmpReady) return;

  mpuIntStatus = mpu.getIntStatus();

  // get current FIFO count
  fifoCount = mpu.getFIFOCount();
  /*
  Serial.print(", after mpuIntStatus : ");
  Serial.print(mpuIntStatus);
  Serial.print(", fifocount : ");
  Serial.println(fifoCount);
  */
  // check for overflow (this should never happen unless our code is too inefficient)
  if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
      // reset so we can continue cleanly
      mpu.resetFIFO();

  // otherwise, check for DMP data ready interrupt (this should happen frequently)
  } else if (mpuIntStatus & 0x02) {
    // wait for correct available data length, should be a VERY short wait
    while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();

    // read a packet from FIFO
    mpu.getFIFOBytes(fifoBuffer, packetSize);
    
    // track FIFO count here in case there is > 1 packet available
    // (this lets us immediately read more without waiting for an interrupt)
    fifoCount -= packetSize;

    #ifdef OUTPUT_READABLE_YAWPITCHROLL
        // display Euler angles in degrees
        mpu.dmpGetQuaternion(&q, fifoBuffer);
        mpu.dmpGetGravity(&gravity, &q);
        mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
        //Serial.print(pinNum);
        //Serial.print(":");
        Serial.print(int(ypr[0] * 180/M_PI));
        Serial.print(",");
        Serial.print(int(ypr[1] * 180/M_PI));
        Serial.print(",");
        Serial.print(int(ypr[2] * 180/M_PI));
        Serial.print("/");
    #endif
  }
  delay(20);
}

이제 블루투스로 넘어가서 동작시켜보자

위 코드를 블루투스 시리얼로 고친결과 잘 나온다.

문제는 보조베터리 상태에서도 잘되느냐

와 이제 보조베터리 전원으로도 

블루투스 통해서 빠르게 ypr 값을 가져올수 있게됫다. 

ESP-WROOM-32 핀아웃

스펙

MPU6050은 원래 3.3V 사용하니까 

3v3 핀을 vcc로 주면될듯

처음 쓰므로 대충 다음 링크 참고

https://makerspace.steamedu123.com/entry/ESP32-ESP32-%EC%95%84%EB%91%90%EC%9D%B4%EB%85%B8-IDE-%EC%97%90%EC%84%9C-%EC%82%AC%EC%9A%A9%ED%95%98%EA%B8%B0

https://www.bneware.com/blogPost/esp32_arduino_ide

처음쓰면서 업로드 설정이 뭔가 복잡해보이는데

 ESP32 Dev Module에 장치관리자에 나온 COM10포트 지정해줫더니 

대충 올라가지긴한다.

간단하게

아래 링크참고해서 블루투스 예제 가져와서 사용해봄

https://m.blog.naver.com/heennavi1004/221799043375

업로드한 블루투스 예제

// This example code is in the Public Domain (or CC0 licensed, at your option.)
// By Evandro Copercini - 2018
//
// This example creates a bridge between Serial and Classical Bluetooth (SPP)
// and also demonstrate that SerialBT have the same functionalities of a normal Serial
// Note: Pairing is authenticated automatically by this device

#include "BluetoothSerial.h"

String device_name = "ESP32-BT-Slave";

// Check if Bluetooth is available
#if !defined(CONFIG_BT_ENABLED) || !defined(CONFIG_BLUEDROID_ENABLED)
  #error Bluetooth is not enabled! Please run `make menuconfig` to and enable it
#endif

// Check Serial Port Profile
#if !defined(CONFIG_BT_SPP_ENABLED)
  #error Serial Port Profile for Bluetooth is not available or not enabled. It is only available for the ESP32 chip.
#endif

BluetoothSerial SerialBT;

void setup() {
  Serial.begin(115200);
  SerialBT.begin(device_name); //Bluetooth device name
  //SerialBT.deleteAllBondedDevices(); // Uncomment this to delete paired devices; Must be called after begin
  Serial.printf("The device with name \"%s\" is started.\nNow you can pair it with Bluetooth!\n", device_name.c_str());
}

void loop() {
  if (Serial.available()) {
    SerialBT.write(Serial.read());
  }
  if (SerialBT.available()) {
    Serial.write(SerialBT.read());
  }
  delay(20);
}

hc-05랑 다르게 비번 설정없이 바로 BT로 연결가능

추가 블루투스 옵션에 들어가서

컴포트 확인해보면 COM12가 송신으로 되어있음

COM12

시리얼, 블루투스 둘다 115200으로 열어서

키입력하면 코드넣은대로

상대편에 보내는걸 확인할수 있다.

일단 지난글 방식대로 해서

AD0로 MPU6050 주소를 2가지만 선택이 가능하지만

여러개로부터 값을 가져올수 있도록 정리했었다.

이번엔 아예 3개를 연결해서 가져와지나 테스트

가져와지는건 좋지만 여전히 데이터가 몇개 빠져서 들어올때가 있다.

데이터 송수신시 다른 조건문으로 들어가서라 생각되는데

void SendData(int pinNum)
{
    for (int i = 0; i < 2; i++)
    {
      if (mpuPinNums[i] == pinNum)
        digitalWrite(mpuPinNums[i], LOW);  //set selected mpu6050 addr 0x68
      else
        digitalWrite(mpuPinNums[i], HIGH);  //set other mpu6050 addr 0x69
    }

    // if programming failed, don't try to do anything
    if (!dmpReady) return;

    // wait for MPU interrupt or extra packet(s) available
    while (!mpuInterrupt && fifoCount < packetSize) {
    }

    // reset interrupt flag and get INT_STATUS byte
    mpuInterrupt = false;
    mpuIntStatus = mpu.getIntStatus();

    // get current FIFO count
    fifoCount = mpu.getFIFOCount();

    // check for overflow (this should never happen unless our code is too inefficient)
    if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
        // reset so we can continue cleanly
        mpu.resetFIFO();

    // otherwise, check for DMP data ready interrupt (this should happen frequently)
    } else if (mpuIntStatus & 0x02) {
        // wait for correct available data length, should be a VERY short wait
        while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();

        // read a packet from FIFO
        mpu.getFIFOBytes(fifoBuffer, packetSize);
        
        // track FIFO count here in case there is > 1 packet available
        // (this lets us immediately read more without waiting for an interrupt)
        fifoCount -= packetSize;

        #ifdef OUTPUT_READABLE_YAWPITCHROLL
            // display Euler angles in degrees
            mpu.dmpGetQuaternion(&q, fifoBuffer);
            mpu.dmpGetGravity(&gravity, &q);
            mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
            BTSerial.print(pinNum);
            BTSerial.print(":");
            BTSerial.print(int(ypr[0] * 180/M_PI));
            BTSerial.print(",");
            BTSerial.print(int(ypr[1] * 180/M_PI));
            BTSerial.print(",");
            BTSerial.print(int(ypr[2] * 180/M_PI));
            BTSerial.print("/");
        #endif
    }
}

mpu 상태가져오기전에 사용되는 값들 출력시켜봣는데

    Serial.println("");
    Serial.print(pinNum);
    Serial.print("pin dmpReady : ");
    Serial.print(dmpReady);
    Serial.print(", before mpuInterrupt : ");
    Serial.print(mpuInterrupt);
    Serial.print(", fifocount : ");
    Serial.println(fifoCount);

    // if programming failed, don't try to do anything
    if (!dmpReady) return;

    // wait for MPU interrupt or extra packet(s) available
    while (!mpuInterrupt && fifoCount < packetSize) {
    }

이것만으로 어쩔때 나오고 어쩔때 안나오는지 판단할수가 없엇음.

이번에는

상태랑 피포카운트 출력하도록 수정

    // reset interrupt flag and get INT_STATUS byte
    mpuInterrupt = false;
    mpuIntStatus = mpu.getIntStatus();

    // get current FIFO count
    fifoCount = mpu.getFIFOCount();
    Serial.print(", after mpuIntStatus : ");
    Serial.print(mpuIntStatus);
    Serial.print(", fifocount : ");
    Serial.println(fifoCount);

    // check for overflow (this should never happen unless our code is too inefficient)
    if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
        // reset so we can continue cleanly
        mpu.resetFIFO();

    // otherwise, check for DMP data ready interrupt (this should happen frequently)
    } else if (mpuIntStatus & 0x02) {
        // wait for correct available data length, should be a VERY short wait
        while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();

        // read a packet from FIFO
        mpu.getFIFOBytes(fifoBuffer, packetSize);
        
        // track FIFO count here in case there is > 1 packet available
        // (this lets us immediately read more without waiting for an interrupt)
        fifoCount -= packetSize;

        #ifdef OUTPUT_READABLE_YAWPITCHROLL
            // display Euler angles in degrees
            mpu.dmpGetQuaternion(&q, fifoBuffer);
            mpu.dmpGetGravity(&gravity, &q);
            mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
            Serial.print(pinNum);
            Serial.print(":");

위 코드 대로 mpuIntStatus 가 2일때만 진입해서 출력하는걸 확인

2 외에

1, 3, 4, 19가 자주 발생한다.

잠깐 지금 보니 6, 7때 같은값을 가져오고있었네

mpu3개 연결했는데

루프문을 몇군대 잘못하기도하고

세번째 mpu를 반대로 놓거나

핀아웃을 안해놔서 안떳다.

거기다가 블루투스때문에 보드레이트를 9600 해놨었는데

115200으로 했을때 더 잘나온다.

//RXD3, TXD4
SoftwareSerial BTSerial(4, 3);
int mpuPinNums[] = {5, 6, 7};
int mpuNum = sizeof(mpuPinNums) / sizeof(int);


void InitMPU(int pinNum);
void SendData(int pinNum);

void setup() {
  for (int i = 0; i < mpuNum;i++)
    pinMode(mpuPinNums[i], OUTPUT);

  Serial.begin(115200);

  #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
      Wire.begin();
      TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz)
  #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
      Fastwire::setup(400, true);
  #endif

  while (!Serial); // wait for Leonardo enumeration, others continue immediately

  for (int i = 0; i < mpuNum;i++)
    InitMPU(mpuPinNums[i]);
}


void loop() {

  for (int i = 0; i < mpuNum;i++)
    SendData(mpuPinNums[i]);

  Serial.println("");
}


void InitMPU(int pinNum)
{
  for (int i = 0; i < mpuNum; i++)
  {
    if (mpuPinNums[i] == pinNum)
      digitalWrite(pinNum, LOW);  //set selected mpu6050 addr 0x68
    else
      digitalWrite(mpuPinNums[i], HIGH);  //set other mpu6050 addr 0x69
  }
  Serial.print("start init pin ");
  Serial.println(pinNum);


  // initialize device
  mpu.initialize();

  // verify connection
  Serial.println(F("Testing device connections..."));
  Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));

  // load and configure the DMP
  Serial.println(F("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) {
      // turn on the DMP, now that it's ready
      Serial.println(F("Enabling DMP..."));
      mpu.setDMPEnabled(true);

      // enable Arduino interrupt detection
      Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
      attachInterrupt(0, dmpDataReady, RISING);
      //attachInterrupt(digitalPinToInterrupt(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(F("DMP ready! Waiting for first interrupt..."));
      dmpReady = true;

      // get expected DMP packet size for later comparison
      packetSize = mpu.dmpGetFIFOPacketSize();
  } else {
      Serial.print(F("DMP Initialization failed (code "));
      Serial.print(devStatus);
      Serial.println(F(")"));
  }
}

void SendData(int pinNum)
{
  for (int i = 0; i < mpuNum; i++)
  {
    if (mpuPinNums[i] == pinNum)
      digitalWrite(mpuPinNums[i], LOW);  //set selected mpu6050 addr 0x68
    else
      digitalWrite(mpuPinNums[i], HIGH);  //set other mpu6050 addr 0x69
  }


  Serial.println("");

  Serial.print(pinNum);
  Serial.print("pin dmpReady : ");

  // if programming failed, don't try to do anything
  if (!dmpReady) return;

  // wait for MPU interrupt or extra packet(s) available
  while (!mpuInterrupt && fifoCount < packetSize) {
  }

  // reset interrupt flag and get INT_STATUS byte
  mpuInterrupt = false;
  mpuIntStatus = mpu.getIntStatus();

  // get current FIFO count
  fifoCount = mpu.getFIFOCount();
  Serial.print(", after mpuIntStatus : ");
  Serial.print(mpuIntStatus);
  Serial.print(", fifocount : ");
  Serial.println(fifoCount);

  // check for overflow (this should never happen unless our code is too inefficient)
  if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
      // reset so we can continue cleanly
      mpu.resetFIFO();

  // otherwise, check for DMP data ready interrupt (this should happen frequently)
  } else if (mpuIntStatus & 0x02) {
      // wait for correct available data length, should be a VERY short wait
      while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();

      // read a packet from FIFO
      mpu.getFIFOBytes(fifoBuffer, packetSize);
      
      // track FIFO count here in case there is > 1 packet available
      // (this lets us immediately read more without waiting for an interrupt)
      fifoCount -= packetSize;

      #ifdef OUTPUT_READABLE_YAWPITCHROLL
          // display Euler angles in degrees
          mpu.dmpGetQuaternion(&q, fifoBuffer);
          mpu.dmpGetGravity(&gravity, &q);
          mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
          Serial.print(pinNum);
          Serial.print(":");
          Serial.print(int(ypr[0] * 180/M_PI));
          Serial.print(",");
          Serial.print(int(ypr[1] * 180/M_PI));
          Serial.print(",");
          Serial.print(int(ypr[2] * 180/M_PI));
          Serial.print("/");
      #endif
  }
}

블루투스로 확인하기전에

잠깐 mpu intstatus의미부터 확인해보면

깃헙에서 찾은걸보면

이게 인터럽트 레지스터 값 가져오는거같다

각 값별 의미는 없어서 데이터시트 봐야할듯

데이터시트에서 말하길

레지스터맵 레지스터 설명 다큐먼트보랜다.

레지스터 다큐먼트에서 인터럽트 상태 레지스터 족 찾긴했는데

좀이해는 안간다.

비트1이 없으면 왜 2, 3같은 값이 나오는걸까?

내가 잘못 찾거나 잘못이해한거라 생각되는데

뭔가 이상하다 싶어서

구글링하다가

우리나라에서도 이것때문에 해맨 분글을 찾았다.

진짜 눈물난다.

해외에서도 이렇게 잘 설명해준게 흔치않는데

우리나라 분이 10년전이 정리해서 올려놨다니 ㅜㅜ

정리하면 데이터시트에서 제대로 작성안한 히든레지스터가 있어서 그렇다고한다.

다시돌아와 지금은 115200 보드레이트를 쓰면서 잘 출력이 됬었지만

블루투스에서 9600 보드레이트를 쓰다보니

9600보드레이트로 낮출때

인터럽트스테이트가 19가 나왔따.

다시 보면

인터럽트 스테이터스가 19일때는 피포 오버플로우로 출력이 안됬꼬

나머지 출력이 안된 상황인 1, 4의 경우 피포 오버플로가 안됬지만

1번비트가 하이가 아니여서 조건문에거 걸린것으로 보인다.

어쨋거나 mpu동작속도 생각하면

115200쓰는게 좋아보이긴한데

배터리 블루투스로하면 왜 못받아오는지 찾질못했었지만

이제 상태값도 조금 찾아봣겟다

블루투스 설정을 115200으로해서 다시봐도 될듯하다.

이전에 보드레이트변경 코드를 가져와서 사용

AT로 동작확인

AT+BUAD8로 115200로 변경

노트북 USB전원으로 동작시킬때는

블루투스 115200으로도 잘 동작한다.

시리얼 115200때보다 인터럽트 상태가 19되는경우가 잦은느낌이긴한데

베터리 전원을 줄때 인터럽트 코드확인

역시나 115200 hc-05를 보조베터리 전원을줄때 안된다.

내가 갖고있는 보조베터리 출력이 5V승압없이 3.7V 2A 라 그런것같기도하고 

그냥 원래 생각했던데로 esp32나 쓸까부다--

원래 주말에 해야지 생각만하고 계속 질질 미루다

오늘 아침이 되서야 MPU6050 나머지 땜질 다했다.

11개가 준비됬는데

일단 2개만 어떻게하면 동시에 가져올수 있을까 정리

이건 내가 기존에 사용하던 코드인데

MPU6050 mpu;를 여러개 만들어서 일단 정리해야할듯

#include "I2Cdev.h"
#include <SoftwareSerial.h> 

#include "MPU6050_6Axis_MotionApps20.h"
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
    #include "Wire.h"
#endif

// class default I2C address is 0x68
// specific I2C addresses may be passed as a parameter here
// AD0 low = 0x68 (default for SparkFun breakout and InvenSense evaluation board)
// AD0 high = 0x69
MPU6050 mpu;
//MPU6050 mpu(0x69); // <-- use for AD0 high

#define OUTPUT_READABLE_YAWPITCHROLL


#define LED_PIN 13 // (Arduino is 13, Teensy is 11, Teensy++ is 6)
bool blinkState = false;

// MPU control/status vars
bool dmpReady = false;  // set true if DMP init was successful
uint8_t mpuIntStatus;   // holds actual 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]         quaternion container
VectorInt16 aa;         // [x, y, z]            accel sensor measurements
VectorInt16 aaReal;     // [x, y, z]            gravity-free accel sensor measurements
VectorInt16 aaWorld;    // [x, y, z]            world-frame accel sensor measurements
VectorFloat gravity;    // [x, y, z]            gravity vector
float euler[3];         // [psi, theta, phi]    Euler angle container
float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector

// packet structure for InvenSense teapot demo
uint8_t teapotPacket[14] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' };


volatile bool mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high
void dmpDataReady() {
    mpuInterrupt = true;
}



//RXD3, TXD4
SoftwareSerial BTSerial(4, 3);

void setup() {
    BTSerial.begin(9600);

    #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
        Wire.begin();
        TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz)
    #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
        Fastwire::setup(400, true);
    #endif

    while (!BTSerial); // wait for Leonardo enumeration, others continue immediately

    // initialize device
    BTSerial.println(F("Initializing I2C devices..."));
    mpu.initialize();
    //pinMode(INTERRUPT_PIN, INPUT);

    // verify connection
    BTSerial.println(F("Testing device connections..."));
    BTSerial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));

    // load and configure the DMP
    BTSerial.println(F("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) {
        // turn on the DMP, now that it's ready
        BTSerial.println(F("Enabling DMP..."));
        mpu.setDMPEnabled(true);

        // enable Arduino interrupt detection
        BTSerial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
        attachInterrupt(0, dmpDataReady, RISING);
        //attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), dmpDataReady, RISING);

        mpuIntStatus = mpu.getIntStatus();

        // set our DMP Ready flag so the main loop() function knows it's okay to use it
        BTSerial.println(F("DMP ready! Waiting for first interrupt..."));
        dmpReady = true;

        // get expected DMP packet size for later comparison
        packetSize = mpu.dmpGetFIFOPacketSize();
    } else {
        BTSerial.print(F("DMP Initialization failed (code "));
        BTSerial.print(devStatus);
        BTSerial.println(F(")"));
    }
}


void loop() {
    // if programming failed, don't try to do anything
    if (!dmpReady) return;

    // wait for MPU interrupt or extra packet(s) available
    while (!mpuInterrupt && fifoCount < packetSize) {
    }

    // reset interrupt flag and get INT_STATUS byte
    mpuInterrupt = false;
    mpuIntStatus = mpu.getIntStatus();

    // get current FIFO count
    fifoCount = mpu.getFIFOCount();

    // check for overflow (this should never happen unless our code is too inefficient)
    if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
        // reset so we can continue cleanly
        mpu.resetFIFO();
        //BTSerial.println(F("FIFO overflow!"));

    // otherwise, check for DMP data ready interrupt (this should happen frequently)
    } else if (mpuIntStatus & 0x02) {
        // wait for correct available data length, should be a VERY short wait
        while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();

        // read a packet from FIFO
        mpu.getFIFOBytes(fifoBuffer, packetSize);
        
        // track FIFO count here in case there is > 1 packet available
        // (this lets us immediately read more without waiting for an interrupt)
        fifoCount -= packetSize;

        #ifdef OUTPUT_READABLE_YAWPITCHROLL
            // display Euler angles in degrees
            mpu.dmpGetQuaternion(&q, fifoBuffer);
            mpu.dmpGetGravity(&gravity, &q);
            mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
            BTSerial.print(int(ypr[0] * 180/M_PI));
            BTSerial.print(",");
            BTSerial.print(int(ypr[1] * 180/M_PI));
            BTSerial.print(",");
            BTSerial.print(int(ypr[2] * 180/M_PI));
            BTSerial.print("/");
        #endif
    }
    //delay(30);
    delay(100);

}

하려다가 mpu 기본주소가 0x68인데

ad0핀으로 어떻게 조정하는지부터 찾아봐야겟다.

이제 이해한게 mpu6050은 ad0로 주소를 2가지 경우로만 설정못한다고 한다.

저 내용 밑에보면

jremington이란 사람이

mpu6050 여러개 연결해서 

가져올것만 ad0 핀일 low하고 나머지는 high로 설정해주면 된다고 설명한다.

이게 괜찬은 방법인진 모르겟지만 일단 대충해보자

일단 지레밍턴? 말대로 해서 한번 만들어봤다.

프리징 같은걸로 우노보드랑 모듈들 배선 정리해서 올리고싶긴한데

유료화되서 쓰기도 힘들고

다른 적당한걸 못찾아서 그냥 만들기만함.

#include "I2Cdev.h"
#include <SoftwareSerial.h> 

#include "MPU6050_6Axis_MotionApps20.h"
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
    #include "Wire.h"
#endif

// class default I2C address is 0x68
// specific I2C addresses may be passed as a parameter here
// AD0 low = 0x68 (default for SparkFun breakout and InvenSense evaluation board)
// AD0 high = 0x69
MPU6050 mpu;
//MPU6050 mpu(0x69); // <-- use for AD0 high

#define OUTPUT_READABLE_YAWPITCHROLL


#define LED_PIN 13 // (Arduino is 13, Teensy is 11, Teensy++ is 6)
bool blinkState = false;

// MPU control/status vars
bool dmpReady = false;  // set true if DMP init was successful
uint8_t mpuIntStatus;   // holds actual 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]         quaternion container
VectorInt16 aa;         // [x, y, z]            accel sensor measurements
VectorInt16 aaReal;     // [x, y, z]            gravity-free accel sensor measurements
VectorInt16 aaWorld;    // [x, y, z]            world-frame accel sensor measurements
VectorFloat gravity;    // [x, y, z]            gravity vector
float euler[3];         // [psi, theta, phi]    Euler angle container
float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector

// packet structure for InvenSense teapot demo
uint8_t teapotPacket[14] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' };


volatile bool mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high
void dmpDataReady() {
    mpuInterrupt = true;
}



//RXD3, TXD4
SoftwareSerial BTSerial(4, 3);
int mpuPinNums[] = {5, 6};


void InitMPU(int pinNum);
void SendData(int pinNum);

void setup() {
  pinMode(5, OUTPUT);//MPU #1
  pinMode(6, OUTPUT);//MPU #2

  Serial.begin(115200);

  #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
      Wire.begin();
      TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz)
  #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
      Fastwire::setup(400, true);
  #endif

  while (!Serial); // wait for Leonardo enumeration, others continue immediately

  for (int i = 0; i<2; i++)
    InitMPU(mpuPinNums[i]);
}


void loop() {
  for (int i = 0; i<2; i++)
    SendData(mpuPinNums[i]);

  Serial.println("");
  delay(100);
}


void InitMPU(int pinNum)
{
  for (int i = 0; i < 2; i++)
  {
    if (mpuPinNums[i] == pinNum)
      digitalWrite(pinNum, LOW);  //set selected mpu6050 addr 0x68
    else
      digitalWrite(mpuPinNums[i], HIGH);  //set other mpu6050 addr 0x69
  }
  Serial.print("start init pin ");
  Serial.println(pinNum);


  // initialize device
  mpu.initialize();

  // verify connection
  Serial.println(F("Testing device connections..."));
  Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));

  // load and configure the DMP
  Serial.println(F("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) {
      // turn on the DMP, now that it's ready
      Serial.println(F("Enabling DMP..."));
      mpu.setDMPEnabled(true);

      // enable Arduino interrupt detection
      Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
      attachInterrupt(0, dmpDataReady, RISING);
      //attachInterrupt(digitalPinToInterrupt(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(F("DMP ready! Waiting for first interrupt..."));
      dmpReady = true;

      // get expected DMP packet size for later comparison
      packetSize = mpu.dmpGetFIFOPacketSize();
  } else {
      Serial.print(F("DMP Initialization failed (code "));
      Serial.print(devStatus);
      Serial.println(F(")"));
  }
}

void SendData(int pinNum)
{
    for (int i = 0; i < 2; i++)
    {
      if (mpuPinNums[i] == pinNum)
        digitalWrite(mpuPinNums[i], LOW);  //set selected mpu6050 addr 0x68
      else
        digitalWrite(mpuPinNums[i], HIGH);  //set other mpu6050 addr 0x69
    }

    // if programming failed, don't try to do anything
    if (!dmpReady) return;

    // wait for MPU interrupt or extra packet(s) available
    while (!mpuInterrupt && fifoCount < packetSize) {
    }

    // reset interrupt flag and get INT_STATUS byte
    mpuInterrupt = false;
    mpuIntStatus = mpu.getIntStatus();

    // get current FIFO count
    fifoCount = mpu.getFIFOCount();

    // check for overflow (this should never happen unless our code is too inefficient)
    if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
        // reset so we can continue cleanly
        mpu.resetFIFO();

    // otherwise, check for DMP data ready interrupt (this should happen frequently)
    } else if (mpuIntStatus & 0x02) {
        // wait for correct available data length, should be a VERY short wait
        while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();

        // read a packet from FIFO
        mpu.getFIFOBytes(fifoBuffer, packetSize);
        
        // track FIFO count here in case there is > 1 packet available
        // (this lets us immediately read more without waiting for an interrupt)
        fifoCount -= packetSize;

        #ifdef OUTPUT_READABLE_YAWPITCHROLL
            // display Euler angles in degrees
            mpu.dmpGetQuaternion(&q, fifoBuffer);
            mpu.dmpGetGravity(&gravity, &q);
            mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
            Serial.print(pinNum);
            Serial.print(":");
            Serial.print(int(ypr[0] * 180/M_PI));
            Serial.print(",");
            Serial.print(int(ypr[1] * 180/M_PI));
            Serial.print(",");
            Serial.print(int(ypr[2] * 180/M_PI));
            Serial.print("/");
        #endif
    }
}

기존의 초기화, 각 가져오는 코드는 따로빼고

AD0 핀들 output 설정

int mpuPinNums[] = {5, 6};

void InitMPU(int pinNum);
void SendData(int pinNum);

void setup() {
  pinMode(5, OUTPUT);//MPU #1
  pinMode(6, OUTPUT);//MPU #2

  Serial.begin(115200);

  #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
      Wire.begin();
      TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz)
  #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
      Fastwire::setup(400, true);
  #endif

  while (!Serial); // wait for Leonardo enumeration, others continue immediately

  for (int i = 0; i<2; i++)
    InitMPU(mpuPinNums[i]);
}

초기화 코드에서는

모든 mpu를 돌아가면서 초기화하는데

선택된 mpu만 주소를 0x68로 설정하고 수행

나머지는 초기화전 0x69로 설정

void InitMPU(int pinNum)
{
  for (int i = 0; i < 2; i++)
  {
    if (mpuPinNums[i] == pinNum)
      digitalWrite(pinNum, LOW);  //set selected mpu6050 addr 0x68
    else
      digitalWrite(mpuPinNums[i], HIGH);  //set other mpu6050 addr 0x69
  }
  Serial.print("start init pin ");
  Serial.println(pinNum);


  // initialize device
  mpu.initialize();

  // verify connection
  Serial.println(F("Testing device connections..."));
  Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));

  // load and configure the DMP
  Serial.println(F("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) {
      // turn on the DMP, now that it's ready
      Serial.println(F("Enabling DMP..."));
      mpu.setDMPEnabled(true);

      // enable Arduino interrupt detection
      Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
      attachInterrupt(0, dmpDataReady, RISING);
      //attachInterrupt(digitalPinToInterrupt(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(F("DMP ready! Waiting for first interrupt..."));
      dmpReady = true;

      // get expected DMP packet size for later comparison
      packetSize = mpu.dmpGetFIFOPacketSize();
  } else {
      Serial.print(F("DMP Initialization failed (code "));
      Serial.print(devStatus);
      Serial.println(F(")"));
  }
}

루프의 경우 각 mpu 돌아가면서 각 출력하고 딜레이 주는식으로 정리

void loop() {
  for (int i = 0; i<2; i++)
    SendData(mpuPinNums[i]);

  Serial.println("");
  delay(50);
}

senddata의 경우

초기화때와 마찬가지로 선택된 mpu만 가져오도록 설정

void SendData(int pinNum)
{
    for (int i = 0; i < 2; i++)
    {
      if (mpuPinNums[i] == pinNum)
        digitalWrite(mpuPinNums[i], LOW);  //set selected mpu6050 addr 0x68
      else
        digitalWrite(mpuPinNums[i], HIGH);  //set other mpu6050 addr 0x69
    }

    // if programming failed, don't try to do anything
    if (!dmpReady) return;

    // wait for MPU interrupt or extra packet(s) available
    while (!mpuInterrupt && fifoCount < packetSize) {
    }

    // reset interrupt flag and get INT_STATUS byte
    mpuInterrupt = false;
    mpuIntStatus = mpu.getIntStatus();

    // get current FIFO count
    fifoCount = mpu.getFIFOCount();

    // check for overflow (this should never happen unless our code is too inefficient)
    if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
        // reset so we can continue cleanly
        mpu.resetFIFO();

    // otherwise, check for DMP data ready interrupt (this should happen frequently)
    } else if (mpuIntStatus & 0x02) {
        // wait for correct available data length, should be a VERY short wait
        while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();

        // read a packet from FIFO
        mpu.getFIFOBytes(fifoBuffer, packetSize);
        
        // track FIFO count here in case there is > 1 packet available
        // (this lets us immediately read more without waiting for an interrupt)
        fifoCount -= packetSize;

        #ifdef OUTPUT_READABLE_YAWPITCHROLL
            // display Euler angles in degrees
            mpu.dmpGetQuaternion(&q, fifoBuffer);
            mpu.dmpGetGravity(&gravity, &q);
            mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
            Serial.print(pinNum);
            Serial.print(":");
            Serial.print(int(ypr[0] * 180/M_PI));
            Serial.print(",");
            Serial.print(int(ypr[1] * 180/M_PI));
            Serial.print(",");
            Serial.print(int(ypr[2] * 180/M_PI));
            Serial.print("/");
        #endif
    }
}

이 코드는 배터리가 아닌 PC전원으로 쓰면서

115200 시리얼 통신으로 가져와 쓰드록 정리

#include "I2Cdev.h"
#include <SoftwareSerial.h> 

#include "MPU6050_6Axis_MotionApps20.h"
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
    #include "Wire.h"
#endif

// class default I2C address is 0x68
// specific I2C addresses may be passed as a parameter here
// AD0 low = 0x68 (default for SparkFun breakout and InvenSense evaluation board)
// AD0 high = 0x69
MPU6050 mpu;
//MPU6050 mpu(0x69); // <-- use for AD0 high

#define OUTPUT_READABLE_YAWPITCHROLL


#define LED_PIN 13 // (Arduino is 13, Teensy is 11, Teensy++ is 6)
bool blinkState = false;

// MPU control/status vars
bool dmpReady = false;  // set true if DMP init was successful
uint8_t mpuIntStatus;   // holds actual 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]         quaternion container
VectorInt16 aa;         // [x, y, z]            accel sensor measurements
VectorInt16 aaReal;     // [x, y, z]            gravity-free accel sensor measurements
VectorInt16 aaWorld;    // [x, y, z]            world-frame accel sensor measurements
VectorFloat gravity;    // [x, y, z]            gravity vector
float euler[3];         // [psi, theta, phi]    Euler angle container
float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector

// packet structure for InvenSense teapot demo
uint8_t teapotPacket[14] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' };


volatile bool mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high
void dmpDataReady() {
    mpuInterrupt = true;
}


//RXD3, TXD4
SoftwareSerial BTSerial(4, 3);
int mpuPinNums[] = {5, 6};

void InitMPU(int pinNum);
void SendData(int pinNum);

void setup() {
  pinMode(5, OUTPUT);//MPU #1
  pinMode(6, OUTPUT);//MPU #2

  Serial.begin(115200);

  #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
      Wire.begin();
      TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz)
  #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
      Fastwire::setup(400, true);
  #endif

  while (!Serial); // wait for Leonardo enumeration, others continue immediately

  for (int i = 0; i<2; i++)
    InitMPU(mpuPinNums[i]);
}


void loop() {
  for (int i = 0; i<2; i++)
    SendData(mpuPinNums[i]);

  Serial.println("");
  delay(50);
}


void InitMPU(int pinNum)
{
  for (int i = 0; i < 2; i++)
  {
    if (mpuPinNums[i] == pinNum)
      digitalWrite(pinNum, LOW);  //set selected mpu6050 addr 0x68
    else
      digitalWrite(mpuPinNums[i], HIGH);  //set other mpu6050 addr 0x69
  }
  Serial.print("start init pin ");
  Serial.println(pinNum);


  // initialize device
  mpu.initialize();

  // verify connection
  Serial.println(F("Testing device connections..."));
  Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));

  // load and configure the DMP
  Serial.println(F("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) {
      // turn on the DMP, now that it's ready
      Serial.println(F("Enabling DMP..."));
      mpu.setDMPEnabled(true);

      // enable Arduino interrupt detection
      Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
      attachInterrupt(0, dmpDataReady, RISING);
      //attachInterrupt(digitalPinToInterrupt(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(F("DMP ready! Waiting for first interrupt..."));
      dmpReady = true;

      // get expected DMP packet size for later comparison
      packetSize = mpu.dmpGetFIFOPacketSize();
  } else {
      Serial.print(F("DMP Initialization failed (code "));
      Serial.print(devStatus);
      Serial.println(F(")"));
  }
}

void SendData(int pinNum)
{
    for (int i = 0; i < 2; i++)
    {
      if (mpuPinNums[i] == pinNum)
        digitalWrite(mpuPinNums[i], LOW);  //set selected mpu6050 addr 0x68
      else
        digitalWrite(mpuPinNums[i], HIGH);  //set other mpu6050 addr 0x69
    }

    // if programming failed, don't try to do anything
    if (!dmpReady) return;

    // wait for MPU interrupt or extra packet(s) available
    while (!mpuInterrupt && fifoCount < packetSize) {
    }

    // reset interrupt flag and get INT_STATUS byte
    mpuInterrupt = false;
    mpuIntStatus = mpu.getIntStatus();

    // get current FIFO count
    fifoCount = mpu.getFIFOCount();

    // check for overflow (this should never happen unless our code is too inefficient)
    if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
        // reset so we can continue cleanly
        mpu.resetFIFO();

    // otherwise, check for DMP data ready interrupt (this should happen frequently)
    } else if (mpuIntStatus & 0x02) {
        // wait for correct available data length, should be a VERY short wait
        while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();

        // read a packet from FIFO
        mpu.getFIFOBytes(fifoBuffer, packetSize);
        
        // track FIFO count here in case there is > 1 packet available
        // (this lets us immediately read more without waiting for an interrupt)
        fifoCount -= packetSize;

        #ifdef OUTPUT_READABLE_YAWPITCHROLL
            // display Euler angles in degrees
            mpu.dmpGetQuaternion(&q, fifoBuffer);
            mpu.dmpGetGravity(&gravity, &q);
            mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
            Serial.print(pinNum);
            Serial.print(":");
            Serial.print(int(ypr[0] * 180/M_PI));
            Serial.print(",");
            Serial.print(int(ypr[1] * 180/M_PI));
            Serial.print(",");
            Serial.print(int(ypr[2] * 180/M_PI));
            Serial.print("/");
        #endif
    }
}

이번엔 블루투스로 변경

딜레이 100주면 115200 써도 되나모르겟다.

안되서 9600으로 다시진행

#include "I2Cdev.h"
#include <SoftwareSerial.h> 

#include "MPU6050_6Axis_MotionApps20.h"
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
    #include "Wire.h"
#endif

// class default I2C address is 0x68
// specific I2C addresses may be passed as a parameter here
// AD0 low = 0x68 (default for SparkFun breakout and InvenSense evaluation board)
// AD0 high = 0x69
MPU6050 mpu;
//MPU6050 mpu(0x69); // <-- use for AD0 high

#define OUTPUT_READABLE_YAWPITCHROLL


#define LED_PIN 13 // (Arduino is 13, Teensy is 11, Teensy++ is 6)
bool blinkState = false;

// MPU control/status vars
bool dmpReady = false;  // set true if DMP init was successful
uint8_t mpuIntStatus;   // holds actual 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]         quaternion container
VectorInt16 aa;         // [x, y, z]            accel sensor measurements
VectorInt16 aaReal;     // [x, y, z]            gravity-free accel sensor measurements
VectorInt16 aaWorld;    // [x, y, z]            world-frame accel sensor measurements
VectorFloat gravity;    // [x, y, z]            gravity vector
float euler[3];         // [psi, theta, phi]    Euler angle container
float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector

// packet structure for InvenSense teapot demo
uint8_t teapotPacket[14] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' };


volatile bool mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high
void dmpDataReady() {
    mpuInterrupt = true;
}



//RXD3, TXD4
SoftwareSerial BTSerial(4, 3);
int mpuPinNums[] = {5, 6};


void InitMPU(int pinNum);
void SendData(int pinNum);

void setup() {
  pinMode(5, OUTPUT);//MPU #1
  pinMode(6, OUTPUT);//MPU #2

  BTSerial.begin(9600);

  #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
      Wire.begin();
      TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz)
  #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
      Fastwire::setup(400, true);
  #endif

  while (!Serial); // wait for Leonardo enumeration, others continue immediately

  InitMPU(5);
  InitMPU(6);
}


void loop() {


  SendData(5);
  SendData(6);
  BTSerial.println("");
  delay(100);
}


void InitMPU(int pinNum)
{
  for (int i = 0; i < 2; i++)
  {
    if (mpuPinNums[i] == pinNum)
      digitalWrite(pinNum, LOW);  //set selected mpu6050 addr 0x68
    else
      digitalWrite(mpuPinNums[i], HIGH);  //set other mpu6050 addr 0x69
  }
  BTSerial.print("start init pin ");
  BTSerial.println(pinNum);


  // initialize device
  mpu.initialize();

  // verify connection
  BTSerial.println(F("Testing device connections..."));
  BTSerial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));

  // load and configure the DMP
  BTSerial.println(F("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) {
      // turn on the DMP, now that it's ready
      BTSerial.println(F("Enabling DMP..."));
      mpu.setDMPEnabled(true);

      // enable Arduino interrupt detection
      BTSerial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
      attachInterrupt(0, dmpDataReady, RISING);
      //attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), dmpDataReady, RISING);

      mpuIntStatus = mpu.getIntStatus();

      // set our DMP Ready flag so the main loop() function knows it's okay to use it
      BTSerial.println(F("DMP ready! Waiting for first interrupt..."));
      dmpReady = true;

      // get expected DMP packet size for later comparison
      packetSize = mpu.dmpGetFIFOPacketSize();
  } else {
      BTSerial.print(F("DMP Initialization failed (code "));
      BTSerial.print(devStatus);
      BTSerial.println(F(")"));
  }
}

void SendData(int pinNum)
{
    for (int i = 0; i < 2; i++)
    {
      if (mpuPinNums[i] == pinNum)
        digitalWrite(mpuPinNums[i], LOW);  //set selected mpu6050 addr 0x68
      else
        digitalWrite(mpuPinNums[i], HIGH);  //set other mpu6050 addr 0x69
    }

    // if programming failed, don't try to do anything
    if (!dmpReady) return;

    // wait for MPU interrupt or extra packet(s) available
    while (!mpuInterrupt && fifoCount < packetSize) {
    }

    // reset interrupt flag and get INT_STATUS byte
    mpuInterrupt = false;
    mpuIntStatus = mpu.getIntStatus();

    // get current FIFO count
    fifoCount = mpu.getFIFOCount();

    // check for overflow (this should never happen unless our code is too inefficient)
    if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
        // reset so we can continue cleanly
        mpu.resetFIFO();

    // otherwise, check for DMP data ready interrupt (this should happen frequently)
    } else if (mpuIntStatus & 0x02) {
        // wait for correct available data length, should be a VERY short wait
        while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();

        // read a packet from FIFO
        mpu.getFIFOBytes(fifoBuffer, packetSize);
        
        // track FIFO count here in case there is > 1 packet available
        // (this lets us immediately read more without waiting for an interrupt)
        fifoCount -= packetSize;

        #ifdef OUTPUT_READABLE_YAWPITCHROLL
            // display Euler angles in degrees
            mpu.dmpGetQuaternion(&q, fifoBuffer);
            mpu.dmpGetGravity(&gravity, &q);
            mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
            BTSerial.print(pinNum);
            BTSerial.print(":");
            BTSerial.print(int(ypr[0] * 180/M_PI));
            BTSerial.print(",");
            BTSerial.print(int(ypr[1] * 180/M_PI));
            BTSerial.print(",");
            BTSerial.print(int(ypr[2] * 180/M_PI));
            BTSerial.print("/");
        #endif
    }
}

아까 유선으로 할때도 그랬지만

종종 데이터가 2개다 출력되지 않고 1개만 출력되는 경우가 생긴다.

조건문에서 생긴 문제같은데

따로 확인해봐야할듯

9600 보드레이트, 100 딜레이줫을때 잘 나온다.

이런식으로 10개까지 늘려도 될것같은데

esp32는 언제쓸까 .

이전 글에서 

회전자 보간으로 모델 회전을 조금은 자연스럽게 만들었다.

하지만 아직 아쉬운건 

오프셋

관성계를 내 정면 방향을 보도록 하고

바닥에 딱 놓으면 160, 0, 0 정도가 나온다.

정면을 향하게 놓을땐 y가 0이 되도록 하고싶은데

오프셋 조정시켜보려다가

처음 키면 yaw 누적되기도하고 어떻게할까싶다가

그냥 포기하고 

다른 모델 가져와서 써봄

근데 다른거 가져와써보려고할때마다

fbx가 자꾸 분할되서 만드렁진다.

왜그런지 몰라서

https://www.youtube.com/watch?v=FsV3ZUxmWLo

위 영상보다가 안게

advanced에 combine mesh 옵션이 있었다.

이거하니 매시가 분할되지않고 만들어지긴 한데 텍스처는 어캐할까.

fbx 가져와도 스폰지밥이 하얗게 나온다.

내가 다운받은 모델에는 따로 페이스, 바디, 쇼츠 텍스쳐도 같이 있긴한데

이건 어케하지 못하고 패스

대신 mr krab의 경우 

택스처랑 엘리먼트랑 대충 맞춰서 비슷하게 만들었다.

아우 모델링 공부하긴해야되는데 귀찬아

spongibob 모델도 다른거 찾아서 가져오니 색상도 잘 받아와진다.

그 외 찾은 모델은 짐

블루투스 + 관성계 + 언리얼로

다양한 모델들을 게임에서 회전 제어 까지 대충은 하긴 했다.

오늘 보간 + 모델 활용까지하면서

1차적인 목표는 달성한듯 싶고

내일 영상으로 정리해야지.

보간이란 단어를 처음본게

컴퓨터 비전에서 리사이즈가 어떻게 동작하는지 공부하다가 알게되었다.

그러다가 언리얼에서도 동작 다루는데 보간한다고 들었는데

https://coding-hell.tistory.com/58

지금 센서 데이터 받아오는건 좋은데 너무 끊기는걸 부드럽게 하고싶어서 gpt한테 물어봄

q : 언리얼엔진에서
1초 간격으로
100,50,3
100,10,2
100,30,1
이 값들이 순서대로 들어와서
액터의 회전값으로 사용했어
하지만 이벤트 틱은 매우 빠르게 반복되잔아?
이 값들이 1초마다 들어오다보니
액터 회전이 부드럽지 않고 딱딱끊겨있어
어떻게하면 회전 동작을 부드럽게 보간할수있을까

a : 언리얼 엔진에서 액터의 회전을 부드럽게 보간하기 위해 사용할 수 있는 방법 중 하나는 보간 기법을 적용하는 것입니다. 아래는 예시 코드로 보간을 구현하는 방법입니다:

1. 액터 클래스에 변수 추가:
   - FVector TargetRotation; // 목표 회전값
   - float InterpSpeed; // 보간 속도

2. 이벤트 틱 함수 내에서 보간 로직 추가:
   - FVector CurrentRotation = GetActorRotation().Euler(); // 현재 회전값
   - FVector NewRotation = FMath::VInterpTo(CurrentRotation, TargetRotation, DeltaTime, InterpSpeed); // 보간된 회전값 계산
   - SetActorRotation(FRotator(NewRotation.X, NewRotation.Y, NewRotation.Z)); // 액터에 회전값 적용

3. 1초마다 값 갱신:
   - 매 1초마다 TargetRotation 변수에 새로운 회전값을 설정합니다.

이렇게 구현하면 이벤트 틱이 매우 빠르게 반복되더라도 보간 기법을 사용하여 액터의 회전을 부드럽게 보간할 수 있습니다. 보간 속도(InterpSpeed)를 조정하여 원하는 부드러움 수준을 설정할 수도 있습니다.

q : C++코드 대신 블루프린트로 설정법 알려줘

a : 블루프린트에서 액터의 회전을 부드럽게 보간하기 위해서는 노드를 조합하여 구현할 수 있습니다.

1. 액터 블루프린트에서 변수 추가:
- TargetRotation: Rotator // 목표 회전값
- InterpSpeed: Float // 보간 속도

2. 블루프린트 그래프에서 보간 로직 추가:
- "GetActorRotation" 노드로 현재 회전값을 가져옵니다.
- "Break Rotator" 노드로 현재 회전값을 나눠서 X, Y, Z 각도 값을 추출합니다.
- "Make Rotator" 노드로 TargetRotation 변수 값을 Rotator 형태로 만듭니다.
- "Delta Seconds" 노드를 사용하여 Delta Time 값을 가져옵니다.
- "VInterp To" 노드로 현재 회전값과 목표 회전값을 보간하여 보간된 회전값을 계산합니다.
- "SetActorRotation" 노드로 액터에 보간된 회전값을 적용합니다.

3. 1초마다 값 갱신:
- "Set Timer by Event" 노드를 사용하여 1초마다 보간할 회전값을 설정하는 이벤트를 생성합니다.
- 이벤트에서 "Set Actor Rotation" 노드를 사용하여 TargetRotation 변수 값을 변경합니다.

이렇게 구현하면 블루프린트에서도 보간 기법을 사용하여 액터의 회전을 부드럽게 보간할 수 있습니다. 보간 속도(InterpSpeed)를 조정하여 원하는 부드러움 수준을 설정할 수도 있습니다.

얘 대답이 맞든 틀리든 대충 비슷하겐 할수잇을듯

위에서 틀린건 나는 벡터가 아니라 로테이터 쓰니

RInterpTo 쓰려고하는데

deltaTime이야 이벤트틱에서 나오는거 쓰면될거같고.

보간속도는 얼마나 하면될지 모르겟다.

아쉬운데로 질문

q : 언리얼엔진 RInterpTo에 대해서 자세히 설명해줘
여기서 interp speed는 몇으로 하는게 적당할까?

a : RInterpTo 함수에서 사용되는 보간 속도(Interp Speed)는 원하는 부드러움의 정도를 결정하는 매개변수입니다. 이 값은 프로젝트의 요구사항과 시각적인 효과에 따라 다를 수 있습니다.

적당한 보간 속도를 선택하는 것은 상황과 개발자의 취향에 따라 다를 수 있지만, 몇 가지 일반적인 가이드라인이 있습니다:

1. 느린 보간 속도: 저속으로 회전하는 액터나 오브젝트에 적합합니다. 예를 들어, 천천히 돌아가는 문이나 회전하는 플랫폼 등을 구현할 때 사용할 수 있습니다.

2. 중간 보간 속도: 대부분의 상황에서 적합한 기본값입니다. 일반적인 캐릭터의 회전이나 카메라 이동 등에 사용될 수 있습니다. 대부분의 경우, 5-10 정도의 값을 사용하는 것이 적당합니다.

3. 빠른 보간 속도: 빠른 이동이나 반응성이 필요한 상황에 적합합니다. 빠른 캐릭터의 회전이나 빠르게 이동하는 물체 등을 구현할 때 사용될 수 있습니다. 높은 값인 15 이상의 값을 사용할 수 있습니다.

위의 값은 일반적인 가이드일 뿐이며, 프로젝트의 필요에 따라 조정할 수 있습니다. 보간 속도를 조정하여 원하는 시각적인 효과를 얻을 수 있으며, 필요에 따라 여러 값을 실험해보면서 적절한 보간 속도를 찾을 수 있습니다.

RPYRotator 가 바뀌면 PrevRPYRotator에 저장하고 새값을 RPYRotator에 저장한 상태에서

RInterpTo를 했는데

여전히 동작이 어색하다.

생각해보니 Prev가 아니라 target에 RPYRotator를 설정하고

커런트를 액터 로테이션으로 쓰면되지 않나싶은데

Prev는 안하고 모델 액터 로테이터를 current로 설정하고,

보간속도를 30으로 주니 좀 나아지긴 했는데

여전히 아쉽다.

Prev가 필요없으니 브랜치 구문도 제거

아무래도 시리얼로 가져오는 속도는 느린데

리드라인 성공했을때만 보간 하도록 설정한게 문젠것같음.

리드라인 성공 실패 둘다 

회전자 보간값으로 액터 로테이션 설정했더니

리드라인 못한 상태에서 보간이 너무빨리이뤄져 

너무 툭툭끊기는 느낌으로 움직인다..

인터폴레이션 속도를 5정도로 지정할때

너무 빠르지도 느리지도 않게 적당한듯.

그래도 어제한것보단 좀 낫다.

putty에선 정상적으로 받지만

언리얼에선 시리얼 포트 읽기가 문제

readstring 대신 뭐가 있는지봄

일시 정지했다가 풀면

정상값이 여러개 들어와진다

그냥 쭉 진행하면 중간에 false가 자주 들어오고

아무래도 틱이 시리얼 포트로 전송속도보다 빠르게 반복되서라 생각든다.

리드스트링대신 리드라인 결과도 깨지는것 없어보인다.

그런데 리드 라인한 뒤에도 움직이니까 또 깨지기 시작한다.

자꾸 깨지는건 곤란한데

string들을 모두 붙여놓고

\n 스플릿 후 가장 마지막을 꺼내놓는게 나을까?

이번엔 스트링 append를 계속시켜줫는데

여전히 맨끝에 깨진 상태 값이 나온다.

putty에선 움직여도 정상적으로 나오는데

버퍼에서 가져오는걸 내가 잘못한거같은데

지금 내가 할수있는 좋은 방법이 생각 안나서

아두이노 마지막에 println대신 추가하도록변경

그냥 언리얼에선 string append 시킨 결과

대충 어디서 잘렸는지는 구분할수 있을듯.

뒤에서 두번째 /와 뒤에서 첫번째 / 사이에 있는 ypr을 사용하면 될듯하다.

append string 하는 함수 따로 만들고

길이가 30까지 되도록 자름

대충 맨 마지막에 있는 두 // 사이 값만 가져오면 되는데

파이썬으로하면 쉽게할걸 bp로 하려니 쉽진않다.

/가 적으면 2개 많으면 3개쯤 나올거같아보이는데

그냥 split 여러번해서 써야할듯. 싶었는데

그냥 굳이 마지막껄 할필요가 없고

어짜피 맨뒤에꺼 길이 30만큼 자른 상태라

앞에 있는 / 2개 사이있는거 써도 별차이가 없내?

따로 함수만들고

AppendYPR 뒤에 만든 함수 GetFirstYPR을 붙인결과 결과

값이 정상적으로 만들어졌다!

그러면 이값으로 이제 모델 로테이션 시켜주면 될듯하다.

YPRString을 RPY Rotator로 만들어 반환하는 함수

월드에는 내가 어제 만들어둔 큐브 블록 BP가 있는데

방금 작성한 함수로 얻은 로테이터를 액터 로테이션으로 설정

촬영하려고하는데

내 놋북에서 다른걸 누르면 느려진다.

설정하는법 다시 찾아서 적어둠

에디터 설정에서 다음 항목 체크 해재

Use less cpu when in background

검색안되면 use less cpu만해도 나온다.

언리얼 게임 실행 중 다른 작업시 느려지는 상황 푸는 법

오늘 너무 개으름 피우다가 이제야 해결했다.

보조베터리 전원으로 우노 BT MPU6050 동작시킨 결과

단순 read string으로 잘리는걸 해결할수 없어서 / 스플릿 해서 만든건 엉망같긴한데 어쩌갯나

값이 느리게 보내지다보니 동작이 부드럽지않고 탁탁 바뀌니 좀 아쉽긴하다.

이게 인터폴레이션 관련된거 같은데

내일 찾아봐서 고쳐야지