   // Set last known encoder ticks
   L_LastEncoderValue = L_encoderValue;
   R_LastEncoderValue = R_encoderValue;
+
+  L_LastEncoderDelta = L_EncoderDelta;
+  R_LastEncoderDelta = R_EncoderDelta;
 }
 
 ros::Subscriber<scr_proto::SpeedCommand> motor_sub("speed_command", MotorCallback);

PID L_DCMotorPID(&L_WheelVelocity,
                 &L_PIDout,
                 &lw_cmd_spd,
                 1.5,
                 0.0,
                 0.1,
                 REVERSE);

PID R_DCMotorPID(&R_WheelVelocity,
                 &R_PIDout,
                 &rw_cmd_spd,
                 1.5,
                 0.0,
                 0.1,
                 REVERSE);

// Motor Callbacks

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;

