   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) {
-    setReferenceVelocity(0, 0);
+  cmd_vel_timeout_exceeded = (current_time.toSec() - last_cmd_vel_time.toSec()) > cmd_vel_timeout_threshold;
+}
+
+/**
+ * Checks the state input for the estop control.
+ */
+bool checkEStop(void) {
+  return (digitalRead(E_STOP_PIN) == LOW);
+}
+
+/**
+ * Checks the state of the input for the given
+ * fob stop.
+ */
+bool checkFobStop(byte fob_stop_to_check) {
+  if (fob_mode != fob_stop_to_check) {
+    return false;
+  }
+  
+  switch(fob_stop_to_check) {
+    case 1:
+      return (digitalRead(FOB_STOP_1_PIN) == LOW);
+      
+    case 2:
+      return (digitalRead(FOB_STOP_2_PIN) == LOW);
+      
+    case 3:
+      return (digitalRead(FOB_STOP_3_PIN) == LOW);
+      
+    case 4:
+      return (digitalRead(FOB_STOP_4_PIN) == LOW);
+      
+    default:
+      return false;
   }
 }
 

  teensy_debug_msg.w_right = getAngularVelocityFromSamples(samples_wr);
  teensy_debug_msg.bumper_left = analogRead(BUMPER_LEFT_PIN);
  teensy_debug_msg.bumper_right = analogRead(BUMPER_RIGHT_PIN);
  ros_teensy_debug_pub.publish(&teensy_debug_msg);
}


  ros_bumper_left_msg.header.frame_id = "/bumper_left";
  ros_bumper_right_msg.header.frame_id = "/bumper_right";
  last_cmd_vel_time = current_time;
  cmd_vel_timeout_exceeded = false;
  last_encoder_read_time = current_time;
  encoder_left_pos = 0;
  encoder_right_pos = 0;
  motor_left_speed = 0.0;
  motor_right_speed = 0.0;
  external_stop = false;
  fob_mode = 0;
  reference_wl = 0.0;
  reference_wr = 0.0;
  pose.x = 0.0;
