Проблема решена. Дело было в ПИД регуляторе. Поставил другой взятый на просторах интернета, теперь все работает как надо. Если кому интересно, вот код.
Проблема решена. Дело было в ПИД регуляторе. Поставил другой взятый на просторах интернета, теперь все работает как надо. Если кому интересно, вот код.