 
   // Calculate and set  refrence wheel angular velocity
   setReferenceVelocity(vl / wheel_radius, vr / wheel_radius);
+}
+
+void setFobModeCallback(const victoria_sensor_msgs::FobMode& fob_mode_msg) {
+  fob_mode = fob_mode_msg.fob_mode;
 }
 
 void doMotorController(void) {

#include <victoria_nav_msgs/Odom2DRaw.h>
#include <victoria_sensor_msgs/DistanceDisplacement1D.h>
#include <victoria_sensor_msgs/EncoderCount.h>
#include <victoria_sensor_msgs/IMURaw.h>

// Teensy pin definitions


  // Register ros subscribers
  ros_nh.subscribe(ros_cmd_vel_sub);
  ros_nh.subscribe(ros_set_fob_mode_sub);
  
  // Initialize and connect to ros
  ros_nh.getHardware()->setBaud(115200);
