 }
 
 void loop()
-{
+{	    	
 	ACC_getADC();
 	Gyro_getADC();
+	#if (defined(PRINTANGLE) || defined(PRINTANGLESUM))
+		static uint16_t previousT;
+ 		uint16_t currentT = micros();
+ 		float scale, deltaGyroAngle[3];
+ 		float static angleSum[3];
+ 		byte axis = 0;
+
+ 		scale = (currentT - previousT) * GYRO_SCALE;
+ 		previousT = currentT;
+
+ 		for (axis = 0; axis < 3; axis++){
+    		deltaGyroAngle[axis] = gyroADC[axis]  * scale * RADTODEG;
+    	}
+
+    #if defined(PRINTANGLESUM)
+    	angleSum[0] += deltaGyroAngle[0];
+    	if(Serial.available() > 0)
+    	{
+    		angleSum[0] = 0;
+    		Serial.read();
+    	}
+    	Serial.println(angleSum[0]);
+   	#endif
+
+   	#if defined(PRINTANGLE)
+    	printAngleData(deltaGyroAngle);
+    #endif
+
+    	delayMicroseconds(650); // emperical delay between readings 
+
+	#endif
 	#if defined(PRINTDATA) 
 		printRawData();
 	#elif defined(SAMPLERATE)

// ************************************************************************************************************
// Helper Functions
// ************************************************************************************************************
void printRawData() //prints raw data over serial port in format "[gX     gY     gZ      aX      aY      aZ]\n"
{
	Serial.print("[");
	Serial.print(gyroADC[0]);
	Serial.print("\t\t");
	Serial.print(gyroADC[1]);
	Serial.print("\t\t");
	Serial.print(gyroADC[2]);
	Serial.print("\t\t");
	Serial.print(accADC[1]);
	Serial.print("\t\t");
	Serial.print(accADC[2]);
	Serial.print("\t\t");
	Serial.print(accADC[3]);
	Serial.println("]");
}

#if defined(SAMPLERATE)
		long ptime = 0;

// UNCOMMENT ONE OF THE FOUR OPTIONS BELOW

//#define PRINTDATA // uncomment to display raw data over serial port in format [gX     gY     gZ      aX      aY      aZ]
//#define PRINTANGLE // identical to PRINTDATA but gyro data is in delta degrees [gX     gY     gZ      aX      aY      aZ]
//#define PRINTANGLESUM // uncomment to display simple intigration of gyro over X axis in degrees
//#define SAMPLERATE // uncomment to display max sampling rate over serial port updates every 2 seconds with sampleing rate in Hz

#include <avr/eeprom.h>

