 void waitForSerialLink(bool isConnected)
 {
   static bool wait_flag = false;
-  
+
   if (isConnected)
   {
     if (wait_flag == false)
-    {      
+    {
       delay(10);
 
       wait_flag = true;

      }

      sprintf(log_msg, "Setup TF on Odometry [%s]", odom_header_frame_id);
      nh.loginfo(log_msg); 

      sprintf(log_msg, "Setup TF on IMU [%s]", imu_frame_id);
      nh.loginfo(log_msg); 

      sprintf(log_msg, "Setup TF on MagneticField [%s]", mag_frame_id);
      nh.loginfo(log_msg); 

      sprintf(log_msg, "Setup TF on JointState [%s]", joint_state_header_frame_id);
      nh.loginfo(log_msg); 

      isChecked = true;
    }


  motor_driver.readEncoder(current_tick[LEFT], current_tick[RIGHT]);

  if (buttons & (1<<0))
  {
    move[LINEAR] = true;
    saved_tick[RIGHT] = current_tick[RIGHT];
