MPU teapot not showing 3d model

Hi, I have a teapot code that is able to receive and write out the ypr of the mpu 6050, but it is unable to make the 3d rendered modeled in processing, it just shows a new window with a white background. What am I missing, or is there something I have to change?

import processing.serial.;
import processing.opengl.;
import toxi.geom.;
import toxi.processing.;

ToxiclibsSupport gfx;

Serial port; // The serial port
char[] teapotPacket = new char[14]; // InvenSense Teapot packet
int serialCount = 0; // current packet byte position
int aligned = 0;
int interval = 0;

float[] q = new float[4];
Quaternion quat = new Quaternion(1, 0, 0, 0);

float[] gravity = new float[3];
float[] euler = new float[3];
float[] ypr = new float[3];

void setup() {
// 300px square viewport using OpenGL rendering
size(300, 300, P3D);
gfx = new ToxiclibsSupport(this);

// setup lights and antialiasing
lights();
smooth();

// display serial port list for debugging/clarity
println(Serial.list());

// get the first available port (use EITHER this OR the specific port code below)
String portName = "/dev/cu.usbmodem14101";

// get a specific serial port (use EITHER this OR the first-available code above)
//String portName = "COM4";

// open the serial port
port = new Serial(this, portName, 115200);

// send single character to trigger DMP init/start
// (expected by MPU6050_DMP6 example Arduino sketch)
port.write('r');
}

void draw() {
if (millis() - interval > 1000) {
// resend single character to trigger DMP init/start
// in case the MPU is halted/reset while applet is running
port.write(‘r’);
interval = millis();
}

// black background
background(0);

// translate everything to the middle of the viewport
pushMatrix();
translate(width / 2, height / 2);

// 3-step rotation from yaw/pitch/roll angles (gimbal lock!)
// ...and other weirdness I haven't figured out yet
//rotateY(-ypr[0]);
//rotateZ(-ypr[1]);
//rotateX(-ypr[2]);

// toxiclibs direct angle/axis rotation from quaternion (NO gimbal lock!)
// (axis order [1, 3, 2] and inversion [-1, +1, +1] is a consequence of
// different coordinate system orientation assumptions between Processing
// and InvenSense DMP)
float[] axis = quat.toAxisAngle();
rotate(axis[0], -axis[1], axis[3], axis[2]);

// draw main body in red
fill(255, 0, 0, 200);
box(10, 10, 200);

// draw front-facing tip in blue
fill(0, 0, 255, 200);
pushMatrix();
translate(0, 0, -120);
rotateX(PI/2);
drawCylinder(0, 20, 20, 8);
popMatrix();

// draw wings and tail fin in green
fill(0, 255, 0, 200);
beginShape(TRIANGLES);
vertex(-100,  2, 30); vertex(0,  2, -80); vertex(100,  2, 30);  // wing top layer
vertex(-100, -2, 30); vertex(0, -2, -80); vertex(100, -2, 30);  // wing bottom layer
vertex(-2, 0, 98); vertex(-2, -30, 98); vertex(-2, 0, 70);  // tail left layer
vertex( 2, 0, 98); vertex( 2, -30, 98); vertex( 2, 0, 70);  // tail right layer
endShape();
beginShape(QUADS);
vertex(-100, 2, 30); vertex(-100, -2, 30); vertex(  0, -2, -80); vertex(  0, 2, -80);
vertex( 100, 2, 30); vertex( 100, -2, 30); vertex(  0, -2, -80); vertex(  0, 2, -80);
vertex(-100, 2, 30); vertex(-100, -2, 30); vertex(100, -2,  30); vertex(100, 2,  30);
vertex(-2,   0, 98); vertex(2,   0, 98); vertex(2, -30, 98); vertex(-2, -30, 98);
vertex(-2,   0, 98); vertex(2,   0, 98); vertex(2,   0, 70); vertex(-2,   0, 70);
vertex(-2, -30, 98); vertex(2, -30, 98); vertex(2,   0, 70); vertex(-2,   0, 70);
endShape();

popMatrix();
}

