   ros_nh.advertise(ros_teensy_debug_pub);
   timer.every(convertFreqToMillis(publish_bumper_debug_info_freq_hz), doBumperDebug);
   timer.every(convertFreqToMillis(publish_encoder_debug_info_freq_hz), doEncoderDebug);
-  timer.every(convertFreqToMillis(1000), doTeensyDebug);
+  timer.every(convertFreqToMillis(publish_teensy_debug_info_freq_hz), doTeensyDebug);
 }
 
 void loop() {

  double wr = getAngularVelocityFromSamples(samples_wr);
  
  // Convert to wheel linear velocity
  double vl = wl * WHEEL_RADIUS;
  double vr = wr * WHEEL_RADIUS;
  
  // Calculate robot vx and vth
  double vx = (vl + vr)/2;
  double vth = (vr - vl)/(2 * TRACK_RADIUS);
  
  // Calculate the deltas since
  double dt = current_time.toSec() - last_raw_odom_publish_time.toSec();

  double wr = motor_controller_p * (set_point_wr - sensed_wr);

  // Calculate motor speed between -1 and 1
  double new_left_speed = max(-1, min(1, wl / max_speed));
  double new_right_speed = max(-1, min(1, wr / max_speed));

  setMotors(new_left_speed, new_right_speed);
}
