 // 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() {
-  // Initialize linked variables
-  Input_theta = 0;
-  Input_dist = 100;
-  Setpoint_theta = 0;
-  Setpoint_dist = 90;
-
   PID_theta.SetMode(AUTOMATIC);
   PID_dist.SetMode(AUTOMATIC);
 

double dist[2];

//set up servos
Servo steeringServo;
Servo motorServo;
const int steeringPin = 9;
const int motorPin = 8;

//define PID variables
double Setpoint_theta, Input_theta, Output_theta, Setpoint_dist, Input_dist, Output_dist;
//specify initial PID links and tuning
int KpT = 1.7;    //1.7
int KiT = 0;      //0
int KdT = 0.01;   //0.01
int KpD = 1.1;      //1
int KiD = 0.075;    //0
int KdD = 0.97;   //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);


  motorServo.attach(motorPin);

  // Begin serial
  Serial.begin(9600);
  xbeeStream.begin(Serial);
  xbeeStream.write("Serial begin");
  // Open and join irc bus as master
  Wire.begin();
  delay(100);

  // Set encoder pin as input and enable internal pull-up resistor
  pinMode(encoder_pin, INPUT);
  digitalWrite(encoder_pin, HIGH);
  attachInterrupt(digitalPinToInterrupt(encoder_pin), encoder_ISR, CHANGE);

  // Set up the lidar pins and change the lidar acquisition count register value
  for (int i = 0; i < 2; i++)   pinMode(lidar_pwr_en[i], OUTPUT);
  for (int i = 0; i < 2; i++) {
    for (int j = 0; j < 2; j++)  digitalWrite(lidar_pwr_en[j], LOW);
