 void setup() {
   PID_theta.SetMode(AUTOMATIC);
   PID_dist.SetMode(AUTOMATIC);
+  PID_speed.SetMode(AUTOMATIC);
 
   // Servo range limits
-  PID_theta.SetOutputLimits(-50, 50);
-  PID_dist.SetOutputLimits(-50, 50);
+  PID_theta.SetOutputLimits(-45, 45);
+  PID_dist.SetOutputLimits(-45, 45);
+  PID_speed.SetOutputLimits(-20, 20);
 
   steeringServo.attach(steeringPin);
   motorServo.attach(motorPin);

      //    Setpoint_theta = constrain(0.5*(Input_dist - Setpoint_dist), -45, 45);

      // Run the PID loops
//      xbeeStream.write(String(String(dist[0], 1) + ',' + String(dist[1], 1) + ',' + String(Input_theta, 1) + ',' + String(Input_dist, 1)));
      PID_theta.Compute();
      PID_dist.Compute();

      motorServo.write(90  - 20 );
      steeringServo.write(90 + (Output_theta) - Output_dist); // + Output_theta - Output_dist
    } // (50/abs(Input_dist - Setpoint_dist)) *
  }
  delay(2);
}

}

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);
      Input_speed = wheel_rpm;
      encoder_count = 0;
      timestamp = now;
      //    dist[0] = 4.5 + dist[0] * (0.98); //scaling equation
