  * Victoria Teensy ROS Node
  * 
  * This code is the ROS node that executes on the Teensy processor
- * for the Victoria RoboMagellan team. It uses the rosserial
- * library to communicate with ROS. It uses Arduino and Teensy
- * libraries to interact with systems and circuits connected to the
- * Teensy processor. This code assumes that a Teensy 3.5 is used.
+ * for the Victoria RoboMagellan team.
+ * 
+ * It uses the rosserial library to communicate with ROS. It uses
+ * Arduino and Teensy libraries to interact with systems and
+ * circuits connected to the Teensy processor. This code assumes
+ * that a Teensy 3.5 is used.
+ * 
+ * https://github.com/victoriarobotics
+ * 
  */
 
 // Teensy includes
 #define ENCODER_OPTIMIZE_INTERRUPTS
 #include <Encoder.h>  // 
 #include <i2c_t3.h>
+#include <LSM6.h>
+#include <LIS3MDL.h>
 
 // ROS includes
 #include <ros.h>

    
  // Start serial port to TRex
  trex.begin(19200);

  // Initialize ros node
  ros_nh.initNode();


  // If we have not received any cmd_vel messages for a while,
  // something is wrong, stop the robot.
  if (current_time.toNsec() - last_cmd_vel_time.toNsec() 
        > CMD_VEL_TIMEOUT_THRESHOLD_NS) {
    // TODO: Do something to stop the robot!
  }
  
  // Read bumper sensors
  unsigned int new_bumper_left = analogRead(BUMPER_LEFT_PIN);
