   // publish measurements
   if (dt > 50) {
     // publish encodeer measurement
-    enc_msg.x = FL_count;
-    enc_msg.y = FR_count;
-    enc_msg.z = 0;
-    pub_encoder.publish(&enc_msg);
+    
+    encoder.FL = FL_count;
+    encoder.FR = FR_count;
+    encoder.BL = 0;
+    encoder.BR = 0;
+    pub_encoder.publish(&encoder);
     
     // publish ultra-sound measurement
-    ultrasound.data[0] = us_fr.getRange();
-    ultrasound.data[1] = us_bk.getRange();
-    ultrasound.data[2] = us_lt.getRange();
-    ultrasound.data[3] = us_rt.getRange();
+    ultrasound.front = us_fr.getRange();
+    ultrasound.back = us_bk.getRange();
+    ultrasound.left = us_lt.getRange();
+    ultrasound.right = us_rt.getRange();
     pub_ultrasound.publish(&ultrasound);
     t0 = millis();
-  }
-  
-  new_val = digitalRead(pin_BR);   // read the input pin
-  if (val != new_val){
-    B_count++;
-    val = new_val;
   }
   
   nh.spinOnce();

volatile unsigned long dt;
volatile unsigned long t0;

// pin to poll rear wheel encoder 
int pin_BR = 5;   // back right
boolean val, new_val;
int B_count = 0;


ros::NodeHandle nh;

// define global message variables
// Encoder, Electronic Control Unit, Ultrasound
geometry_msgs::Vector3 enc_msg;
geometry_msgs::Vector3 esc_cmd_msg;
std_msgs::Float32MultiArray ultrasound;

ros::Publisher pub_encoder("encoder", &enc_msg);
ros::Publisher pub_ultrasound("ultrasound", &ultrasound);


/**************************************************************************
ESC COMMAND {MOTOR, SERVO} CALLBACK
**************************************************************************/
void messageCb(const geometry_msgs::Vector3& esc_cmd_msg){
  // deconstruct esc message
  motorCMD = saturateMotor( int(esc_cmd_msg.x) );
  servoCMD = saturateServo( int(esc_cmd_msg.y) );
  
  // apply commands to motor and servo
  motor.write( motorCMD );   
  steering.write(  servoCMD );
}
// ECU := Engine Control Unit
ros::Subscriber<geometry_msgs::Vector3> s("ecu", messageCb);

// Set up ultrasound sensors
Maxbotix us_fr(14, Maxbotix::PW, Maxbotix::LV); // front

 
// include libraries
#include <ros.h>
#include <barc/Ultrasound.h>
#include <barc/Encoder.h>
#include <barc/ECU.h>
#include <Servo.h>
#include "Maxbotix.h"

