This is the code i finally got it working with well i say working working very badly. If anyone can spot anything drastically wrong please feel free to point it out
#include <Wire.h>
#include <ADXL345.h>
const float alpha = 0.5;
double fXg = 0;
double fYg = 0;
double fZg = 0;
ADXL345 acc;
void setup()
{
acc.begin();
Serial.begin(9600);
delay(100);
}
void loop()
{
double pitch, roll, Xg, Yg, Zg;
acc.read(&Xg, &Yg, &Zg);
//Low Pass Filter
fXg = Xg * alpha + (fXg * (1.0 - alpha));
fYg = Yg * alpha + (fYg * (1.0 - alpha));
fZg = Zg * alpha + (fZg * (1.0 - alpha));
//Roll & Pitch Equations
roll = (atan2(-fYg, fZg)180.0)/M_PI;
pitch = (atan2(fXg, sqrt(fYgfYg + fZg*fZg))*180.0)/M_PI;
Serial.print(pitch);
Serial.print(":");
Serial.println(roll);
delay(10);
}
And this is the Processing code:
import processing.serial.*;
Serial fd;
int pitch = 0;
int roll = 0;
void setup ()
{
size(640, 360, P3D);
//Connect to the corresponding serial port
fd = new Serial(this, Serial.list()[5], 9600);
// Defer callback until new line
fd.bufferUntil('\n');
}
void draw ()
{
//Set background
background(0.5);
pushMatrix();
translate(width/2, height/2, -30);
//Rotate
rotateX(((float)pitch)*PI/180.0);
rotateZ(((float)roll)*PI/180.0);
//Print data
print("Pitch: ");
print(pitch);
print(", Roll: ");
println(roll);
scale(90);
beginShape(QUADS);
fill(0, 255, 0); vertex(-1, 1, 1);
fill(0, 255, 0); vertex( 1, 1, 1);
fill(0, 255, 0); vertex( 1, -1, 1);
fill(0, 255, 0); vertex(-1, -1, 1);
fill(0, 255, 255); vertex( 1, 1, 1);
fill(0, 255, 255); vertex( 1, 1, -1);
fill(0, 255, 255); vertex( 1, -1, -1);
fill(0, 255, 255); vertex( 1, -1, 1);
fill(255, 0, 255); vertex( 1, 1, -1);
fill(255, 0, 255); vertex(-1, 1, -1);
fill(255, 0, 255); vertex(-1, -1, -1);
fill(255, 0, 255); vertex( 1, -1, -1);
fill(255, 255, 0); vertex(-1, 1, -1);
fill(255, 255, 0); vertex(-1, 1, 1);
fill(255, 255, 0); vertex(-1, -1, 1);
fill(255, 255, 0); vertex(-1, -1, -1);
fill(255, 0, 0); vertex(-1, 1, -1);
fill(255, 0, 0); vertex( 1, 1, -1);
fill(255, 0, 0); vertex( 1, 1, 1);
fill(255, 0, 0); vertex(-1, 1, 1);
fill(0, 0, 255); vertex(-1, -1, -1);
fill(0, 0, 255); vertex( 1, -1, -1);
fill(0, 0, 255); vertex( 1, -1, 1);
fill(0, 0, 255); vertex(-1, -1, 1);
endShape();
popMatrix();
}
void serialEvent (Serial fd)
{
// get the ASCII string:
String rpstr = fd.readStringUntil('\n');
if (rpstr != null) {
String[] list = split(rpstr, ':');
pitch = ((int)float(list[0]));
roll = ((int)float(list[1]));
}
}