       if (Wire.requestFrom(LIDARLITE_ADDR, 2) >= 2) {
         uint16_t val = Wire.read() << 8 | Wire.read();
         if (val < 500 && val > 10) dist[enabled_lidar] = 0.2 * dist[enabled_lidar] + 0.8 * val;
-       
+
         enabled_lidar = 1 - enabled_lidar;
         lidar_state = 0;
       } else state_cycle_count++;

}

void loop() {
  now = millis();
  read_lidars();
  if (now - timestamp > 40) {
    timestamp = now;
//    dist[0] = 4.5 + dist[0] * (0.98); //scaling equation
    Input_theta = calcAngle(dist[0], dist[1]);
    Input_dist = calcPerpDist(dist[0], dist[1], Input_theta);
    Input_theta = Input_theta * 180 / pi;
//    Setpoint_theta = constrain(0.5*(Input_dist - Setpoint_dist), -45, 45);

    // Run the PID loops
    Serial.print(dist[0]);
    Serial.print(", ");
    Serial.print(dist[1]);
    Serial.print(", ");
    Serial.print(Input_theta);
    Serial.print(", ");
    Serial.println(Input_dist);
    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);
}


// Set pi
const float pi = 3.142;

// Wheel encoder pulse counter variable
const int encoder_pin = 2;
volatile int encoder_count = 0;
double wheel_rpm;
#define ENCODER_SCALING 1

XbeeApiStream xbeeStream = XbeeApiStream();
bool state_paused = true;


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

