import processing.serial.*; import toxi.geom.*; import toxi.processing.*; // NOTE: requires ToxicLibs to be installed in order to run properly. // 1. Download from http://toxiclibs.org/downloads // 2. Extract into [userdir]/Processing/libraries // (location may be different on Mac/Linux) // 3. Run and bask in awesomeness // The serial port Serial port; String message; float[] q = new float[4]; Quaternion quat = new Quaternion(1, 0, 0, 0); // New line character in ASCII final char newLine = '\n'; String [] massQ = new String [4]; float[] ypr = new float[3]; void setup() { // Size form 400x400 size(400, 400, P3D); // Open serial port // Replace "COM7" with the COM port on which your arduino is connected port = new Serial(this, "COM7", 9600); } void draw() { // Read and parse incoming serial message serialEvent(); // Set background to black background(0); printQuaternions(); printYawPitchRoll(); // Set position to centre translate(width / 2, height / 2); // Begin object pushMatrix(); float[] axis = quat.toAxisAngle(); rotate(axis[0], axis[2], axis[3], axis[1]); // Draw main body in red drawBody(); // Draw front-facing tip in blue drawCylinder(); // Draw Triangles drawTriangles(); // Draw Quads drawQuads(); // End of object popMatrix(); // Send character 's' to Arduino //port.write('s'); } void serialEvent() { // Read from port until new line (ASCII code 13) message = port.readStringUntil(newLine); if (message != null) { // Split message by commas and store in String array massQ = split(message, ","); q[0] = float(massQ[0]); q[1] = float(massQ[1]); q[2] = float(massQ[2]); q[3] = float(massQ[3]); } // Print values to console print(q[0]); print("\t"); print(q[1]); print("\t"); print(q[2]); print("\t"); print(q[3]); println("\t"); // Set our toxilibs quaternion to new data quat.set(q[0], q[1], q[2], q[3]); } void drawCylinder() { float topRadius = 0; float bottomRadius = 20; float tall = 20; int sides = 8; // Begin object pushMatrix(); translate(0, 0, -120); rotateX(PI/2); fill(0, 0, 255, 200); float angle = 0; float angleIncrement = TWO_PI / sides; beginShape(QUAD_STRIP); for (int i = 0; i < sides + 1; ++i) { vertex(topRadius * cos(angle), 0, topRadius * sin(angle)); vertex(bottomRadius * cos(angle), tall, bottomRadius * sin(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(); } popMatrix(); } void drawBody() { fill(255, 0, 0, 200); box(10, 10, 200); } void drawTriangles() { // Draw wings and tail fin in green fill(0, 255, 0, 200); beginShape(TRIANGLES); // Wing top layer vertex(-100, 2, 30); vertex(0, 2, -80); vertex(100, 2, 30); // Wing bottom layer vertex(-100, -2, 30); vertex(0, -2, -80); vertex(100, -2, 30); // Tail left layer vertex(-2, 0, 98); vertex(-2, -30, 98); vertex(-2, 0, 70); // Tail right layer vertex( 2, 0, 98); vertex( 2, -30, 98); vertex( 2, 0, 70); endShape(); } void drawQuads() { 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(); } void printQuaternions() { // Set text mode to shape textMode(SHAPE); textSize(13); fill(255, 255, 255); text("Quaternions:", 20, 20, 10); text(q[0], 20, 40, 10); text(q[1], 20, 60, 10); text(q[2], 20, 80, 10); text(q[3], 20, 100, 10); } void printYawPitchRoll() { // 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) * 57.2; ypr[1] = atan2(2 * q[2] * q[3] - 2 * q[0] * q[1], 2 * q[0] * q[0] + 2 * q[3] * q[3] - 1) * 57.2; ypr[2] = -atan2(2 * (q[0] * q[2] - q[1] * q[3]), 1 - 2 * (q[2] * q[2] + q[1] *q[1])) * 57.2; text("Yaw:", 150, 20, 10); text(ypr[0], 150, 40, 10); text("Pitch:", 220, 20, 10); text(ypr[1], 220, 40, 10); text("Roll:", 290, 20, 10); text(ypr[2], 290, 40, 10); }