 
 void loop()
 {
-  // fromRC100();
-
-  if (Serial.available())
-  {
-    MUTEX::wait();
-    fromProcessing(Serial.readStringUntil('\n'));
-    MUTEX::release();
-  }
+  getData(0.100f);
+
+
+  // if (Serial.available())
+  // {
+  //   MUTEX::wait();
+  //   fromProcessing(Serial.readStringUntil('\n'));
+  //   MUTEX::release();
+  // }
 
   // switch(seq)
   // {

    loop();
    showLedStatus();
  }
}
  initManipulator();

  connectProcessing();
  connectRC100();

  // goal_position.push_back(0.0);
  // goal_position.push_back(0.0);
  // goal_position.push_back(0.0);
  // goal_position.push_back(0.0);

  // goal_pose.position = OM_MATH::makeVector3(-0.050, 0.0, 0.203);
  // goal_pose.orientation = Eigen::Matrix3f::Identity();

  initThread();
  startThread();
