thanks for the reply, heres the full arduino code, I am highlighting what I added (thats not from the tutorials I listed)
////////////////////////////////////////////////////////////
// Arduino firmware for use with FreeSixCube processing example
////////////////////////////////////////////////////////////
#include <FreeSixIMU.h>
#include <FIMU_ADXL345.h>
#include <FIMU_ITG3200.h>
#define DEBUG
#ifdef DEBUG
#include "DebugUtils.h"
#endif
#include "CommunicationUtils.h"
#include "FreeSixIMU.h"
#include <Wire.h>
float q[4]; //hold q values
// Set the FreeIMU object
FreeSixIMU my3IMU = FreeSixIMU();
void setup() {
Serial.begin(115200);
Wire.begin();
delay(5);
my3IMU.init();
delay(5);
}
void loop() {
my3IMU.getQ(q);
serialPrintFloatArr(q, 4);
// Declare variables that will store values read from each sensor
float channels[5];
//read input pins with analog values
channels[0] = analogRead(A0);
channels[1] = analogRead(A1);
channels[2] = analogRead(A2);
channels[3]= analogRead(A3);
channels[4] = analogRead(A4);
//print value on the serial monitor screen
Serial.print(" ("); Serial.print(channels[0]);
Serial.print(","); Serial.print(channels[1]);
Serial.print(","); Serial.print(channels[2]);
Serial.print(","); Serial.print(channels[3]);
Serial.print(","); Serial.println(channels[4]);
Serial.print(")");
Serial.println(""); //line break
delay(60);
}
and here is the full processing code, NOTE: its not finished yet since I am still working on the parts I mentioned above
I marked what I added, most of this code is not mine
import processing.serial.*;
Serial myPort; // Create object from Serial class
final String serialPort = COM4; // 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
// THIS IS MY CODE
float [] channels = new float[5] //END OF MY CODE
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 = 800, VIEW_SIZE_Y = 600;
void setup()
{
size(VIEW_SIZE_X, VIEW_SIZE_Y, P3D);
myPort = new Serial(this, serialPort, 115200);
font = createFont("Courier", 32);
/*
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");
// THIS IS MY CODE
smooth();
rectangle1 = createShape(RECT, 50, 50,100,50);
rectangle2 = createShape(RECT, 175,50,100,50);
rectangle3 = createShape(RECT, 300,50,100,50);
rectangle4 = createShape(RECT, 425,50,100,50);
}
// END OF MY CODE
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]);
//MY CODE STARTS
channels[0] = inputStringArr[4]
channels[1] = inputStringArr[5]
channels[2] = inputStringArr[6]
channels[3] = inputStringArr[7]
channels[4] = inputStringArr[8]
// MY CODE ENDS
}
}
}
}
void readS() {
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
s[0] = decodeFloat(inputStringArr[0]);
s[1] = decodeFloat(inputStringArr[1]);
s[2] = decodeFloat(inputStringArr[2]);
s[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();
// THIS IS MY CODE
// DRAW THE BOXES THAT REPRESENT EACH CHANNEL AND THEIR RESPONSE TO THE CHANNEL VALUE
if(channel[0]>799){
rectangle1.setFill(color(0,255,0));}
else{
rectangle1.setFill(color(255,0,0))
}
shape(rectangle1);
shape(rectangle2);
if(channel[1]>799){
rectangle2.setFill(color(0,255,0));}
else{
rectangle2.setFill(color(255,0,0))
}
shape(rectangle3);
if(channel[2]>799){
rectangle3.setFill(color(0,255,0));}
else{
rectangle3.setFill(color(255,0,0))
}
shape(rectangle4);
if(channel[3]>799){
rectangle4.setFill(color(0,255,0));}
else{
rectangle4.setFill(color(255,0,0))
}
// END OF MY CODE
}
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;
}