   rw_cmd_spd = motor_com.right_motor_w;
 }
 
+// Command Callback
+void CommandCallback(const std_msgs::Int32& mode_msg){
+
+  if(mode_msg.data == 1){
+    mode = mode_msg.data;
+    nh.loginfo("Arduino Normal Mode Active");
+  }
+  else if(mode_msg.data == 0){
+    mode = mode_msg.data;
+    nh.loginfo("Arduino Safe Mode Active");
+  }
+  else{
+    nh.loginfo("Invalid Mode Command. Modes are 0 for Safe mode and 1 for normal mode");
+  }
+
+}
+
 void updateEncoderReading() {
   // Get raw ticks
   long L_encoderValue = L_DCMotorEncoder.read();
   long R_encoderValue = R_DCMotorEncoder.read();
   
-  // Get position in revolutions (includes multiple revolutions)
-// Serial.println(encoderValue);
+  // Current time in micro seconds
+  long now = micros();
+  
+  long dt = (now - last_encoder_time);
+  
+  // Get position in ticks 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;
-// Serial.println(g_EncoderAngle);
-  
-  //double dt = nh.now().toSec() - last_encoder_time.toSec();
-  double dt = micros() - last_encoder_time;
-
-  // Get velocity in ticks/sec
-  L_EncoderVelocity = 1000000*(double)(L_EncoderDelta)/(double)dt;
-  R_EncoderVelocity = 1000000*(double)(R_EncoderDelta)/(double)dt;
-
-  // Convert to revolutions/sec
-  L_EncoderVelocity /= (double)TICKS_PER_REVOLUTION;
-  R_EncoderVelocity /= (double)TICKS_PER_REVOLUTION;
-  
-  // Convert to rad/s
+  
+  // 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;
 
+  last_encoder_time = now;
+
   L_LastEncoderDelta = L_EncoderDelta;
   R_LastEncoderDelta = R_EncoderDelta;
 
-  last_encoder_time = micros();
 }
 
 ros::Subscriber<scr_proto::SpeedCommand> motor_sub("speed_command", MotorCallback);
-
+ros::Subscriber<std_msgs::Int32> command_sub("arduino_mode", CommandCallback);
 
 // Rx, Tx, Reset Pins
 PololuQik2s12v10 pmd(PMD_RX, PMD_TX, PMD_RESET);

PID L_DCMotorPID(&L_WheelVelocity,
                 &L_PIDout,
                 &lw_cmd_spd,
                 0.65,
                 0.00,
                 0.10,
                 REVERSE);

PID R_DCMotorPID(&R_WheelVelocity,
                 &R_PIDout,
                 &rw_cmd_spd,
                 0.6,
                 0.00,
                 0.10,
                 REVERSE);

// Motor Callbacks
void MotorCallback(const scr_proto::SpeedCommand& motor_com){

  lm_cmd += L_PIDout;
  rm_cmd += R_PIDout;

 if(mode == 0){
    lm_cmd = 0;
    rm_cmd = 0;
  }
  // Populate messages
  l_wheel_msg.data = L_WheelVelocity;
  r_wheel_msg.data = R_WheelVelocity;
