Anip Shah
Published © MIT

3D-Gyro Simulation to control orientation of virtual object

Arduino Nano & 3-axis MEMS sensor (MPU-6050) takes real-time motion data and controls an orientation of virtual 3D cube in Processing IDE.

IntermediateFull instructions provided2 hours1,445
3D-Gyro Simulation to control orientation of virtual object

Things used in this project

Hardware components

Arduino Nano R3
Arduino Nano R3
×1
Inertial Measurement Unit (IMU) (6 deg of freedom)
Inertial Measurement Unit (IMU) (6 deg of freedom)
×1

Software apps and online services

Arduino IDE
Arduino IDE
Processing
The Processing Foundation Processing

Hand tools and fabrication machines

Breadboard, 400 Pin
Breadboard, 400 Pin
10 Pc. Jumper Wire Kit, 5 cm Long
10 Pc. Jumper Wire Kit, 5 cm Long

Story

Read more

Schematics

Interfacing MPU-6050 with Arduino Nano

Code

3D-Gyro (Arduino IDE)

C/C++
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"

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

MPU6050 mpu;
#define OUTPUT_READABLE_YAWPITCHROLL
#define LED_PIN 13 
bool blinkState = false;
bool dmpReady = false;  
uint8_t mpuIntStatus;   
uint8_t devStatus;      
uint16_t packetSize;    
uint16_t fifoCount;     
uint8_t fifoBuffer[64]; 

Quaternion q;           
VectorInt16 aa;         
VectorInt16 aaReal;     
VectorInt16 aaWorld;    
VectorFloat gravity;    
float euler[3];         
float ypr[3];           
uint8_t teapotPacket[14] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' };

volatile bool mpuInterrupt = false;     
void dmpDataReady() {
    mpuInterrupt = true;
}

void setup() {
    
    #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
        Wire.begin();
        TWBR = 24; 
    #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
        Fastwire::setup(400, true);
    #endif

    Serial.begin(115200);
    while (!Serial); 
    mpu.initialize();
    devStatus = mpu.dmpInitialize();

    
    mpu.setXGyroOffset(220);
    mpu.setYGyroOffset(76);
    mpu.setZGyroOffset(-85);
    mpu.setZAccelOffset(1788); 

    if (devStatus == 0) {
        mpu.setDMPEnabled(true);
        attachInterrupt(0, dmpDataReady, RISING);
        mpuIntStatus = mpu.getIntStatus();       
        dmpReady = true;
        packetSize = mpu.dmpGetFIFOPacketSize();
    } else {
        
        Serial.print(devStatus);
        Serial.println(F(")"));
    }
    pinMode(LED_PIN, OUTPUT);
}


void loop() {
    if (!dmpReady) return;
    while (!mpuInterrupt && fifoCount < packetSize) {
    }

    mpuInterrupt = false;
    mpuIntStatus = mpu.getIntStatus();
    fifoCount = mpu.getFIFOCount();

    if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
        mpu.resetFIFO();

    } else if (mpuIntStatus & 0x02) {
        while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
        mpu.getFIFOBytes(fifoBuffer, packetSize);
        fifoCount -= packetSize;

        #ifdef OUTPUT_READABLE_QUATERNION
            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
            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
            mpu.dmpGetQuaternion(&q, fifoBuffer);
            mpu.dmpGetGravity(&gravity, &q);
            mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
            Serial.print(ypr[2] * 18/M_PI);
            Serial.print(" ");
            Serial.print(ypr[1] * 180/M_PI);
            Serial.print(" ");
            Serial.println(ypr[0] * 180/M_PI);
        #endif

        #ifdef OUTPUT_READABLE_REALACCEL
            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
            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
            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]++; 
        #endif

        blinkState = !blinkState;
        digitalWrite(LED_PIN, blinkState);
    }
}

3D-Gyro (Processing IDE)

Java
import processing.serial.*;
import cc.arduino.*;

int W = 800;
int H = 800; 

Serial port;
float Phi;
float Theta;
float Psi;

int cubeSize = 70; 
int numCubes = 3;
int spacing = 2; 

int[][][] cubeColors;

void setup() {
  size(800, 800, P3D);
  rectMode(CENTER);
  smooth();
  strokeCap(SQUARE); 

  background(255); 

  println(Serial.list());
  port = new Serial(this, Serial.list()[0], 115200);
  port.bufferUntil('\n');

  cubeColors = new int[numCubes][numCubes][numCubes];

  // Initialize the cube colors
  for (int x = 0; x < numCubes; x++) {
    for (int y = 0; y < numCubes; y++) {
      for (int z = 0; z < numCubes; z++) {
        cubeColors[x][y][z] = color(random(255), random(255), random(255));
      }
    }
  }
}

void draw() {
  background(255);
  translate(W / 2, H / 2);

  // Rotate the entire grid
  rotateX(radians(Theta));
  rotateY(radians(Phi));
  rotateZ(radians(Psi));

  // Draw the grid of cubes
  for (int x = 0; x < numCubes; x++) {
    for (int y = 0; y < numCubes; y++) {
      for (int z = 0; z < numCubes; z++) {
        pushMatrix();
        translate((x - numCubes / 2) * (cubeSize + spacing),
          (y - numCubes / 2) * (cubeSize + spacing),
          (z - numCubes / 2) * (cubeSize + spacing));
        fill(cubeColors[x][y][z]);
        box(cubeSize);
        popMatrix();
      }
    }
  }
}

void serialEvent(Serial port) 
{
  String input = port.readStringUntil('\n');
  if (input != null) {
    input = trim(input);
    String[] values = split(input, " ");
    if (values.length == 3) {
      float phi = float(values[0]);
      float theta = float(values[1]);
      float psi = float(values[2]);
      Phi = phi;
      Theta = theta;
      Psi = psi;
    }
  }
}

Credits

Anip Shah

Anip Shah

9 projects • 4 followers
Tech enthusiast and programmer who also happens to be a Biomedical Engineer.

Comments

Related channels and tags
  • 3D Printing
  • Arduino
  • The Processing Foundation
  • Virtual Reality