un esempio grafico ma un poco avanzato è questo:
/**
* 3d visualization of quaternion, and PID regulation
* 27/07/2011by lesto
*/
import processing.serial.*;
import controlP5.*;
Serial myPort; // Create object from Serial class
float [] Q = new float [4];
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 = 600, VIEW_SIZE_Y = 600;
Textfield inputP;
Textfield inputI;
Textfield inputD;
Textfield inputPorizon;
Textfield inputIorizon;
ControlP5 controlP5;
void setup()
{
size(VIEW_SIZE_X, VIEW_SIZE_Y, P3D);
myPort = new Serial(this, "/dev/ttyUSB0", 19200);
font = loadFont("CourierNew36.vlw");
controlP5 = new ControlP5(this);
inputP = controlP5.addTextfield("p",400,100,100,20);
inputI = controlP5.addTextfield("i",400,120,100,20);
inputD = controlP5.addTextfield("d",400,140,100,20);
inputPorizon = controlP5.addTextfield("P",400,160,100,20);
inputIorizon = controlP5.addTextfield("I",400,180,100,20);
// The font must be located in the sketch's "data" directory to load successfully
delay(100);
if (myPort != null) {
myPort.clear();
//myPort.write("start");
}
else {
return;
}
}
void readQ() {
int num=0;
while(myPort.available() > 0 && num < 1000) {
String inputString = myPort.readStringUntil('\n');
// print("letto: ");
// println(inputString);
if (inputString != null && inputString.length() > 0) {
String [] inputStringArr = split(inputString, ' ');
if(inputStringArr.length == 4) {
try {
float d[]=new float[4];
d[0] = Float.parseFloat(inputStringArr[0]);
d[1] = Float.parseFloat(inputStringArr[1]);
d[2] = Float.parseFloat(inputStringArr[2]);
d[3] = Float.parseFloat(inputStringArr[3]);
for (int i=0; i < 4; i++) {
Q[i]=d[i];
}
num++;
}
catch(Exception e) {
//println(e);
//println(inputString);
}
}
else {
if (inputString!=null) {
if (!inputString.equals("\r\n") ) {
print(inputString);
// println(inputString.length());
}
}
}
}
}
//if (num>0)
//println("numero letture"+num);
}
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(300, 350, 0);
scale(5,5,5);
// please, don't ask me why these angles are negated
rotateZ(-Euler[2]);
rotateX(-Euler[1]);
rotateY(-Euler[0]);
buildBoxShape();
popMatrix();
}
void draw() {
controlP5.draw();
readQ();
quaternionToEuler(Q, Euler);
background(#000000);
fill(#ffffff);
textFont(font, 20);
//float temp_decoded = 35.0 + ((float) (temp + 13200)) / 280;
//text("temp:\n" + temp_decoded + " C", 350, 250);
text("Q:\n" + Q[0] + "\n" + Q[1] + "\n" + Q[2] + "\n" + Q[3], 20, 50);
// text("Euler Angles:\npsi : " + degrees(Euler[0]) + "\ntheta: " + degrees(Euler[1]) + "\nphi : " + degrees(Euler[2]), 200, 50);
text("Euler Angles:\npsi : " + Euler[0] + "\ntheta: " + Euler[1] + "\nphi : " + Euler[2], 200, 20);
float e[] = new float[3];
quatToEuler( Q, e );
Euler[0]=e[0];
Euler[1]=e[1];
Euler[2]=e[2];
text("Euler Angles:\npsi : " + degrees(e[0]) + "\ntheta: " + degrees(e[1]) + "\nphi : " + degrees(e[2]), 400, 20);
drawCube();
text("p", 350, 120);
text("i", 350, 140);
text("d", 350, 160);
text("O P", 350, 180);
text("O I", 350, 200);
controlP5.draw();
}
// See Sebastian O.H. Madwick report "An efficient orientation filter for inertial and intertial/magnetic sensor arrays
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 pitch
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 yaw?
}
void quatToEuler(float q1[], float euler[]) {
//float[] euler = new float[3];
//q1.normalize();
double test = q1[0] * q1[1] + q1[2] * q1[3];
if (test > 0.499) { // singularity at north pole
euler[1] = (float) (2 * Math.atan2(q1[0], q1[3]));
euler[2] = (float) (Math.PI / 2);
euler[0] = 0;
//return new Vector3f(euler);
return;
}
if (test < -0.499) { // singularity at south pole
euler[1] = (float) (2 * Math.atan2(q1[0], q1[3]));
euler[2] = (float) -(Math.PI / 2);
euler[0] = 0;
//return new Vector3f(euler);
return;
}
double sqx = q1[0] * q1[0];
double sqy = q1[1] * q1[1];
double sqz = q1[2] * q1[2];
euler[1] = (float) Math.atan2(2 * q1[1] * q1[3] - 2 * q1[0] * q1[2], 1 - 2 * sqy - 2 * sqz);
euler[2] = (float) Math.asin(2 * test);
euler[0] = (float) Math.atan2(2 * q1[0] * q1[3] - 2 * q1[1] * q1[2], 1 - 2 * sqx - 2 * sqz);
//return new Vector3f(euler);
}
void controlEvent(ControlEvent theEvent) {
println("controlEvent: accessing a string from controller '"+theEvent.controller().name()+"': "+theEvent.controller().stringValue());
if (theEvent.controller().stringValue().length() < 10) {
try {
Float.parseFloat( theEvent.controller().stringValue() );
myPort.write("*"+theEvent.controller().name()+":"+theEvent.controller().stringValue()+"\n");
println("Inviato:"+"*"+theEvent.controller().name()+":"+theEvent.controller().stringValue()+"\n");
}
catch(Exception e) {
println("Non inviato:"+theEvent.controller().name()+":"+theEvent.controller().stringValue());
//println(e);
println("Non float o seriale non pronta");
}
}
else {
println("Non inviato:"+theEvent.controller().name()+":"+theEvent.controller().stringValue());
println("Input too long");
}
}
questo codice legge un quaternione (quattro numeri separati da : e poi a capo) e ruota un rettangolo di conseguenza. In oltre ci sono 6 texbox in cui settare un valore, al pèremere di invio il valore viene inviato all'arduino.
