 {
   if(piSerial.available() == 0)
     return;
-  int pos = 0;
+  int cnt = 0;
   while(piSerial.available() > 0)
   {
+    cnt++;
     int c = piSerial.read();
-    /* piSerial.print((unsigned int8_t)(c)); */
-    /* piSerial.print(","); */
     indata[pos] = c;
     pos += 1;
-    if(c == '\n' || piSerial.available() <= 0)
+    if(c == '\n')
     {
       indata[pos-1] = '\0';
       pos = 0;
     }
   }
 
+  if(pos > 0) return;
+
+  /* piSerial.print("Total Cnt = "); */
+  /* piSerial.println(cnt); */
+  /* piSerial.print("Raw data = "); */
+  /* piSerial.println(indata); */
+
+  piSerial.println(indata);
+  piSerial.flush();
+
+  //Parse Json
+  StaticJsonBuffer<256> jsonBuffer;
+  JsonObject& root = jsonBuffer.parseObject(indata);
+  if(!root.success()) return;
+  if(!root.containsKey("motor")) return;
+  if(!root["motor"].is<JsonArray&>()) return;
+  JsonArray& marr = root["motor"];
+  for(int i=0; i<4; i++)
+    motor[i] = marr[i];
+  
+  /* for(int i=0; i<4; i++) */
+  /* { */
+    /* piSerial.print(motor[i]); */
+    /* piSerial.print(","); */
+  /* } */
   /* piSerial.println(""); */
-  /* piSerial.println(cnt); */
-  piSerial.print("Data : ");
-  piSerial.println(indata);
-
+  /* piSerial.flush(); */
+}
+
+void set_motor()
+{
+  for(int i=0; i<4; i++)
+  {
+    servo[i].writeMicroseconds(motor[i]);
+  }
 }
 
 void print_json(Measure &meas)
 {
   StaticJsonBuffer<256> jsonBuffer;
   JsonObject& root = jsonBuffer.createObject();
-  root["time"] = micros();
+  root["time"].set(gettime(), 6);
+  root["timez"].set(global_time_offset, 6);
 
   JsonArray& arr_acc = root.createNestedArray("accel");
   arr_acc.add(meas.accel.x, 3);


  bmp085.setControl(BMP085_MODE_TEMPERATURE);

}

Measure& read_data() {

  root["pressure"].set(meas.pressure, 0);

  root.printTo(piSerial);
  piSerial.println("");
  
  piSerial.flush();
}

void print_data(Measure &meas)
{
  Serial.print(F("Time : "));
  Serial.print(gettime(), 6);
  Serial.println(F(""));

  Serial.print(F("accel : "));
