 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){

  rw_cmd_spd = motor_com.right_motor_w;
}

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);
  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
  L_WheelVelocity = L_EncoderVelocity * 2 * PI;
  R_WheelVelocity = R_EncoderVelocity * 2 * PI;

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

  L_LastEncoderDelta = L_EncoderDelta;
  R_LastEncoderDelta = R_EncoderDelta;

  last_encoder_time = micros();
}

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


// Rx, Tx, Reset Pins
PololuQik2s12v10 pmd(PMD_RX, PMD_TX, PMD_RESET);

#define PMD_RESET 8

// Ros Initialization stuff
ros::NodeHandle_<ArduinoHardware, 10, 10, 256, 256>  nh;

// Wheel Speed Messages
std_msgs::Float32 l_wheel_msg;
