   rw_cmd_spd = motor_com.right_motor_w;
 }
 
+// Subscriber to speed commands on computer
+ros::Subscriber<scr_proto::SpeedCommand> motor_sub("speed_command", MotorCallback);
+
 void updateEncoderReading() {
   // Get raw ticks
-  long L_encoderValue = L_DCMotorEncoder.read();
-  long R_encoderValue = R_DCMotorEncoder.read();
-  
+  L_EncoderValue = L_DCMotorEncoder.read();
+  R_EncoderValue = R_DCMotorEncoder.read();
+  
+  // Current time in micro seconds
   long now = micros();
   
   long dt = (now - last_encoder_time);
   
-  // Get position in revolutions (includes multiple revolutions)
-// Serial.println(encoderValue);
-  L_EncoderDelta = (double)(L_encoderValue - L_LastEncoderValue) * (1-beta) + L_LastEncoderDelta * beta;
-  R_EncoderDelta = (double)(R_encoderValue - R_LastEncoderValue) * (1-beta) + R_LastEncoderDelta * beta;
-// Serial.println(g_EncoderAngle);
-  
-  // Get velocity in ticks/sec
-  L_EncoderVelocity = 1000000.0*(double)(L_EncoderDelta)/dt;
-  R_EncoderVelocity = 1000000.0*(double)(R_EncoderDelta)/dt;
-
-  // Convert to revolutions/sec
-  L_EncoderVelocity /= (double)TICKS_PER_REVOLUTION;
-  R_EncoderVelocity /= (double)TICKS_PER_REVOLUTION;
-  
-  // Convert to rad/s
+  // Get position in revolutions (includes multiple revolutions) with low pass filter
+  L_EncoderDelta = (double)(L_EncoderValue - L_LastEncoderValue) * (1-beta) + L_LastEncoderDelta * beta;
+  R_EncoderDelta = (double)(R_EncoderValue - R_LastEncoderValue) * (1-beta) + R_LastEncoderDelta * beta;
+  
+  // Get velocity in revs/sec
+  L_EncoderVelocity = 1000000.0/dt * L_EncoderDelta/TICKS_PER_REVOLUTION;
+  R_EncoderVelocity = 1000000.0/dt * R_EncoderDelta/TICKS_PER_REVOLUTION;
+  
+  // Convert to rad/sec
   L_WheelVelocity = L_EncoderVelocity * 2 * PI;
   R_WheelVelocity = R_EncoderVelocity * 2 * PI;
 
-  // Set last known encoder ticks
+  // Update previous values
   L_LastEncoderValue = L_encoderValue;
   R_LastEncoderValue = R_encoderValue;
 

double lw_cmd_spd = 0;
double rw_cmd_spd = 0;

// Encoder initialization
Encoder L_DCMotorEncoder(LEFT_ENCODER_A_PIN, LEFT_ENCODER_B_PIN);
Encoder R_DCMotorEncoder(RIGHT_ENCODER_A_PIN, RIGHT_ENCODER_B_PIN);

long last_encoder_time = micros();  // not the best way to get this done... should check existence in Update Wheel Function
float L_EncoderVelocity = 0; // Radians per second
long L_LastEncoderValue = 0;
double L_EncoderDelta = 0; 
double L_LastEncoderDelta = 0; 
float R_EncoderVelocity = 0; // Radians per second
long R_LastEncoderValue = 0;
double R_EncoderDelta = 0; 
double R_LastEncoderDelta = 0; 
double beta = 0.0;  // Low Pass Parameter for encoder velocity

// Speed Controller Initialization
double L_PIDout = 0;

/*
 * Squad Collaborative Robotics
 * Prototype Embedded Collector Bot Code
 * Arduino Mega 2560 R3
 */

// ROS Includes
