We are about to switch to a new forum software. Until then we have removed the registration on this forum.
Hi,
First time I'm working with arduino processing. Now I was playing arround with the Arduino MPU 6050 gyroscopic sensor. For this I used the Teapot libarary to rotate a box. This box rotates arround his gravity of center. Now I would like to change this So it doesn't rotate arround it center of gravity but arround 1 of his sides.
Does anybody know how to change this?
Attached is the code. Kind regards
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 synced = 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(1920, 600, OPENGL); 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 = Serial.list()[0];
// get a specific serial port (use EITHER this OR the first-available code above)
String portName = "COM3";
// 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]);
// Box
fill(255,0,0,200);
box(10,10,200);
popMatrix();
}
void serialEvent(Serial port) { interval = millis(); while (port.available() > 0) { int ch = port.read();
if (synced == 0 && ch != '$') return; // initial synchronization - also used to resync/realign if needed
synced = 1;
print ((char)ch);
if ((serialCount == 1 && ch != 2)
|| (serialCount == 12 && ch != '\r')
|| (serialCount == 13 && ch != '\n')) {
serialCount = 0;
synced = 0;
return;
}
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);
*/
}
}
}
}
Answers
You'll get a better response if you format your code. Here's how:
http://forum.processing.org/two/discussion/8045/how-to-format-code-and-text