     total += samples[count];
   }
   return total/10.0;
-}
-
-/*
- * Sets the speed for the motors. Values are expected
- * to be between -1 and 1. Values outside this range
- * will be pinned.
- */
-void setMotors(double motor_left_speed, double motor_right_speed) {
-  byte command_byte_left = 0xC4;
-  if (motor_left_speed < 0) {
-    command_byte_left = command_byte_left | 0x01;
-  } else {
-    command_byte_left = command_byte_left | 0x02;
-  }
-  int trex_left_speed = min(127, 127.0 * abs(motor_left_speed));
-
-  byte command_byte_right = 0xCC;
-  if (motor_right_speed < 0) {
-    command_byte_right = command_byte_right | 0x02;
-  } else {
-    command_byte_right = command_byte_right | 0x01;
-  }
-  int trex_right_speed = min(127, 127.0 * abs(motor_right_speed));
-  
-  trex.write(command_byte_left);
-  trex.write((byte)trex_left_speed);
-  trex.write(command_byte_right);
-  trex.write((byte)trex_right_speed);
 }
 
 // Callback for blink

  // Setup Timer callbacks
  timer.every((1.0/read_encoders_freq_hz) * 1000, doReadEncoders);
  timer.every((1.0/publish_odom_freq_hz) * 1000, doPublishOdom);
  timer.every(BLINK_DURATION, doBlink);
  
  current_time = ros_nh.now();

    publish_odom_freq_hz = 100;
  }
  
  int publish_imu_freq_hz;
  if (!ros_nh.getParam("publish_imu_freq_hz", &publish_imu_freq_hz)) { 
    publish_odom_freq_hz = 2;
  }
  
  int publish_magnetometer_freq_hz;
  if (!ros_nh.getParam("publish_magnetometer_freq_hz", &publish_magnetometer_freq_hz)) { 
    publish_odom_freq_hz = 2;
  }
  
  // Initialize broadcasters
  ros_odom_transform.header.frame_id = ros_odom_header_frame_id;
  ros_odom_transform.child_frame_id = ros_odom_child_frame_id;
