arduino with accelerometer and gyroscope

[alvinthen]

hi, i am doing a project using arduino chipkit 32 as my microcontroller, accelerometer and gyroscope as my sensors, and interfacing it to labview. my objective of this project is to obtain the result generated from the accelerometer and gyroscope and display it on a graph using labview. i manage to get the arduino to establish connection to the labview and manage to get some data going through them. But i have recently found out that the data transferred is kind of unstable. Please help me. 

#include <Wire.h>
#include <SPI.h>
#include <Servo.h>
#include "LabVIEWInterface.h"

#include <math.h>
#define ACC_X 2
#define ACC_Y 1
#define GYR_Z 0
#define M_PI 3.14159265358979323846

int STD_LOOP_TIME = 9;
int lastLoopTime = STD_LOOP_TIME;
int lastLoopUsefulTime = STD_LOOP_TIME;
unsigned long loopStartTime = 0;
int actAngle;
double actDegree;
double Yvalue;
double Gvalue;
int ACC_angle;
int GYRO_rate;
int Gyro_offset = 0;
int yZero = 0;
// Kalman filter module
float Q_angle = 0.001;
float Q_gyro = 0.003;
float R_angle = 0.03;

float x_angle = 0;
float x_bias = 0;
float P_00 = 0, P_01 = 0, P_10 = 0, P_11 = 0;
float dt, y, S;
float K_0, K_1;

float degree = 0;

float kalmanCalculate(float newAngle, float newRate,int looptime) {
dt = float(looptime)/1000;
x_angle += dt * (newRate - x_bias);
P_00 += - dt * (P_10 + P_01) + Q_angle * dt;
P_01 += - dt * P_11;
P_10 += - dt * P_11;
P_11 += + Q_gyro * dt;

y = newAngle - x_angle;
S = P_00 + R_angle;
K_0 = P_00 / S;
K_1 = P_10 / S;

x_angle += K_0 * y;
x_bias += K_1 * y;
P_00 -= K_0 * P_00;
P_01 -= K_0 * P_01;
P_10 -= K_1 * P_00;
P_11 -= K_1 * P_01;

return x_angle;
}

float angleCalculate(float newDegree) {
degree = ((actAngle * 5.0)/ 1023.0) * (180.0 / M_PI);
return degree;
}

/*********************************************************************************
** setup()
**
** Initialize the Arduino and setup serial communication.
**
** Input: None
** Output: None
*********************************************************************************/
void setup()
{
// Initialize Serial Port With The Default Baud Rate
syncLV();

pinMode(2, OUTPUT);
pinMode(3, OUTPUT);
pinMode(4, OUTPUT);
pinMode(5, OUTPUT);
pinMode(6, OUTPUT);
pinMode(7, OUTPUT);
Serial.begin(115200);
delay(2000);
Gyro_offset += analogRead(GYR_Z);
Gyro_offset += analogRead(GYR_Z);
Gyro_offset += analogRead(GYR_Z);
Gyro_offset += analogRead(GYR_Z);
Gyro_offset += analogRead(GYR_Z);
Gyro_offset += analogRead(GYR_Z);
Gyro_offset += analogRead(GYR_Z);
Gyro_offset += analogRead(GYR_Z);
Gyro_offset += analogRead(GYR_Z);
Gyro_offset += analogRead(GYR_Z);

Gyro_offset = Gyro_offset/10;
yZero = analogRead(ACC_Y);

}

/*********************************************************************************
** loop()
**
** The main loop. This loop runs continuously on the Arduino. It
** receives and processes serial commands from LabVIEW.
**
** Input: None
** Output: None
*********************************************************************************/
void loop()
{
// Check for commands from LabVIEW and process them.
checkForCommand();

//Sensor aquisition & filtering
ACC_angle = getAccAngle(); // in Quids
GYRO_rate = getGyroRate(); // in Quids/seconds
actAngle = kalmanCalculate(ACC_angle, GYRO_rate, lastLoopTime); // calculate filtered Angle
actDegree = angleCalculate(actAngle);
Yvalue = (((getAccAngle() * 5.0)/ 1023.0) * (180.0 / M_PI));

//looptime control
lastLoopUsefulTime = millis()-loopStartTime;
if(lastLoopUsefulTime<STD_LOOP_TIME)
delay(STD_LOOP_TIME-lastLoopUsefulTime);
lastLoopTime = millis() - loopStartTime;
loopStartTime = millis();

// Print Debug info
// Serial.print("Raw value : ");
//Serial.print(actAngle);
// Serial.print("\t\t");
// Serial.print("Angle : ");

// ASCII 10 = newline character,
// used to separate the data strings

Serial.println(Yvalue); // send sensor 2 value as decimal number
// send ASCII string "10"
// ASCII 10 = newline character,
// used to separate the data strings

// ASCII 10 = newline character,
// used to separate the data strings

// Display on LED and vibration motor
if (actDegree>80)// blue upright
{
analogWrite(4, 255);
analogWrite(7, 255);
}
else if (actDegree>50 && actDegree<76)//green 1st degree bend
{
analogWrite(2, 255);
analogWrite(5, 255);
}
else if (actDegree>20 && actDegree<46)// red 2nd degree bend
{
analogWrite(3, 255);
analogWrite(6, 255);
}
else
{
analogWrite(1, 0);
analogWrite(2, 0);
analogWrite(3, 0);
analogWrite(4, 0);
analogWrite(5, 0);
analogWrite(6, 0);
analogWrite(7, 0);
}
}
int getAccAngle() {
return (analogRead(ACC_Y)-yZero); // in Quid: 1024/(2*PI))
}
int getGyroRate() { // ARef=5V, Gyro sensitivity=25mV/(deg/sec)
return int(((analogRead(GYR_Z)-Gyro_offset))*1/90); // in quid/sec:(1024/360)/1024 * 5/0.025)
}