PID-а се слага във веригата, може с него, може и без него. Но без него ще имаш "насам-натам".
И не, не е филтър. Макар че ще ти помогне и да филтрираш показанията - именно чрез интегралната си съставка.
Не ти трябват синуси, повярвай ми. Тези сметки са много ресурсоемки за едно ардуино.
Тръгваш от точка с координати 10,10, трябва да стигнеш до точка с координати 1000, 1000. По моята сметка коефициента се получава 990 / 990 = 1
Първото ти измерване ти дава координати 20,22. 1000-20 е 980, 1000 - 22 е 978. Коефициента е 980/978, или 1.002. T.e. трябва ти завой в едната посока (в случая ще е надясно, но все се надявам че ще можеш да обърнеш движението на руля ако е грешно
)
Втори измерване - дава ти координати 30,31. Съгласно резултатите от предното измерване коефициента е 1.002. Сегашното е 1000-30 =970 и 1000-31 = 969 или 970/969
=> коефициент 1.001. Той е по-малък от предишния, съответно корекцията трябва да е в обратна посока - наляво. Щото ако продължиш в тази посока, следващото измерване ще е 40,40, следващото - 50,49 и ... отиваш у канавката
Трето измерване ти дава 40,41. 1000-40 = 960, 1000 - 41 = 959, коефициент ... 1.001. Идеално, същия като предния, продължаваме направо.
И така си вървиш, сравнявайки две числа - коефициента от текущото измерване с това от предишното. Ако двата коефициента са равни - вървиш направо, Ако са различни - завиваш в съответната посока.
Трябва ти и една сметка все пак дали не вървиш точно в противоположната посока, но се надявам една Питагорова теорема да можеш да имплементираш