So: Felix, du hattest recht
jetzt habe ich die minimale Anzahl an Float operationen erreicht, es bleiben nur je sechs mal zwei Wurzeln, vier Winkelfunktionen und vier Divisionen bei der einen und sechs Winkelfunktionen bei der anderen Funktion, die ich nur durch eigene Festkommafunktionen beschleunigen könnte.

vorher / nachher Funktion
2500/325 Berechnung der benötigten Pulslängen (8 mal schneller)
34000/382 Berechnung der Translation (100 mal schneller)
175000/51000 Berechnung der Rotation (3.4 mal schneler)
400000/276000 Berechnung der Gelenkwinkel IK (45% schneller)
jetzt liege ich bei eine gesamten Rechndauer mit Interrups bei 22.5ms,
also fast echtzeit. Eigentlich muss mich nurnoch die Beschleunigung von der IK Interessieren, da diese mit 17,5ms den größten Ausschlag macht.

vielen Dank an Alle, ohne euch wäre ich vorher warscheinlich schon verzweifelt!!! =D>
edit: meine Festpunkt Sinus/Cosinus Funktion mit max 0.0001 Abweichung funktioniert jetzt. Also:
175000/11100 Berechnung der Rotation (15 mal schneler) \/
Fehlt nurnoch eine Festpunkt Wurzel und Arkus-funktionen

mfg WarChild