How to read IMU Sensor Roll, Yaw, Pitch values in Blender for realtime motion tracking

motion-graphics
real-time
add-ons
simulator

(Deep) #1

I am Noob, working with 9 DoF sensor, want to view the sensor values in 3D with Blender.

I want the IMU sensor Roll Yaw Pitch data to be received by the blender Cube and performs real time tracking of the sensor.

Please Help,
Using

  • Blender 2.79,

  • PySerial 3.4.

I want to do something like this!

//Arduino Code
#include "MPU9250.h"

MPU9250 mpu;

uint8_t buffer[20]; //Buffer needed to store data packet for transmission
int16_t data1 = 1;
int16_t data2 = 2;
int16_t data3 = 3;
int16_t data4 = 4;
bool debug = false;

uint8_t buffer2[20];

void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);

    Wire.begin();

    delay(2000);
    mpu.setup();

     Serial.println(" roll, \t pitch, \t yaw, ");

}

int16_t value = 0;
int8_t direction = 10;
void loop() {
  // put your main code here, to run repeatedly: 
  //Serial.write(value);
  static uint32_t prev_ms = millis();
    if ((millis() - prev_ms) > 16)
    {
        mpu.update();
       // mpu.print();

  value = (value + direction);
  if (value > 100)
    direction = -10;
  else if (value < -50)
    direction = 10;
  //data3 = value;
  //plot(value, value/2, value/4, value/8);
  for (uint8_t i = 0; i<4; i++) {
    switch (i) {
      case 0:
      Serial.print(mpu.getRoll());
      break;
      case 1:
      Serial.print(mpu.getPitch());
      break;
      case 2:
      Serial.println(mpu.getYaw());
      break;
     
    }
    if (i < 4)
      Serial.print(" ");
  }
  Serial.print('\r');
  delay(5);
  prev_ms = millis();
    }
}
uint8_t variableA = {0x00};
void plot(int16_t data1, int16_t data2, int16_t data3, int16_t data4) {
  int16_t pktSize;
  
  buffer[0] = 0xCDAB;             //SimPlot packet header. Indicates start of data packet
  //buffer[1] = 4*sizeof(int16_t);      //Size of data in bytes. Does not include the header and size fields
  buffer[1] = 1;
  buffer[2] = 5;
  buffer[3] = 6;
  buffer[4] = 7;
  buffer[5] = 8;
    
  pktSize = 2 + 2 + (4*sizeof(int16_t)); //Header bytes + size field bytes + data
  
  if (!debug) {
    Serial.print(data1);
    Serial.print(" ");
    Serial.print(data2);
    Serial.print(" ");
    Serial.print(data3);
    Serial.print(" ");
    Serial.print(data4);
    Serial.print('\r');
  }
  else {
    Serial.print("Size: ");
    Serial.println(pktSize, HEX);
    for (int i = 0; i<pktSize; i++) {
      Serial.print(buffer[i], HEX);
      Serial.print(" ");
    }
    Serial.println();
  }
}
#Pyserial code
#It's wrong, calculating euler, but need roll yaw pitch calculations
import bge
import serial
import math

ser = serial.Serial('COM7', 115200)

a = 0.0
b = 0.0
c = 0.0

def AnaLoop():
    while ser.inWaiting() !=0:
        global a
        global b
        global c
        
        a = (ord(ser.read(100)))
        b = (ord(ser.read(100)))
        c = (ord(ser.read(100)))
        print (a)
        print (b)
        print (c)
        
def Cube():
    global a
    global b
    global c
    
    scene = bge.logic.getCurrentScene()
    cont = bge.logic.getCurrentController()
    own = cont.owner
    
    xyz = own.localOrientation.to_euler()
    xyz[0] = math.radians(a)
    xyz[1] = math.radians(b)
    xyz[2] = math.radians(c)
    
    own.localOrientation = xyz.to_matrix()