   
   // Get position in revolutions (includes multiple revolutions)
 // Serial.println(encoderValue);
-  L_EncoderAngle = (double)L_encoderValue / (double)TICKS_PER_REVOLUTION;
-  R_EncoderAngle = (double)R_encoderValue / (double)TICKS_PER_REVOLUTION;
+  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 = 1000.0*(float)(L_encoderValue-L_LastEncoderValue)/(float)ENCODER_TIME_DELAY_MS;
-  R_EncoderVelocity = 1000.0*(float)(R_encoderValue-R_LastEncoderValue)/(float)ENCODER_TIME_DELAY_MS;
+  L_EncoderVelocity = 1000.0*(double)(L_EncoderDelta)/(double)ENCODER_TIME_DELAY_MS;
+  R_EncoderVelocity = 1000.0*(double)(R_EncoderDelta)/(double)ENCODER_TIME_DELAY_MS;
 
   // Convert to revolutions/sec
   L_EncoderVelocity /= (double)TICKS_PER_REVOLUTION;

  // Set last known encoder ticks
  L_LastEncoderValue = L_encoderValue;
  R_LastEncoderValue = R_encoderValue;
}

ros::Subscriber<scr_proto::SpeedCommand> motor_sub("speed_command", MotorCallback);

scr_proto::SpeedCommand motor_msg;

// Initialize Publishers
//ros::Publisher left_encoder_pub("left_encoder", &l_enc_msg);
ros::Publisher left_wheel_pub("lw_speed", &l_wheel_msg);
//ros::Publisher right_encoder_pub("right_encoder", &l_enc_msg);
ros::Publisher right_wheel_pub("rw_speed", &r_wheel_msg);

// Placeholders for wheel speeds in rad/s
