readServos

#define Servo1In 2
#define Servo2In 3

unsigned long lastServo1 = 0;
unsigned long Servo1Time = 0;
unsigned long lastServo2 = 0;
unsigned long Servo2Time = 0;
unsigned long longestTime = 0;
unsigned long shortestTime = 0-1;

int count = 0;

void setup() {
  pinMode(2, INPUT_PULLUP);
  pinMode(3, INPUT_PULLUP);
  pinMode(4, OUTPUT);
  digitalWrite(4, LOW);
  attachInterrupt(digitalPinToInterrupt(Servo1In), readServo1, CHANGE);
  attachInterrupt(digitalPinToInterrupt(Servo2In), readServo2, CHANGE);
  Serial.begin(115200);
}

void readServo1() {
  if (((micros() - lastServo1) > 1000) && ((micros() - lastServo1) < 2000)) {
    Servo1Time = micros() - lastServo1;
  }
  lastServo1 = micros();
}

void readServo2() {
  if (((micros() - lastServo2) > 1000) && ((micros() - lastServo2) < 2000)) {
    Servo2Time = micros() - lastServo2;
  }
  lastServo2 = micros();
}

void loop() {
 if (Servo1Time > longestTime) {
  longestTime = Servo1Time; 
 }
 if (Servo1Time < shortestTime) {
  shortestTime = Servo1Time;
 }
 if (count == 10000) {
  Serial.println(map(Servo1Time, 1100, 1870, 0, 255));
  Serial.println(map(Servo2Time, 1100, 1870, 0, 255));
  count = 0;
 }
 count++;
}