 }
 
 void loop() {
-  if(xbeeStream.available() && xbeeStream.read() == 0x00){
+  if (xbeeStream.available() && xbeeStream.read() == 0x00) {
     state_paused = !state_paused;
-    if(state_paused)  motorServo.write(90);
+    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

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

  // Servo range limits
  PID_theta.SetOutputLimits(-50, 50);
  PID_dist.SetOutputLimits(-50, 50);

  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();
      PID_speed.Compute();

      motorServo.write(90  - Output_speed );
      if (abs(Output_dist - Setpoint_dist) < 10 && abs(Output_theta - Setpoint_theta) < 5) {
        steeringServo.write(90 - Output_theta);
      } else {
        steeringServo.write(90 + (Output_theta) - Output_dist); // + Output_theta - Output_dist
      } // (50/abs(Input_dist - Setpoint_dist)) *
    }
  }
  delay(2);
}
