 char ros_odom_header_frame_id[] = "/odom";
 char ros_odom_child_frame_id[] = "/base_link";
 
-// ROS Debug publishers
-#define ENABLE_BUMPER_DEBUG  // Uncomment to publish bumper debug info
-#define ENABLE_ENCODER_DEBUG  // Uncomment to publish encoder debug info
-#define ENABLE_IMU_DEBUG  // Uncomment to publish imu debug info
-#define ENABLE_MAGNETOMETER_DEBUG  // Uncomment to publish magnetometer debug info
-#define ENABLE_TEENSY_DEBUG  // Uncomment to publish teensy debug info
-
 char debug_str[80];
 std_msgs::String ros_debug_msg;
 
-#ifdef ENABLE_BUMPER_DEBUG
 ros::Publisher ros_bumper_debug_pub("bumper_debug", &ros_debug_msg);
-#endif
-
-#ifdef ENABLE_ENCODER_DEBUG
 ros::Publisher ros_encoder_debug_pub("encoder_debug", &ros_debug_msg);
-#endif
-
-#ifdef ENABLE_IMU_DEBUG
 ros::Publisher ros_imu_debug_pub("imu_debug", &ros_debug_msg);
-#endif
-
-#ifdef ENABLE_MAGNETOMETER_DEBUG
 ros::Publisher ros_magnetometer_debug_pub("magnetometer_debug", &ros_debug_msg);
-#endif
-
-#ifdef ENABLE_TEENSY_DEBUG
-ros::Publisher ros_teensy_debug_pub("teensy_debug_info", &ros_debug_msg);
-#endif
 
 // Debug blink
 // Blink duration is in milliseconds.
-#define BLINK_DURATION 500
+const int BLINK_DURATION(500);
 int blink_state = LOW;
   
 void setup() {

double motor_right_speed;

// Victoria constants
#define TICKS_PER_RADIAN 9072 // TODO(mwomack): Verify real value
#define TRACK_RADIUS 0.235  // in meters
#define WHEEL_RADIUS 0.127  // in meters
#define MAX_SPEED 4.71      // in meters/second

// Bumper sensors
bool bumper_stop = false; // Set to true if bumpers indicate robot should stop

  digitalWrite(BLINK_PIN, blink_state);
}

void doBumperDebug() {
  // Read the bumper sensors
  snprintf(debug_str, sizeof(debug_str), 
