-
für Interessierte:
Hier meine selbst geschriebenen und auch selsbt ausgedachten (außer sinus) Festpunktfunktionen.
Alle Funktionen arbeiten im Gradmaß!!! mit 7 Festkommabits das erspart einem die umrechnung mit PI
Code:
int16_t arccos(long number); //Eingabe 14 Festkommabits Ausgabe 7 Festkommabits
int16_t arctan(long number); //Eingabe 10 Festkommabits Ausgabe 7 Festkommabits
int16_t arctan2(int16_t y,int16_t x); //Eingaben proportional Ausgabe 7 Festkommabits
int16_t cosinus(long x); //Eingabe 7 Festkommabits Ausgabe 14 Festkommabits
inline int8_t signf(float number); //liefert 1 für positive und -1 für negtive Zahlen des tüps float
inline int8_t signl(long number); //des typs long (32-Bit)
inline int8_t signi(int16_t number); //des typs int16_t
int16_t sinus(long x); //Eingabe 7 Festkommabits Ausgabe 14 Festkommabits
uint16_t sqroot(unsigned long number); //Eingabe positive ganzzahl
//**************************************************// 100-1200 Takte*
uint16_t sqroot(unsigned long number) // O(n)=log_2(n) Max Abweichung meistens 0 oder 1, selten 2 bzw. 0.1% für große zahlen
{
if(number==0) return 0; //triviale Fälle abfangen
if(number==1) return 1;
unsigned long r2;
uint16_t root=0,delta;
if(number>=64536) //falls die Zahl größer als eine 16 Bit Zahl ist
delta=32768; //maximal mögliche Wurzel versuchen (2^15+2^14+2^13...=65535)
else
delta=(number>>1); //sonst beginn bei der Zahl selbst druch 2
while (delta) //liefert binnen 15 Iterationen eine Lösung
{
r2=(unsigned long)root*root;
if(r2<number)
root+=delta;
else
root-=delta;
delta=(delta>>1);
}
return root;
}
//**************************************************//
//**************************************************// 762 Takte*
const long W=23040UL; //Winkel eines Halbkreises Skalierung 128 = 7 Festkommabits
const long W2=46080UL; //Winkel eines Vollkreises
const uint16_t Wd2=11520; //Winkel eines Viertelkreises häufige verwendung spart Rechenzeit
int16_t sinus(long x) //
{
if(x>W2) x%=W2; //Skalierung auf den bereich [0;W2] (Sinuswelle)
if(x>W) x=-x+W; //x wird auf das Intervall [-W;W] normiert (aufgrund der Achsensymmetrie um x=0)
if(x<-W) x=-x-W;
//Parabel
const long B = 182; //2^-13 //(4/W); //linearer Formfaktor der Parabel
const long C = -259;//2^-21 //(-4/(W*W)); //quadratischer Formfaktor der Parabel
long y=((B*x)>>6)+((((C*x)>>11)*x*signl(x))>>10); //2^-14 //Funktionswert der einfachen Parabel
//Parabel Korrektur
const long Q = 99; //2^-7 //0.775; //Linearfaktor der einfachen parabel
const long P = 29; //2^-7 //0.225; //Linearfaktor der quadrierten korektur Parabel
y=((Q*y)>>7)+((((P*y)>>7)*y*signl(y))>>14); //2^-14 //Endergebnis nach gewichteter Summenbildung
return y;
}
//**************************************************//
//**************************************************// 768 Takte*
int16_t cosinus(long x) //
{ //
return sinus((x+Wd2));
}
//**************************************************//
//**************************************************// 1800 Takte*
int16_t arccos(long number) //
{ //
if (number==0) return Wd2; // 0 Abfangen
else if (number>=16384) return 0; // Werte größer 1 und kleiner -1 abfangen
else if (-number>=16384) return W;
if (signl(number)==1) // number>=0
return ((((long)sqroot(16384-number)*151511)>>11)+(((long)(16384-number)*4101)>>15)); //Faktoren 2^-11 und 2^-8 Ergebnis 2^-7
else
return (W-((((long)sqroot(16384+number)*151511)>>11)+(((long)(16384+number)*4101)>>15)));
}
//**************************************************//
//**************************************************// 974 Takte*
int16_t arctan(long number) //
{ //
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) //
{ //
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;
}
//**************************************************//
//**************************************************// 5 Takte*
inline int8_t signf(float number)
{
if (signbit(number))
return -1;
else
return 1;
}
inline int8_t signi(int16_t number)
{
if(number&(1<<15))
return -1;
else
return 1;
}
inline int8_t signl(long number)
{
if(number&(1<<31))
return -1;
else
return 1;
}
//**************************************************//
-
Sehr interessante Funktionen!
Festpunktarithmetik zu benutzen habe ich mir auch schon überlegt, mache das aber in nur wenigen Funktionen. (arccos habe ich z.B. so geschrieben, mit einem Tabellen lookup, das Ergebniss ist zuwar ganzzahlig, dafür dauert die Berechnung aber auch nur 30Takte. Eine Tabelle von 1024byte im Flash stört ja nicht).
Das mit der Datenflut ist nicht unbedingt notwendig. Ich z.B. mache das so, dass ich im globalen Systen (auf dem ersten Controller) eine Funktion berechne nach der sich der Fuß bewegen soll und dann diese dem anderen Controller übergebe. Dieser rechnet dann für die jeweiligen Zeiten die nötige Position aus und fährt sie an. So habe ich mehr Spielraum und kann es mir leisten, mit float zu rechnen. Außerdem kann man dann leichter Peripherie anschließen und auswerten.
Gerade bin ich aber immernoch dabei, die mathematischen Funktionen zu erfinden, mit denen sich der Hexa bewegen soll. ist nicht ganz einfach sowas...
Bin gerade dabei, das Kurvengehen um einen bestimmten Radius zu implementieren. Ist nicht ganz einfach (aber das kennst du ja bestimmt =) )
Gruß, Yaro
-
Hallo allerseits,
ich stand seinerzeit auch vor dem Problem, 18 Servos gleichzeitig mit einem Controller anzusteuern. Realisiert habe ich es letztendlich mit einem Atmega128 und dessen beiden 16-Bit Timern. Jeder Timer steuert 9 Servos mit einer nur wenigen Takten langen ISR, damit sich beide Interrupts nicht merkbar in die Quere kommen. Die Servos werden nacheinander angesteuert, womit auch keine allzu starken Lastspitzen auftreten.
Die Servoansteuerung auf diese Art verursacht fast keine Controller-Last, sodaß der Controller noch die IK für alle 6 Beine mitberechnen konnte.
Gruß MeckPommER
-
ja...
ich habe aber eine (aus meiner sicht) geniale idee.
ich habe ein array in dem ist die position der fußpunkte enthalten, wie er ganz normal stehen soll. das bleibt so konstant. Und nun wird dies einmalig in ein zweites array kopiert, mit dem gearbeitet wird. (aufstehprozess).
Nun kommt die 1. funktion.
Die kopierten kordinaten werden mit 50Hz um ds_x, ds_y verschoben und um dphi_z gedreht und im gleichen array gespeichert. dies führt zu einer kontinuierlichen verschiebung der fußpunkte. Mit diesen parametern lässt sich jede fortlaufende Bewegung realisieren. Die anderen Freiheitsgrade kommen später.
jetzt wird eine funktion aufgerufen. Mit der die Abweichung der koordinate jedes gedreht und verschobenen fußpunktes vom stillstehenden ausgangspunkt perechnet wird (natürlich wird da zwischen steigender und fallender abstand unterschieden). Der fuß mit maximalem abstand wird als erstes gesetzt.
Jetzt kommt die frage wohn? dies lässt sich ebenfalls anhand von ds_x, ds_y und dphi_z feststellen. in verbindung mit dem ausgangspunkt wird der neue fußpunkt bestimmt und angefahren. (vlt sinusförmig).
nun folgen die sonstigen statischen transformationen um x,y,z, und phix, phiy und phiz. fertig ist das laufen.
das komplizierteste daran ist das wohin. mit dem Fuß und welcher als nächstes. die eigentliche bewegung des torsos auf einer kurvenbahn ist dagegen banal.
mfg WarChild
-
@ Meckpommer
Ich habe gerade nochaml druch deine HP gestöbert.
Dein Bot hat ja schon ein paar evolutionen mehr hinter sich als meiner, aber zu der form der bewegung deines beines: Wenn du dir die gedanken mit sanften bewegungen machst, solltest du dann nicht alle punkte deiner bewegung betrachten? das mit dem abheben und aufsetzen ist sehr sanft bei dir, aber der mathematische ausdruck für sanft oder flüssig ist Stetigkeit, d.h. eine funktion ohne Sprünge und Knicke. Was würdest du von einer Cosinusfunktion, umgedreht und verschoben ins positive halten? Also: 1-cos(x) (optinal normieren mit 1/2) von 0° bis 180° da hast du auch deinen sanften Anfang und Ende, aber dafür keinen Schlagartigen wechsel von heben zu senken. Ist vlt. auch schonender für die servos.
Aber sonst: großes Vorbild!
mfg WarChild
-
Hört sich sehr durchdacht an, auch wenn ich zugeben muss, dass ich es nicht wirklich durchblickt habe... =)
Gruß, Yaro
-
Jo, Warchild,
der Gedanke kam mir auch schon. Hat nur den Nachteil, das das Bein dann zu lange in Bodennähe "herumschleift". Bei 0 bzw. 180 Grad bleibt der Cosinus lange in den jeweiligen Maximalbereichen, was bei dem Bein zur Folge hat, das der Bot das Bein schon als angehoben ansieht, während es in Wirklichkeit noch so dicht über dem Boden ist, das es noch schleift und die Bewegung des Bots stört.
Das mit der Schonung der Servos ist immer zu beachten, da hast du absolut recht. Deshalb haben die Beine in Marvin Version 4 auch keine Servos mehr ^^ ... aber dazu in 2-3 Monaten mehr, wenn ich meine Ideen umgesetzt habe, oder aus bastlerischer Verzweiflung dem Fenster gesprungen bin ;-)
Das mit dem Drehen und Schieben des Koordinatensystems birgt auch so seine Tücken. Wenn du Botposition und -winkel vom Ursprung aus bearbeitest, mußt du erst drehen und dann schieben. Ansonsten würde ein Bot, der sich einen Meter vom Ursprung entfernt hat, nicht um seine eigene Achse drehen, sondern um den Ursprung, und mathematisch betrachtet fliegen oder im Boden versinken.
Gruß MeckPommER
-
Hi,
ich habe mir auch ein Konzept für eine Kinematik zurechtgelegt: Der Controller kennt die Position des Bots im raumfesten Koordinatensystem, die Position des Bodenpunktes (ebenfalls im raumfesten System), sowie die drei (Euler-)Winkel für die Drehung im Raum.
Nun wird zuerst der Ursprung des roboterfesten Systems in den Ursprung des raumfesten Koordinatensystems gelegt, der Bodenpunkt mitverschoben. Nun wird die Position des Bodenpunktes vom raumfesten ins roboterfeste Koordinatensystem umgerechnet (https://www.roboternetz.de/phpBB2/vi...=452170#452170), was die weitere Berechnung enorm vereinfacht. Zu guter Letzt wird noch der Befestigungspunkt des Beines (der ja bekannt ist) in den Ursprung verschoben, und die Winkel per Tangens und Cosinussatz ausgerechnet.
Nun frage ich mich, wo der Haken an der ganzen Sache sein könnte? Mir kommt das nämlich nach einigem Nachdenken zu einfach vor...
Könnt ihr was entdecken?
Viele Grüße
ikarus_177
-
nein das siht ganz ok aus. Man sollte sich nur von dem gedanken verabschieden, die bot von einem ortsfesten punkt aus betrachten zu wollen. ich habe bei mir als ursprung des globalen Koordinatensystems den Schwerpunk des Bots genommen. wenn ich nun transformiere, dann laufe ich gefahr, dass meine transformierten fußpunkte irgendwann im nirvana landen, aber die fußnachsetzfunktion holt die fußpunkte immer wieder ran. Ich rate also davon ab irgendeinen punkt, der nicht auf dem Bot liegt und sich nicht mitbewegt als bezugspunkt zu wählen. Ich denke das erspart einem einiges an rechnerei.
mfg WarChild.
-
Ich habe zwei Koordinatensysteme - ein Ortsfestes und ein Bot-orientiertes. Das ist für den Fall sinnvoll, das der Bot irgendwo z.B. ein Hindernis feststellt und dieses auf einer internen Karte abspeichert. Hat der Bot kein festes System, in das er seine Sensorwerte eintragen kann, so kann er Sensorwerte nicht ortsfest positionieren, sondern kann nur aktuelle Messwerte verarbeiten.
Gruß MeckPommER
