   pinMode(BLINK_PIN, OUTPUT);
 
   // Setup Timer callbacks
-  timer.every(convertFreqToMillis(read_encoders_freq_hz), doReadEncoders);
-  timer.every(convertFreqToMillis(motor_controller_freq_hz), doMotorController);
-  timer.every(convertFreqToMillis(publish_raw_odom_freq_hz), doPublishRawOdom);
-  timer.every(convertFreqToMillis(publish_raw_imu_freq_hz), doPublishRawImu);
-  timer.every(convertFreqToMillis(publish_bumper_debug_info_freq_hz), doBumperDebug);
-  timer.every(convertFreqToMillis(publish_encoder_debug_info_freq_hz), doEncoderDebug);
-  timer.every(convertFreqToMillis(publish_teensy_debug_info_freq_hz), doTeensyDebug);
-  timer.every(BLINK_DURATION, doBlink);
+  setupTimerCallback(read_encoders_freq_hz, doReadEncoders);
+  setupTimerCallback(motor_controller_freq_hz, doMotorController);
+  setupTimerCallback(publish_raw_odom_freq_hz, doPublishRawOdom);
+  setupTimerCallback(publish_raw_imu_freq_hz, doPublishRawImu);
+  setupTimerCallback(publish_bumper_info_freq_hz, doPublishBumpers);
+  setupTimerCallback(publish_teensy_debug_info_freq_hz, doTeensyDebug);
+  setupTimerCallback(BLINK_FREQ_HZ, doBlink);
 
   ros::Time current_time = ros_nh.now();
-  ros_raw_odom_msg.header.frame_id = ros_odom_header_frame_id;
-  ros_raw_odom_msg.child_frame_id = ros_odom_child_frame_id;
+  ros_raw_odom_msg.header.frame_id = "/odom";
+  ros_raw_odom_msg.child_frame_id = "/base_link";
+  ros_bumper_left_msg.header.frame_id = "/bumper_left";
+  ros_bumper_right_msg.header.frame_id = "/bumper_right";
   last_cmd_vel_time = current_time;
   last_encoder_read_time = current_time;
   encoder_left_pos = 0;

  ros_raw_imu_msg.magnetometer = convertMagnetometer(mag);

  ros_raw_imu_pub.publish(&ros_raw_imu_msg);
}

/*

    blink_state = LOW;
  }
  digitalWrite(BLINK_PIN, blink_state);
}

void doTeensyDebug() {
