Servo & speed reading

Projects General Guidance
1

I'm trying to build a combustion engine constant speed controller. My hardware consists of pulse counter that produces 5 V square wave thats frequency is proportional to engine RPM and a servo that rotates the throttle of the engine.

My problem is to produce a proper PWM to servo when I have a pulse counter and some other code (I also have to read some buttons). I mean, other parts of my code involving delays takes time and makes low state of the PWM signal too long which makes the servo unstable.

When I test parts of my program individually, no problems. I can print the RPM to serial and turn a servo.

My code:

const int speedpulsePin = 5; //RPM sensor
const int servoPin = 6; //servo PWM

const int sampletime = 1000;
int pwmhighduration; //Duration of PWM high state
int pulsecount;
int error;
const int setpoint = 200; //Pulse count for one second at wanted RPM

void setup() {
  pinMode(servoPin, OUTPUT);
  Serial.begin(9600);

  //Pulse counter
  TCCR1A = 0;
  TCNT1 = 0;
  
}

void loop() {
  
    bitSet(TCCR1B, CS12);
    bitSet(TCCR1B, CS11);
    delay(sampletime);
    TCCR1B = 0;
    pulsecount = TCNT1;
    TCNT1 = 0;
    error = setpoint - pulsecount;
    pwmhighduration = map(error, 0, 100, 1000, 2000);
    digitalWrite(servoPin, HIGH);
    delayMicroseconds(pwmhighduration);
    digitalWrite(servoPin, LOW);
    delayMicroseconds(20000 - pwmhighduration); //Servo pulse freq = 50 Hz, so low state 20000-high duration
  }
}

So,

digitalWrite(servoPin, LOW);
delayMicroseconds(20000 - pwmhighduration);

sets servo pin at low, but if I want it works like expected, the next line should be

digitalWrite(servoPin, HIGH);

so that the delay would be right.
Well, my pulse counter prolongs the delay.

Hope you understood, it's quite a hard to explain since my native language isn't English!

2

Why not use the Servo library?

...R