Arduino servo controller

pilot.ino

#include <Wire.h>
#include <Servo.h> 

#define MAX_POS 2000
#define MIN_POS 1000

Servo servos[8];

volatile uint16_t servo_inputs[]  = {1500,1500,1500,1500,1500,1500,1500,1500};

void setup() {
  Serial.begin(9600);
  Serial.println("hello");
  
  servos[0].attach(6);
  servos[1].attach(7);
  servos[2].attach(8);
  servos[3].attach(9);
  
  // enable pin change interrupt
  PCICR = _BV(PCIE2);
  PCMSK2 = 0b00111100;
  
  // I2C slave
  Wire.begin(23);
  Wire.onReceive(receiveEvent);
  Wire.onRequest(requestEvent);
}

void receiveEvent(int howMany) {
  if(Wire.available()) {
    int idx = Wire.read();
    while(1 < Wire.available()) {
      uint16_t value = Wire.read() | (Wire.read() << 8);
      servos[idx].writeMicroseconds(value);
      idx++;
    }
  }
}

void requestEvent() {
  Wire.write((char*)servo_inputs, 16);
}

void loop() {
  for(int i = 0; i < 8; i++) {
    Serial.println(servo_inputs[i]);
  }
  for(int i = 0; i < 8; i++) {
    Serial.println(servos[i].read());
  }
  delay(1000);
}

ISR(PCINT2_vect) {
  static uint8_t pin_state = 0;
  static unsigned long servo_timers[]  = {0,0,0,0,0,0,0,0};
  
  uint8_t old_pins = pin_state;
  pin_state = PIND;
  uint8_t i = 1;
  for(int j = 0; j < 8 ; j++) {
    if((old_pins & i) && ~(pin_state & i)) { // falling edge
      unsigned long val = micros() - servo_timers[j];
      if(val < MAX_POS && val > MIN_POS) {
        servo_inputs[j] = val;
      }
    } else if(~(old_pins & i) && (pin_state & i)) { // rising edge
      servo_timers[j] = micros();
    }
    i = i << 1;
  }
}