Hast du denn mal die Variablen Angle, AnglePi und zorder[K] geloggt?
Nein.
Was mit jetzt ein Log da bringen würde entschließt sich mir.
Aber wie gesagt bin bereit jemanden das Projekt zu schocken damit er es mal testen kann.
Was nicht bedeuten soll das er meine Arbeit tun soll
Du würdest sehen, ob die errechneten Werte so stimmen oder ob da schon VOR der Sortierung etwas faul ist.