 PID L_DCMotorPID(&L_WheelVelocity,
                  &L_PIDout,
                  &lw_cmd_spd,
-                 0.65,
+                 0.15,
                  0.00,
-                 0.10,
+                 0.010,
                  REVERSE);
 
 PID R_DCMotorPID(&R_WheelVelocity,
                  &R_PIDout,
                  &rw_cmd_spd,
-                 0.6,
+                 0.15,
                  0.00,
-                 0.10,
+                 0.010,
                  REVERSE);
+
+int mode = 0;
 
 // Motor Callbacks
 void MotorCallback(const scr_proto::SpeedCommand& motor_com){


void loop()
{
  // Get Freshest Values
  updateEncoderReading();


  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;
