     
   // Start serial port to TRex
   trex.begin(19200);
+  
+  // Setup IMU and magenetometer that use I2C
+  if (imu.init()) {
+    imu.enableDefault();
+    // TODO: set timeout?
+  } else {
+    // TODO: How to handle error?
+  }
+  
+  if (mag.init()) {
+    mag.enableDefault();
+    // TODO: set timeout?
+  } else {
+    // TODO: How to handle error?
+  }
 
   // Initialize ros node
   ros_nh.initNode();

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

// Motor encoders
Encoder encoder_left(ENCODER_LEFT_PIN_1, ENCODER_LEFT_PIN_2);
Encoder encoder_right(ENCODER_RIGHT_PIN_1, ENCODER_RIGHT_PIN_2);
unsigned long encoder_left_pos;
unsigned long encoder_right_pos;

// Bumper sensors
#define BUMPER_THRESHOLD 500  // Threshold that will indicate robot should stop
unsigned int bumper_left_base;
unsigned int bumper_right_base;

// ROS node handle
ros::NodeHandle ros_nh;

// ROS cmd_vel subscriber
void cmdVelCallback(const geometry_msgs::Twist& twist_msg);
ros::Subscriber<geometry_msgs::Twist> ros_cmd_vel_sub("cmd_vel", cmdVelCallback);


 * 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.
 * 
 * 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>
