in/out usb for detection

Integration and Hardware

ieee

in Integration and Hardware • 17 days ago

good morning every one;

i start with "processing" and i use the 32 bits version on my win xp computer..

i try to detect my arduino uno who's got the mpu5060 interface (may be i'm not in the right post!?)

the processing programm did not detect the mpu values sent...

my cube stay without signals...

import processing.serial.*;

Serial myPort; // Create object from Serial class

final String serialPort = "com5"; // replace this with your serial port. On windows you will need something like "COM1".

float [] q = new float [4];

float [] hq = null;

float [] Euler = new float [3]; // psi, theta, phi

int lf = 10; // 10 is '\n' in ASCII

byte[] inBuffer = new byte[22]; // this is the number of chars on each line from the Arduino (including /r/n)

PFont font;

final int VIEW_SIZE_X = 1024, VIEW_SIZE_Y = 768;

void setup()

{

size(VIEW_SIZE_X, VIEW_SIZE_Y, P3D);

myPort = new Serial(this, serialPort, 115200);

// The font must be located in the sketch's "data" directory to load successfully

font = loadFont("CourierNew36.vlw");

float [] axis = new float[3];

axis[0] = 0.0;

axis[1] = 0.0;

axis[2] = 1.0;

float angle = PI/2.0;

hq = quatAxisAngle(axis, angle);

hq = new float[4];

hq[0] = 0.0;

hq[1] = 0.0;

hq[2] = 0.0;

hq[3] = 1.0;

delay(100);

myPort.clear();

myPort.write("1");

}

float decodeFloat(String inString) {

byte [] inData = new byte[4];

if(inString.length() == 8) {

inData[0] = (byte) unhex(inString.substring(0, 2));

inData[1] = (byte) unhex(inString.substring(2, 4));

inData[2] = (byte) unhex(inString.substring(4, 6));

inData[3] = (byte) unhex(inString.substring(6, 8));

}

int intbits = (inData[3] << 24) | ((inData[2] & 0xff) << 16) | ((inData[1] & 0xff) << 8) | (inData[0] & 0xff);

return Float.intBitsToFloat(intbits);

}

void readQ() {

if(myPort.available() >= 18) {

String inputString = myPort.readStringUntil('\n');

//print(inputString);

if (inputString != null && inputString.length() > 0) {

String [] inputStringArr = split(inputString, ",");

if(inputStringArr.length >= 5) { // q1,q2,q3,q4,\r\n so we have 5 elements

q[0] = decodeFloat(inputStringArr[0]);

q[1] = decodeFloat(inputStringArr[1]);

q[2] = decodeFloat(inputStringArr[2]);

q[3] = decodeFloat(inputStringArr[3]);

}

void buildBoxShape() {

//box(60, 10, 40);

noStroke();

beginShape(QUADS);

//Z+ (to the drawing area)

fill(#00ff00);

vertex(-30, -5, 20);

vertex(30, -5, 20);

vertex(30, 5, 20);

vertex(-30, 5, 20);

//Z-

fill(#0000ff);

vertex(-30, -5, -20);

vertex(30, -5, -20);

vertex(30, 5, -20);

vertex(-30, 5, -20);

//X-

fill(#ff0000);

vertex(-30, -5, -20);

vertex(-30, -5, 20);

vertex(-30, 5, 20);

vertex(-30, 5, -20);

//X+

fill(#ffff00);

vertex(30, -5, -20);

vertex(30, -5, 20);

vertex(30, 5, 20);

vertex(30, 5, -20);

//Y-

fill(#ff00ff);

vertex(-30, -5, -20);

vertex(30, -5, -20);

vertex(30, -5, 20);

vertex(-30, -5, 20);

//Y+

fill(#00ffff);

vertex(-30, 5, -20);

vertex(30, 5, -20);

vertex(30, 5, 20);

vertex(-30, 5, 20);

endShape();

}

void drawCube() {

pushMatrix();

translate(VIEW_SIZE_X/2, VIEW_SIZE_Y/2 + 50, 0);

scale(5,5,5);

// a demonstration of the following is at

// http://www.varesano.net/blog/fabio/ahrs-sensor-fusion-orientation-filter-3d-graphical-rotating-cube

rotateZ(-Euler[2]);

rotateX(-Euler[1]);

rotateY(-Euler[0]);

buildBoxShape();

popMatrix();

}

void draw() {

background(#000000);

fill(#ffffff);

readQ();

if(hq != null) { // use home quaternion

quaternionToEuler(quatProd(hq, q), Euler);

text("Disable home position by pressing \"n\"", 20, VIEW_SIZE_Y - 30);

}

else {

quaternionToEuler(q, Euler);

text("Point FreeIMU's X axis to your monitor then press \"h\"", 20, VIEW_SIZE_Y - 30);

}

textFont(font, 20);

textAlign(LEFT, TOP);

text("Q:\n" + q[0] + "\n" + q[1] + "\n" + q[2] + "\n" + q[3], 20, 20);

text("Euler Angles:\nYaw (psi) : " + degrees(Euler[0]) + "\nPitch (theta): " + degrees(Euler[1]) + "\nRoll (phi) : " + degrees(Euler[2]), 200, 20);

drawCube();

}

void keyPressed() {

if(key == 'h') {

println("pressed h");

// set hq the home quaternion as the quatnion conjugate coming from the sensor fusion

hq = quatConjugate(q);

}

else if(key == 'n') {

println("pressed n");

hq = null;

}

// See Sebastian O.H. Madwick report

// "An efficient orientation filter for inertial and intertial/magnetic sensor arrays" Chapter 2 Quaternion representation

void quaternionToEuler(float [] q, float [] euler) {

euler[0] = atan2(2 * q[1] * q[2] - 2 * q[0] * q[3], 2 * q[0]*q[0] + 2 * q[1] * q[1] - 1); // psi

euler[1] = -asin(2 * q[1] * q[3] + 2 * q[0] * q[2]); // theta

euler[2] = atan2(2 * q[2] * q[3] - 2 * q[0] * q[1], 2 * q[0] * q[0] + 2 * q[3] * q[3] - 1); // phi

}

float [] quatProd(float [] a, float [] b) {

float [] q = new float[4];

q[0] = a[0] * b[0] - a[1] * b[1] - a[2] * b[2] - a[3] * b[3];

q[1] = a[0] * b[1] + a[1] * b[0] + a[2] * b[3] - a[3] * b[2];

q[2] = a[0] * b[2] - a[1] * b[3] + a[2] * b[0] + a[3] * b[1];

q[3] = a[0] * b[3] + a[1] * b[2] - a[2] * b[1] + a[3] * b[0];

return q;

}

// returns a quaternion from an axis angle representation

float [] quatAxisAngle(float [] axis, float angle) {

float [] q = new float[4];

float halfAngle = angle / 2.0;

float sinHalfAngle = sin(halfAngle);

q[0] = cos(halfAngle);

q[1] = -axis[0] * sinHalfAngle;

q[2] = -axis[1] * sinHalfAngle;

q[3] = -axis[2] * sinHalfAngle;

return q;

}

// return the quaternion conjugate of quat

float [] quatConjugate(float [] quat) {

float [] conj = new float[4];

conj[0] = quat[0];

conj[1] = -quat[1];

conj[2] = -quat[2];

conj[3] = -quat[3];

return conj;

}

is there a little prog i can use to make shure my processing receive rightly the data from my usb port ?

thank you for...

best...

in/out usb for detection

Statistics

Tags

Actions

Related Posts