     publishImuMsg();
     tTime[3] = t;
   }
+
+  // Receive data from RC100 
+  receiveRemoteControlData();
 
   // Check push button pressed for simple test drive
   checkPushButtonState();


  tfs_msg.transform.translation.x = -0.032;
  tfs_msg.transform.translation.y = 0.0;
  tfs_msg.transform.translation.z = 0.068;

  tfbroadcaster.sendTransform(tfs_msg);
}

    wheel_r = 0.0;

  delta_s     = WHEEL_RADIUS * (wheel_r + wheel_l) / 2.0;
  theta       = atan2f(imu.quat[1]*imu.quat[2] + imu.quat[0]*imu.quat[3],
                       0.5f - imu.quat[2]*imu.quat[2] - imu.quat[3]*imu.quat[3]);
  delta_theta = theta - last_theta;

  v = delta_s / step_time;
  w = delta_theta / step_time;
