 }
 
 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);
+  if(xbeeStream.available() && xbeeStream.read() == 0x00)  state_paused = !state_paused;
+  if (!state_paused) {
+    now = millis();
+    read_lidars();
+    if (now - timestamp > 40) {
+      wheel_rpm = ENCODER_SCALING * encoder_count / (now - timestamp);
+      encoder_pin = 0;
+      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();
+      // 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)) * 
+      motorServo.write(90  - 20 );
+      steeringServo.write(90 + (Output_theta) - Output_dist); // + Output_theta - Output_dist
+    } // (50/abs(Input_dist - Setpoint_dist)) *
+  }
   delay(2);
 }
 


double calcPerpDist(double dist0, double dist1, double angle) {
  return ((dist0 + dist1) * cos(angle)) / 2;
}



      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++;
