#include <I2Cdev.h>
#include <MPU60X0.h>
#include <EEPROM.h>
//#define DEBUG
#include "DebugUtils.h"
//#include "CommunicationUtils.h"
#include "FreeIMU.h"
#include <Wire.h>
#include <SPI.h>
int raw_values[9];
float ypr[3]; // yaw pitch roll
FreeIMU TRI_IMU
= FreeIMU();
long time;
long time2;
long initZ=0;
int initZi=0;
int LEDi;
void setup() {
Serial
.begin(115200);
Wire
.begin();
TRI_IMU.init
(true);
// LED
pinMode(13, OUTPUT);
time=millis();
time2=millis();
while
(millis()
-time <30000){
TRI_IMU.getRawValues(raw_values);
TRI_IMU.getYawPitchRoll(ypr);
// Serial.print("x");
Serial
.print
(int(ypr[2]));
Serial
.print
(",");
Serial
.print
(int(ypr[1]));
Serial
.print
(",");
Serial
.print
(int(raw_values[5])/5);
Serial
.println
(",");
initZ+=int(raw_values[5]/5);
initZi++;
if
(millis()
- time2 > 50 ){
LEDi++;
if (LEDi == 1) {
digitalWrite(13, HIGH);
}
if (LEDi == 2) {
digitalWrite(13, LOW);
LEDi
= 0;
}
time2
= millis();
}
}
initZ=initZ/initZi;
}
void loop() {
TRI_IMU.getRawValues(raw_values);
TRI_IMU.getYawPitchRoll(ypr);
Serial.print("z");
Serial.print((raw_values[5]/5)-initZ);
Serial.print("x");
Serial.print(int(ypr[2]*100));
Serial.print("y");
Serial.print(int(ypr[1]*100));
Serial.println("e");
if (millis() - time2 > 500 ){
LEDi++;
if (LEDi == 1) {
digitalWrite(13, HIGH);
}
if (LEDi == 2) {
digitalWrite(13, LOW);
LEDi = 0;
}
time2 = millis();
}
}