 const double WHEEL_RADIUS(0.127);  // in meters
 const double MAX_SPEED(4.71);      // in meters/second
 
-// Bumper sensors
-bool bumper_stop = false; // Set to true if bumpers indicate robot should stop
-unsigned int bumper_left;
-unsigned int bumper_right;
-
 // Motor encoders
 Encoder encoder_left(ENCODER_LEFT_PIN_1, ENCODER_LEFT_PIN_2);
 Encoder encoder_right(ENCODER_RIGHT_PIN_1, ENCODER_RIGHT_PIN_2);

void doBumperDebug() {
  // Read the bumper sensors
  snprintf(debug_str, sizeof(debug_str), 
    "L: %d    R: %d    STOP: %d", bumper_left, bumper_right, bumper_stop);
  ros_debug_msg.data = debug_str;
  ros_bumper_debug_pub.publish(&ros_debug_msg);
}

void doEncoderDebug() {
  // Read the encoders
  snprintf(debug_str, sizeof(debug_str), "L: %ld    R: %ld",
    encoder_left.read(), 
    encoder_right.read());
  ros_debug_msg.data = debug_str;
  ros_encoder_debug_pub.publish(&ros_debug_msg);
}
