   current_time = ros_nh.now();
   last_time = current_time;
   last_cmd_vel_time = current_time;
+  //last_encoder_read_time = current_time;
+  //next_encoder_read_time = current_time + next_encoder_read_duration;
   encoder_left_pos = 0.0;
   encoder_right_pos = 0.0;
-  bumper_left_base = analogRead(BUMPER_LEFT_PIN);
-  bumper_right_base = analogRead(BUMPER_RIGHT_PIN);
+  motor_left_speed = 0.0;
+  motor_right_speed = 0.0;
 }
 
 void loop() {
-  ros_nh.spinOnce();
+  
+    
   current_time = ros_nh.now();
 
   // If we have not received any cmd_vel messages for a while,

  unsigned int new_bumper_right = analogRead(BUMPER_RIGHT_PIN);

  // Check bumper thresholds to stop robot
  
  // Read encoders
  unsigned long new_encoder_left_pos = encoder_left.read();
  unsigned long new_encoder_right_pos = encoder_right.read();

  // Read imu data

  // Broadcast odometry transform
  ros_odom_transform.header.stamp = current_time;

  // TODO: set all the transform data

  ros_odom_broadcaster.sendTransform(ros_odom_transform);
  
  // Publish Odometry
  ros_odom_msg.header.stamp = current_time;
  
  // TODO: Set all the odometry data
  
  ros_odom_pub.publish(&ros_odom_msg);
  
  last_time = current_time;
}

void cmdVelCallback(const geometry_msgs::Twist& twist_msg) {
  last_cmd_vel_time = ros_nh.now();
  
  // Do something interesting with the cmd_vel
  setMotors(100, 100);
}

/*
 * Sets the speed for the motors. Values are expected
 * to be between -255 and 255. Values outside this range
 * will be pinned.
 */
void setMotors(int motor_left_speed, int 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;
  }
  motor_left_speed = abs(motor_left_speed);
  if (motor_left_speed > 255) {
    motor_left_speed = 255;
  }

  byte command_byte_right = 0xCC;
  if (motor_right_speed < 0) {
    command_byte_right = command_byte_right | 0x01;
  } else {
    command_byte_right = command_byte_right | 0x02;
  }
  motor_right_speed = abs(motor_right_speed);
  if (motor_right_speed > 255) {
    motor_right_speed = 255;
  }
  
  trex.write(command_byte_left);
  trex.write((byte)motor_left_speed);
  trex.write(command_byte_right);
  trex.write((byte)motor_right_speed);
}


// 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 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 300  // Threshold that will indicate robot should stop
bool bumper_stop = false;
unsigned int bumper_left_base;
unsigned int bumper_right_base;

