     
   // Start the TRex
   trex_err = startTRex();
-
-  // Setup pin configurations
-  pinMode(BLINK_PIN, OUTPUT);
 
   // Setup Timer callbacks
   setupTimerCallback(read_encoders_freq_hz, doReadEncoders);

  setupTimerCallback(publish_raw_imu_freq_hz, doPublishRawImu);
  setupTimerCallback(publish_bumper_info_freq_hz, doPublishBumpers);
  setupTimerCallback(publish_encoder_info_freq_hz, doPublishEncoders);
  setupTimerCallback(publish_teensy_debug_info_freq_hz, doTeensyDebug);
  setupTimerCallback(BLINK_FREQ_HZ, doBlink);


void setupTimerCallback(int callback_freq_hz, void (*callback)(void));

void setup() {
  
  // Setup pin configurations
  pinMode(FOB_STOP_1_PIN, INPUT);
  pinMode(FOB_STOP_2_PIN, INPUT);
  pinMode(FOB_STOP_3_PIN, INPUT);
  pinMode(FOB_STOP_4_PIN, INPUT);
  pinMode(E_STOP_PIN, INPUT);
  pinMode(BLINK_PIN, OUTPUT);
  
  // Set to high to indicate teensy is alive
  digitalWrite(BLINK_PIN, HIGH);
  
  // Register ros publishers
  ros_nh.advertise(ros_raw_odom_pub);
  ros_nh.advertise(ros_raw_imu_pub);
