   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.motor_speed_left = motor_left_speed;
  teensy_debug_msg.motor_speed_right = motor_right_speed;
  teensy_debug_msg.last_cmd_vel_time = last_cmd_vel_time;
  teensy_debug_msg.encoder_left = encoder_left_pos;
  teensy_debug_msg.encoder_right = encoder_right_pos;
  teensy_debug_msg.last_encoder_read_time = last_encoder_read_time;

#include <victoria_sensor_msgs/IMURaw.h>

// Teensy pin definitions
const int FOB_STOP_1_PIN(2);       // Teensy digital pin 2
const int FOB_STOP_2_PIN(3);       // Teensy digital pin 3
const int FOB_STOP_3_PIN(4);       // Teensy digital pin 4
const int FOB_STOP_4_PIN(5);       // Teensy digital pin 5
const int E_STOP_PIN(6);           // Teensy digital pin 6
const int BLINK_PIN(13);           // Teensy digital pin 13
const int ENCODER_LEFT_PIN_1(29);  // Teensy digital pin 29
const int ENCODER_LEFT_PIN_2(30);  // Teensy digital pin 30