void serialEvent(Serial port) {
interval = millis();
while (port.available() > 0) {
int ch = port.read();
print((char)ch);
if (ch == ‘') {serialCount = 0;} // this will help with alignment if (aligned < 4) { // make sure we are properly aligned on a 14-byte packet if (serialCount == 0) { if (ch == '’) aligned++; else aligned = 0;
} else if (serialCount == 1) {
if (ch == 2) aligned++; else aligned = 0;
} else if (serialCount == 12) {
if (ch == ‘\r’) aligned++; else aligned = 0;
} else if (serialCount == 13) {
if (ch == ‘\n’) aligned++; else aligned = 0;
}
//println(ch + " " + aligned + " " + serialCount);
serialCount++;
if (serialCount == 14) serialCount = 0;
} else {
if (serialCount > 0 || ch == ‘$’) {
teapotPacket[serialCount++] = (char)ch;
if (serialCount == 14) {
serialCount = 0; // restart packet byte position

                // get quaternion from data packet
                q[0] = ((teapotPacket[2] << 8) | teapotPacket[3]) / 16384.0f;
                q[1] = ((teapotPacket[4] << 8) | teapotPacket[5]) / 16384.0f;
                q[2] = ((teapotPacket[6] << 8) | teapotPacket[7]) / 16384.0f;
                q[3] = ((teapotPacket[8] << 8) | teapotPacket[9]) / 16384.0f;
                for (int i = 0; i < 4; i++) if (q[i] >= 2) q[i] = -4 + q[i];
                
                // set our toxilibs quaternion to new data
                quat.set(q[0], q[1], q[2], q[3]);

                /*
                // below calculations unnecessary for orientation only using toxilibs
                
                // calculate gravity vector
                gravity[0] = 2 * (q[1]*q[3] - q[0]*q[2]);
                gravity[1] = 2 * (q[0]*q[1] + q[2]*q[3]);
                gravity[2] = q[0]*q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3];
    
                // calculate Euler angles
                euler[0] = atan2(2*q[1]*q[2] - 2*q[0]*q[3], 2*q[0]*q[0] + 2*q[1]*q[1] - 1);
                euler[1] = -asin(2*q[1]*q[3] + 2*q[0]*q[2]);
                euler[2] = atan2(2*q[2]*q[3] - 2*q[0]*q[1], 2*q[0]*q[0] + 2*q[3]*q[3] - 1);
    
                // calculate yaw/pitch/roll angles
                ypr[0] = atan2(2*q[1]*q[2] - 2*q[0]*q[3], 2*q[0]*q[0] + 2*q[1]*q[1] - 1);
                ypr[1] = atan(gravity[0] / sqrt(gravity[1]*gravity[1] + gravity[2]*gravity[2]));
                ypr[2] = atan(gravity[1] / sqrt(gravity[0]*gravity[0] + gravity[2]*gravity[2]));
    
                // output various components for debugging
                //println("q:\t" + round(q[0]*100.0f)/100.0f + "\t" + round(q[1]*100.0f)/100.0f + "\t" + round(q[2]*100.0f)/100.0f + "\t" + round(q[3]*100.0f)/100.0f);
                //println("euler:\t" + euler[0]*180.0f/PI + "\t" + euler[1]*180.0f/PI + "\t" + euler[2]*180.0f/PI);
                //println("ypr:\t" + ypr[0]*180.0f/PI + "\t" + ypr[1]*180.0f/PI + "\t" + ypr[2]*180.0f/PI);
                */
            }
        }
    }
}
}

