 ros::Time last_encoder_read_time;
 
 #define NUM_VEL_SAMPLES 10
-double ang_vel_samples_l[NUM_VEL_SAMPLES];
-double ang_vel_samples_r[NUM_VEL_SAMPLES];
+double ang_v_samples_l[NUM_VEL_SAMPLES];
+double ang_v_samples_r[NUM_VEL_SAMPLES];
 int velocity_index = 0;
 
 // IMU and Magnetometer

    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

  unsigned long diff_r = new_encoder_right_pos - encoder_right_pos;
  double diff_t = current_time.toSec() - last_encoder_read_time.toSec();

  ang_v_samples_l[velocity_index] = (diff_l/TICKS_PER_RADIAN)/diff_t;
  ang_v_samples_r[velocity_index] = (diff_r/TICKS_PER_RADIAN)/diff_t;

  velocity_index = (velocity_index + 1) % NUM_VEL_SAMPLES;
  encoder_left_pos = new_encoder_left_pos;
