 
   // Set the motors to the current speed
   setMotors(motor_left_speed, motor_right_speed);
-
-  // Current time is now the previous time
-  previous_time = current_time;
 }
 
 /*

double y;
double th;

ros::Time current_time;
ros::Time previous_time;

char ros_odom_header_frame_id[] = "/odom";
char ros_odom_child_frame_id[] = "/base_link";


  // Update ROS
  ros_nh.spinOnce();
  
  // If no cmd_vel messages withing threshold time, something is wrong, stop the robot.
  cmd_vel_timeout_stop = (current_time.toSec() - last_cmd_vel_time.toSec()) > CMD_VEL_TIMEOUT_THRESHOLD;
  if (cmd_vel_timeout_stop) {
    motor_left_speed = 0;
