 // ROS includes
 #include <ros.h>
 #include <geometry_msgs/Twist.h>
+#include <geometry_msgs/Pose.h>
 #include <nav_msgs/Odometry.h>
 #include <tf/transform_broadcaster.h>
+#include <std_msgs/String.h>
 
 // Teensy pin definitions
 #define ENCODER_LEFT_PIN_1 29   // Teensy digital pin 29
 #define ENCODER_LEFT_PIN_2 30   // Teensy digital pin 30
 #define ENCODER_RIGHT_PIN_1 31  // Teensy digital pin 31
 #define ENCODER_RIGHT_PIN_2 32  // Teensy digital pin 32
-#define BUMPER_LEFT_PIN 14      // Teensy analog pin 14
-#define BUMPER_RIGHT_PIN 15     // Teensy analog pin 15
+#define BUMPER_LEFT_PIN A15      // Teensy analog pin 15
+#define BUMPER_RIGHT_PIN A14     // Teensy analog pin 14
 
 // TRex motor controller
 HardwareSerial trex = HardwareSerial();
 
+#define BLINK_DURATION 2000
+#define BLINK_PIN 13 // Teensy digital pin 13 for LED
+int blink_state;
+unsigned long next_blink_time;
+
+char debug_str[80];
+
+// Victoria constants
+#define TRACK_RADIUS 0.235  // in meters
+#define WHEEL_RADIUS 0.127  // in meters
+#define MAX_SPEED 4.71      // in meters/second
+
 // Bumper sensors
-#define BUMPER_THRESHOLD 500  // Threshold that will indicate robot should stop
+#define BUMPER_THRESHOLD 300  // Threshold that will indicate robot should stop
+bool bumper_stop = false;
 unsigned int bumper_left_base;
 unsigned int bumper_right_base;
 

Encoder encoder_right(ENCODER_RIGHT_PIN_1, ENCODER_RIGHT_PIN_2);
unsigned long encoder_left_pos;
unsigned long encoder_right_pos;

// IMU and Magnetometer
LSM6 imu;
LIS3MDL mag;

// ROS node handle
ros::NodeHandle ros_nh;

// ROS cmd_vel subscriber
// Threshold, in nanoseconds, to wait for a cmd_vel message else take action


ros::Time current_time;
ros::Time last_time;

char ros_odom_header_frame_id[] = "/odom";
char ros_odom_child_frame_id[] = "/base_link";

//Pose pose;
  
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

  // Setup IMU and magenetometer that use I2C
  if (imu.init()) {
    imu.enableDefault();
    // TODO: set timeout?
  } else {
    imu_err = true;
  }
  
  if (mag.init()) {
    mag.enableDefault();
    // TODO: set timeout?
  } else {
    mag_err = true;
  }
    
  // Start serial port to TRex
  trex.begin(19200);

  bumper_left_base = 5000; //analogRead(BUMPER_LEFT_PIN);
  bumper_right_base = 5000; //analogRead(BUMPER_RIGHT_PIN);

  next_blink_time = millis();
  blink_state = LOW;
  pinMode(BLINK_PIN, OUTPUT);

  // Initialize ros node
  ros_nh.initNode();
