REPLY to "Problem with 3D Box"?
in
Integration and Hardware
•
2 years ago
THIS IS A REPLY TO MY PREVIOUS QUESTION.
For some reason i cant reply or comment to my own question, anyway, yes, everything is sent serially via USB.
This is my processing code, if you need my arduino code i'd be happy to provide it too. Thanks in advance :)
import processing.serial.*;
Serial myPort; // Create object from Serial class
boolean firstSample = true;
float [] RwAcc = new float[3]; //projection of normalized gravitation force vector on x/y/z axis, as measured by accelerometer
float [] Gyro = new float[3]; //Gyro readings
float [] RwGyro = new float[3]; //Rw obtained from last estimated value and gyro movement
float [] Awz = new float[3]; //angles between projection of R on XZ/YZ plane and Z axis (deg)
float [] RwEst = new float[3]; //corrected output values from gyro data and previous estimates
float [] Gyro = new float[3]; //Gyro readings
float [] RwGyro = new float[3]; //Rw obtained from last estimated value and gyro movement
float [] Awz = new float[3]; //angles between projection of R on XZ/YZ plane and Z axis (deg)
float [] RwEst = new float[3]; //corrected output values from gyro data and previous estimates
int lastTime = 0;
int interval = 0;
float wGyro = 10.0;
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)
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; //size for box
final int VIEW_SIZE_X = 600, VIEW_SIZE_Y = 600; //size for box
void setup()
{
size(VIEW_SIZE_X, VIEW_SIZE_Y, P3D); //P3D - Third parameter
myPort = new Serial(this, "COM9", 9600);
font = loadFont("CourierNew36.vlw");
delay(100);
myPort.clear();
myPort.write("start");
}
{
size(VIEW_SIZE_X, VIEW_SIZE_Y, P3D); //P3D - Third parameter
myPort = new Serial(this, "COM9", 9600);
font = loadFont("CourierNew36.vlw");
delay(100);
myPort.clear();
myPort.write("start");
}
float decodeFloat(String inString)
{
//println(inString);
byte [] inData = new byte[4];
inString = inString.substring(2, 10); // discard the leading "f:"
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);
//unhex(inString.substring(0, 8));
return Float.intBitsToFloat(intbits);
}
{
//println(inString);
byte [] inData = new byte[4];
inString = inString.substring(2, 10); // discard the leading "f:"
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);
//unhex(inString.substring(0, 8));
return Float.intBitsToFloat(intbits);
}
void readSensors()
{
if(myPort.available() >= 18)
{
String inputString = myPort.readStringUntil((int) '\n');
if (inputString != null && inputString.length() > 0)
{
String [] inputStringArr = split(inputString, ",");
RwAcc[0] = decodeFloat(inputStringArr[0]);
RwAcc[1] = decodeFloat(inputStringArr[1]);
RwAcc[2] = decodeFloat(inputStringArr[2]);
Gyro[0] = decodeFloat(inputStringArr[3]);
Gyro[1] = decodeFloat(inputStringArr[4]);
Gyro[2] = decodeFloat(inputStringArr[5]);
RwGyro[0] = decodeFloat(inputStringArr[6]);
RwGyro[1] = decodeFloat(inputStringArr[7]);
RwGyro[2] = decodeFloat(inputStringArr[8]);
Awz[0] = decodeFloat(inputStringArr[9]);
Awz[1] = decodeFloat(inputStringArr[10]);
Awz[2] = decodeFloat(inputStringArr[11]);
RwEst[0] = decodeFloat(inputStringArr[0]);
RwEst[1] = decodeFloat(inputStringArr[1]);
RwEst[2] = decodeFloat(inputStringArr[2]);
}
}
}
{
if(myPort.available() >= 18)
{
String inputString = myPort.readStringUntil((int) '\n');
if (inputString != null && inputString.length() > 0)
{
String [] inputStringArr = split(inputString, ",");
RwAcc[0] = decodeFloat(inputStringArr[0]);
RwAcc[1] = decodeFloat(inputStringArr[1]);
RwAcc[2] = decodeFloat(inputStringArr[2]);
Gyro[0] = decodeFloat(inputStringArr[3]);
Gyro[1] = decodeFloat(inputStringArr[4]);
Gyro[2] = decodeFloat(inputStringArr[5]);
RwGyro[0] = decodeFloat(inputStringArr[6]);
RwGyro[1] = decodeFloat(inputStringArr[7]);
RwGyro[2] = decodeFloat(inputStringArr[8]);
Awz[0] = decodeFloat(inputStringArr[9]);
Awz[1] = decodeFloat(inputStringArr[10]);
Awz[2] = decodeFloat(inputStringArr[11]);
RwEst[0] = decodeFloat(inputStringArr[0]);
RwEst[1] = decodeFloat(inputStringArr[1]);
RwEst[2] = decodeFloat(inputStringArr[2]);
}
}
}
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();
}
{
//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); //translate(VIEW_SIZE_X/2, VIEW_SIZE_Y/2 + 50, 0);
scale(4,4,4);
rotateX(-RwEst[0]);
rotateY(RwEst[1]);
rotateZ(RwEst[2]);
buildBoxShape();
popMatrix();
}
{
pushMatrix();
translate(300, 350, 0); //translate(VIEW_SIZE_X/2, VIEW_SIZE_Y/2 + 50, 0);
scale(4,4,4);
rotateX(-RwEst[0]);
rotateY(RwEst[1]);
rotateZ(RwEst[2]);
buildBoxShape();
popMatrix();
}
void draw()
{
readSensors();
background(#000000);
fill(#ffffff);
//display real-time values
textFont(font, 20);
text("RwAcc (G):\n" + RwAcc[0] + "\n" + RwAcc[1] + "\n" + RwAcc[2], 20, 50);
text("Gyro (°/s):\n" + Gyro[0] + "\n" + Gyro[1] + "\n" + Gyro[2], 220, 50);
text("Awz (°):\n" + Awz[0] + "\n" + Awz[1] + "\n" + Awz[2], 420, 50);
text("RwGyro (°/s):\n" + RwGyro[0] + "\n" + RwGyro[1] + "\n" + RwGyro[2], 20, 180);
text("RwEst :\n" + RwEst[0] + "\n" + RwEst[1] + "\n" + RwEst[2], 220, 180);
// display axes
pushMatrix();
translate(450, 250, 0);
stroke(#ffffff);
scale(100, 100, 100);
line(0,0,0,1,0,0);
line(0,0,0,0,-1,0);
line(0,0,0,0,0,1);
line(0,0,0, -RwEst[0], RwEst[1], RwEst[2]);
popMatrix();
drawCube();
}
{
readSensors();
background(#000000);
fill(#ffffff);
//display real-time values
textFont(font, 20);
text("RwAcc (G):\n" + RwAcc[0] + "\n" + RwAcc[1] + "\n" + RwAcc[2], 20, 50);
text("Gyro (°/s):\n" + Gyro[0] + "\n" + Gyro[1] + "\n" + Gyro[2], 220, 50);
text("Awz (°):\n" + Awz[0] + "\n" + Awz[1] + "\n" + Awz[2], 420, 50);
text("RwGyro (°/s):\n" + RwGyro[0] + "\n" + RwGyro[1] + "\n" + RwGyro[2], 20, 180);
text("RwEst :\n" + RwEst[0] + "\n" + RwEst[1] + "\n" + RwEst[2], 220, 180);
// display axes
pushMatrix();
translate(450, 250, 0);
stroke(#ffffff);
scale(100, 100, 100);
line(0,0,0,1,0,0);
line(0,0,0,0,-1,0);
line(0,0,0,0,0,1);
line(0,0,0, -RwEst[0], RwEst[1], RwEst[2]);
popMatrix();
drawCube();
}
1