void drawCylinder(float topRadius, float bottomRadius, float tall, int sides) {
float angle = 0;
float angleIncrement = TWO_PI / sides;
beginShape(QUAD_STRIP);
for (int i = 0; i < sides + 1; ++i) {
vertex(topRadiuscos(angle), 0, topRadiussin(angle));
vertex(bottomRadiuscos(angle), tall, bottomRadiussin(angle));
angle += angleIncrement;
}
endShape();

// If it is not a cone, draw the circular top cap
if (topRadius != 0) {
    angle = 0;
    beginShape(TRIANGLE_FAN);
    
    // Center point
    vertex(0, 0, 0);
    for (int i = 0; i < sides + 1; i++) {
        vertex(topRadius * cos(angle), 0, topRadius * sin(angle));
        angle += angleIncrement;
    }
    endShape();
}

// If it is not a cone, draw the circular bottom cap
if (bottomRadius != 0) {
    angle = 0;
    beginShape(TRIANGLE_FAN);

    // Center point
    vertex(0, tall, 0);
    for (int i = 0; i < sides + 1; i++) {
        vertex(bottomRadius * cos(angle), tall, bottomRadius * sin(angle));
        angle += angleIncrement;
    }
    endShape();
}
}

Hi! First of all can you format the code with </> ?

I advise you to take a look at the guideline. I want to know, if the problem is specific to serial + toxiclibs or its separable. At the moment it’s difficult to debug for others because we don’t have the same hardware.

1 Like

Hello,

Please format your code as per:
https://discourse.processing.org/faq#format-your-code
I can’t look at it as is.

This appears to be a variation of code by Jeff Rowberg:
i2cdevlib/MPUplane.pde at master · jrowberg/i2cdevlib · GitHub

You should always credit the original author of the code.
There is a section titled “Post Only Your Own Stuff” here:
FAQ - Processing Foundation

Are you able to display just the obj file in your data folder?

Modified example for P3D from:

I had to scale it and rotate it and work around a bug:

PShape s;

void setup() 
  {
  size(500, 500, P3D);
  // The file "bot.obj" must be in the data folder
  // of the current sketch to load successfully
  s = loadShape("biplane.obj");
  s.rotate(TAU/2, 0, 0, 1);  //Fix for a bug in Processing to rotate around an axis
  s.scale(25, 25, 25);
  }

void draw() 
  {
  background(204);
  lights();
  translate(width/2, height/2);
  shape(s, 0, 0);
  }
  }

Works (some modifications) for teapot.obj as well:

:)

1 Like

No I am unable to display anything in there

Is your device connected when you run the processing sketch? I’ve done that before and then realize the sketch won’t work until I connect the device. Can you see output in the Serial monitor in Arduino IDE?

glv’s example sketch isn’t relying on serial.

One more thought, what is your device name? Is this linux? If so, go to /dev directory when you are connected and see your device name. For example, it might be /dev/ttyUSB0 or /dev/ttyACM0, etc. depending on the serial chip your Arduino uses.

If you format your code I can test it:
https://discourse.processing.org/faq#format-your-code
I won’t look at it as is.

:)

ok i thjink i did it

Yes my device is connected when I run the code, and I am using mac osx big sur

Try to copy your code and run it and you will see why I am unable to test it.

If you properly format your code as per the link I provided anyone should be able to copy it and run it without errors.

I did format it though, or did my code not paste correctly or something?

Well, I asked if you are able to run serial monitor in the Arduino IDE and see output because from what I read, on a mac, if it can do that then Processing makes use of the same driver. A blank screen seems to suggest that Processing isn’t connecting.

Yes, I am able to see all the ypr info in the serial monitor of arduino.

Hello,

Take a good look at what you posted in the topic and scrutinize it; there is a long line that shows up in red.

Try to copy it from the forum and back into Processing and you will see the issues.

You can edit the original post.
Format it (original working code) in Processing first.
Test it and ensure that it is working code.
Paste it again as per instructions on how to “Format new code”.
If you “Format existing code” it may not work properly.

:)

Can you try your sketch(using appropriate device port name)on a different platform like windows10? If it works on Windows then you know your code is good and working. If it doesn’t work there either, then go to another area of the troubleshooting flowchart. :wink:

ok I’ll try it tonight Thanks.