I didn't read through the whole sketch but that part with the map function immediately set alarm bells ringing.
If you want something to happen at a fixed interval of the order of a few milliseconds, I would just use the timing technique demonstrated in the 'blink without delay' sample sketch but based on micros rather than millis, to detect when the interval has elapsed.
There are two issues which I think would be significant.
Your timer comparison needs to deal with timer overflow. This is easily done if you code it like this:
if((micros() - lastMicros) > intervalMicros)
{
...
If you want to keep the average frequency accurate (to the extent possible with an uncalibrated clock) then you also need to update the timer based on when your timer became due rather than when you detected it. In other words:
if((micros() - lastMicros) > intervalMicros)
{
lastMicros += intervalMicros;
...