 
   bmp085.setControl(BMP085_MODE_TEMPERATURE);
 
+  for(int i=0; i<4; i++)
+  {
+    servo[i].attach(SERVO_PIN[i]);
+    servo[i].writeMicroseconds(0);
+  }
+}
+
+double gettime()
+{
+  return global_time_offset + micros() * 1E-6;
 }
 
 Measure& read_data() {

  Serial.println(F(""));
}

int scnt = 0;
Measure ms;
void loop()
{
  piSerial.println("\"A\"");
  read_json();
  ms += read_data();
  scnt++;
  if(scnt == N_SAMPLE)
  {
    scnt = 0;
    ms *= 1.0 / N_SAMPLE;
    /* print_json(ms); */
    ms = Measure();
    delay(1000);
  }
}

#include "HMC5883L.h"
#include "BMP085.h"
#include "ArduinoJson.h"
#include "Servo.h"

MPU6050 mpu6050;
HMC5883L hmc5883l;
BMP085 bmp085;
Measure meas;
Servo servo[4];
const int SERVO_PIN[4] = {9, 10, 11, 12};
int64_t lastMicros = 0;
double global_time_offset = 0;
/* SoftwareSerial piSerial(10, 11); */
#define piSerial Serial
char indata[256];
int motor[4];
int pos;

void setup() {
  // put your setup code here, to run once:
