 const int steeringPin = 9, motorPin = 8;
 
 //define PID variables
-double Setpoint_theta = 0, Input_theta, Output_theta, Setpoint_dist = 90, Input_dist, Output_dist;
+double Setpoint_theta = 0, Input_theta, Output_theta, Setpoint_dist = 90, Input_dist, Output_dist, Setpoint_speed = 60, Input_speed, Output_speed;
 //specify initial PID links and tuning
 int KpT = 1.7, KiT = 0, KdT = 0.01;  // {1.7, 0, 0.01}
 int KpD = 1.1, KiD = 0.075, KdD = 0.97;   // {1, 0, 0}
+int KpS = 0.5, KiS = 0, KdS = 0;
 PID PID_theta(&Input_theta, &Output_theta, &Setpoint_theta, KpT, KiT, KdT, DIRECT);
 PID PID_dist(&Input_dist, &Output_dist, &Setpoint_dist, KpD, KiD, KdD, DIRECT);
+PID PID_speed(&Input_speed, &Output_speed, &Setpoint_speed, KpS, KiS, KdS, DIRECT);
 
 //Spacing between LIDARs on vehicle
 double LIDARspacing = 22.5;

}

void loop() {
  if(xbeeStream.available() && xbeeStream.read() == 0x00){
    state_paused = !state_paused;
    if(state_paused)  motorServo.write(90);
  }
  if (!state_paused) {
    now = millis();
    read_lidars();
    if (now - timestamp > 40) {
      wheel_rpm = ENCODER_SCALING * encoder_count / (now - timestamp);
      encoder_count = 0;
      timestamp = now;
      //    dist[0] = 4.5 + dist[0] * (0.98); //scaling equation

void setup() {
  PID_theta.SetMode(AUTOMATIC);
  PID_dist.SetMode(AUTOMATIC);
  PID_speed.SetMode(AUTOMATIC);

  // Servo range limits
  PID_theta.SetOutputLimits(-45, 45);
  PID_dist.SetOutputLimits(-45, 45);
  PID_speed.SetOutputLimits(-20, 20);

  steeringServo.attach(steeringPin);
  motorServo.attach(motorPin);
