это звуковые датчики?
То есть там передатчик и приемник?
Откуда вы взяли методику измерения? Что-то мне кажется у вас оно вообще не правильно сделано... Кто набег фазы измеряет разницей времени регистрации фронта?
надо перемножить сигналы
cos(a)*cos(a+fi) = 1/2(cos(2a + fi) + cos(fi))
дальше вы отфильтровываете 2 частоту, и получаете 1/2*cos(fi), ну а дальше получаете значение fi.
можно сделать фазовую подстройку через PID, то есть берете ваш cos(a) и возвратный cos(a+fi) и формируете еще один сигнал cos(a + fi1), а fi1 подбираете в PID регуляторе чтобы фазы не расходились.
Эти методы значительно более точные, чем ваши +- лапоть. И многие ваши метания отпадут сами собой, когда сделаете правильно....
А в случае звуковых датчиков, сдвиг фазы никто вообще не меряют. Измеряют время между запросом и ответом
алгоритм
1. включаем передатчик
2. фиксируем ответ на приемнике (если ответа нет, то расстояние больше рабочего)
3. выключаем передачик, и сразу запускаем счетчик времени
4. ждем когда пропадает ответ на приемнике, фиксируем время.
дальше из времени и скорости звука, с коррекцией на влажность и температуру (если очень хочется) получается расстояние....
Изобретать велосипеды хорошо, но и на чужие смотреть тоже иногда надо