관성모션 - 15. ESP32 MPU6050 여러 개 언리얼 액터 제어

지난글에 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으로 해서그런가

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