 
 ros::Publisher ros_bumper_debug_pub("bumper_debug", &ros_debug_msg);
 ros::Publisher ros_encoder_debug_pub("encoder_debug", &ros_debug_msg);
+victoria_debug_msgs::TeensyDebug teensy_debug_msg;
+ros::Publisher ros_teensy_debug_pub("teensy_debug", &teensy_debug_msg);
 
 // Debug blink
 // Blink duration is in milliseconds.

int blink_state = LOW;
  
void setup() {
  // Setup basic I2C master mode pins 18/19, external pullups, 400kHz, 200ms default timeout
  Wire.begin(I2C_MASTER, 0x00, I2C_PINS_18_19, I2C_PULLUP_EXT, 400000);
  Wire.setDefaultTimeout(200000); // 200ms


// TRex motor controller
HardwareSerial trex = HardwareSerial();
bool trex_err;

// Timeout, in seconds, for motor controller to receive a command,
// after which the controller will shut down the motors.
double motor_command_timeout;

// Last motor speed set on motors.
double motor_left_speed;
double motor_right_speed;

// Victoria constants
const double TICKS_PER_RADIAN(9072);
const double TRACK_RADIUS(0.235);  // in meters
const double WHEEL_RADIUS(0.127);  // in meters
const double MAX_SPEED(4.71);      // in radians/second

// Motor encoders
Encoder encoder_left(ENCODER_LEFT_PIN_1, ENCODER_LEFT_PIN_2);
Encoder encoder_right(ENCODER_RIGHT_PIN_1, ENCODER_RIGHT_PIN_2);
long encoder_left_pos;
long encoder_right_pos;
ros::Time last_encoder_read_time;

const int NUM_VEL_SAMPLES(10);
