What is better approach for non blocking pid algorithm?

Question

I'm developing software side for 3 axes robot where are 360° continuous rotating servos used Documentation here

these servos have its own feedback system so it's possible to control it's position via pid system, when im using only one PID algorithm in clear sketch just for this pid everything working OK but when I want to use multiple PIDs there are problem with delays and every next function that is used in the sketch will result in bigger delays that make these servos flicking because it's executed in main function

int pinFeedback = pin_comes_here;

float tHigh = 0;
float tLow = 0;
int tCycle = 0;
float dc = 0;
float angle = 0; 
float dcMin = 2.9;
float dcMax = 97.1;

void setup() {
  pinMode(pinFeedback, INPUT);
}

void loop() {
    while(1) //From Parallax spec sheet
    {                                       //There is core of the problem
      tHigh = pulseIn(pinFeedback, HIGH);
      tLow = pulseIn(pinFeedback, LOW);
      tCycle = tHigh + tLow;
      if ( tCycle > 1000 && tCycle < 1200)
        {
          break; //valid tCycle;
        }
    }

  dc = (100 * tHigh) / tCycle; //From Parallax spec sheet, determine the percentage of the HIGH in the pulse

  angle = ((dc - dcMin) * 360) / (dcMax - dcMin + 1); //From Parallax spec sheet

  if (angle < 0)
  {
    angle = 0;
  }
  else if(angle > 359)
  {
    angle = 359;
  }

    //This is just angle calculation part, we have angle as output value from this algorithm so its possible to calculate error how much to left or right value from out target angle value and adjust servo position via standard servo library 

}

Reason why i need non blocking angle calculating is that because with everything this, i need to be able to control angles from firmata protocol and not just firmata but there can't be anything else done in sketch and i mentioned everything else causing bigger delays between reading signal, calculating angle, adjusting position, and every next function that could be placed in main loop.

So i came to the point that i need to use timer and interrupts like servo library uses by creating something like basic servo library wraper where i create PID class that inherit Servo class members for sake of accesing servo members from one object (attach, write) and two different approaches came to my mind

1. This PID class could instead of waiting in while loop for pulseIn sacrifices another one Timer that could divide feedback signal period which is long 1,1millisecond (see documentation, heading: feedback signal) by checking if it's up or down every several microseconds and counting total signal uptime for period and then calculate angle and error and adjust position with inherited write() member for every object from one timer

2. Using attachInterrupt that could behave similar like pulseIn but outside of main loop for every object(with same approach with PID class with inherted servo class)

Answer 1

The main issue you have is that pulseIn() is completely blocking - and your while loop doesn't help much either.

The "proper" (or what I would consider the "proper") way of dealing with this is to use a timer in gated mode. Actually you need two timers, but the Arduino already provides you micros() that will do for one of them.

Basically your pulsed signal is input into the gate input of a timer. That timer, when the gate input is HIGH, will count the number of ticks from whatever clock source you configure it for (system clock, etc, with or without prescaler, etc). It also needs to be configured so that the timer triggers an interrupt at the falling edge of the gate.

That interrupt reads the count from the timer and resets the count to 0 ready for the next pulse. It also uses micros() to work out the time between successive falling edges (should be about 1100us). A little bit of maths is needed to calculate the time from the count.

It's not something I have ever done with an Arduino - I have only used a PIC32 for it - though I assume that AVR timers have a gated mode (they should have; they are fairly comprehensive timers). Check the datasheet to be sure - as well as for how to configure it. On the PIC32 I used two timers both running at the same speed from the same clock source - one in gated mode to count the HIGH time and one in free-running mode to count the full period time. The duty % is then the ratio of the two counts and you don't care about the actual time in human-created units.

Of course, it goes without saying that you need a gated timer for each servo you want to sense...