   
   // 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;

  rm_cmd += R_PIDout;

  // Populate messages
  l_enc_msg.data = lm_cmd;
  r_enc_msg.data = rm_cmd;
  l_wheel_msg.data = L_WheelVelocity;
  r_wheel_msg.data = R_WheelVelocity;

  // Publish Data
  left_encoder_pub.publish( &l_enc_msg );
  right_encoder_pub.publish( &r_enc_msg );
  left_wheel_pub.publish( &l_wheel_msg );
  right_wheel_pub.publish( &r_wheel_msg );
  

Timer g_TimerEncoder;
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 = .95;  // Low Pass Parameter for encoder velocity

// Speed Controller Initialization
double L_PIDout = 0;
double R_PIDout = 0;

