   ros_odom_msg.twist.twist.angular.z = vth;
   
   ros_odom_pub.publish(&ros_odom_msg);
+  
+  // Remember current time for next publish
+  last_odom_publish_time = current_time;
+}
+
+/*
+ * Publish imu data to ros.
+ */
+void doPublishImu() {
+  // TODO(mwomack)
+}
+
+/*
+ * Publish magnetometer data to ros.
+ */
+void doPublishMagnetometer() {
+  // TODO(mwomack)
+}
+
+/*
+ * 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);
 }
 
 /*
  * Returns the calculated angular velocity from all of
- * the samples. Right now it just returns the average
+ * the samples. currently it just returns the average
  * of all the samples.
  */
 double getAngularVelocityFromSamples(double* samples) {

  // 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();

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
