Danke Beserwessi:
Dank der Festkomma ArcTan(x) und ArcTan2(y,x) funktionen konnte ich wieder eine erhebliche Beschleunigung erziehlen.
400000/149000 Berechnung der Gelenkwinkel IK (2.7 mal schneller)
Aufgrund dieses Leistungssprungs schafft der µC alle nötigen berechnungen in nur 10ms zu erledigen. Also noch reichlich Zeit für die PWMs vlt. eine dynamische Fußnachsetz Funktion und I²C kommunikation.
Allerdings reizt es mich die floats gänzlich aus meinem Bot zu verbannen.
Vlt klapts ja noch mit den letzten beiden Funktionen, auch wenn mich die Überlaufkontrolle mitlerweile ankotzt.
Code:
//**************************************************//	974 Takte*
int16_t arctan(long number)							//Eingabe mit Skalierung 1024 = 10 Festkommabits Ausgabe 7 Festkommabits
{
	int8_t v;
	unsigned long x;
	if (number==0)		return 0;
	if (number>0)		{x= number;v=1;}			// Betrag und Vorzeichen von number
	else				{x=-number;v=-1;}		
	if (x> 8388608UL)	return (v* Wd2);			//für |x|>2^13 gilt +-90°
	if (x<1024)													//für x<1 gilt allg.: x/(1+0.28x)
		return v*(((209539UL*x)>>3)/(((x*x)>>10)+3657));		//zähler/nenner= 2^-17/2^-10=2^-7 
	else if(x<60000UL)											// sonst gilt allg.: PI/2-x/(x²+0.28)
		return v*(Wd2-((58671UL*(x>>3))/(((x*x)>>10)+287)));	//zähler/nenner= 2^-17/2^-10=2^-7
	else
	{
		x=(x>>5);												//anpassung zum Schutz vor Überläufen
		return v*(Wd2-(((58671UL*x)>>8)/(((x*x)+287)>>10)));	//zähler/nenner= 2^-7/2^0=2^-7
	}
}
//**************************************************//

//**************************************************//  1729 Takte*
int16_t arctan2(int16_t y,int16_t x)				//Eingaben Ganzzahlig, Ausgabe 7 Festkommabits
{
	if(x==0)										//Vorzeichenorientierte Fallunterscheidung
	{
		if (y==0) 		return 0;
		else 			return signi(y)*Wd2;
	}
	if(x>0)				return arctan(((long)y<<10)/x);
	else				return arctan(((long)y<<10)/x)+signi(y)*W;
}
//**************************************************//
mfg WarChild