Algorytm sterowania z użyciem magnetometru

[Mortis]

Cześć wszystkim!

Mam problem z sterowaniem z użyciem magnetometru. Kiedy robot ma się ustawić na kącie np 120 stopni to sprawa dosyć prosta - jak większy kąt to skręcaj w lewo jak mniejszy to w prawo (oczywiście jakiś jeszcze regulator proporcjonalny). Problem rozpoczyna się gdy ma skręcić na np 10 stopni. a jest ustawiony na 350 to zgodnie z tą zasadą będzie robił prawie pełne koło w lewo bo 350 jest większe od 10. Jak tego uniknąć?

[ethanak]

na przykład: jeśli wyliczony kąt x jest większy niż 180 stopni, trzeba skręcać w drugą stronę o kąt 360-x.