Hallo Tomy83,
das hört sich nach einem "Dreiteiler" an
Viele Grüße
Jörg
Hallo Tomy83,
das hört sich nach einem "Dreiteiler" an
Viele Grüße
Jörg
Wird zwar gefrickel, aber ich Probier es später nochmal. Ist aber Irrsinn, die ganze C-File ist NUR 25kb.
- - - Aktualisiert - - -
hier der Dreiteiler für den C-Code. Damit die Variablen verständlich werden zwei Bilder dazu.
Es handelt sich nicht um das vollständige Programm, nur die Berechnungen für die Beine.
Bitte aber nicht aufknüpfen und vierteilen, für den Code.
- - - Aktualisiert - - -Code://-------------------------------- Servowerte für ISR ------------------------------------- // char SERVO_CHANEL[4];//3 //Angesteuerter Servo je Timer/Counter ISR unsigned short int SERVO_HU[7]; //Hüftservo unsigned short int SERVO_OS[7]; //Oberschenkel Servo unsigned short int SERVO_US[7]; //Unterschenkel Servo unsigned short int SERVO_SUMME[4]; //Summe aller Servozeiten für Perioden berechnung ISR Servo 50Hz static unsigned int Servo_Refresh_F = 57100; //Ansteuerung der Servos aller xxHz (57100 = 70Hz / 14ms) //ENDE----------------------------- Servowerte für ISR ------------------------------------- //Konstaten********************************************************************************* const float L_Bein_Vertikal = 123; //Vertikaler Abstand von Servo_HU zu Robotermitte in mm const float L_Bein_Horizontal = 70; //Horrizontaler Abstand von Servo_HU (Vorn/Hinten Bein:1,2,3,6) zu Robotermitte in mm const float L_Bein_Horizontal_2 = 95; //Horrizontaler Abstand von Servo_HU (VMitte Bein:4,5) zu Robotermitte in mm float BEIN_OS = 80; //Beinlänge Oberschenkel float BEIN_US = 152; //Beinlänge Unterschnekel static const float OS_OFF = 45; //Oberschenkel Offsetwinkel static const float US_OFF = -32.28; //Unterschenkel (Knie) Offsetwinkel static const float L_Bein_Horrizontal_Offset = 90.05; //Abstand Fußspitze zu "servo_HU" Offset static const float FAKTOR = 47.2086; //Faktor zur Umrechnung von Grad in Schritte static const float FIX_HOHE = 79.70; //Höhe Servo & Lager unterhalb Drehpunkt OS in mm float Hohe_Offset = 3; //bei Beinanheben für Rückhub XX in mm static const float Maxauslenkung_Bein_Eingabe = 42; //Eingabe in mm der Maximalen Auslenkung des Beins (25° = 41,99mm) static const int Geschwindigkeit = 310; //Legt den Teiler für ADC fest, um die Geschwindigkeit zu Manipulieren (Entspricht dem empfangenen Maximalwert) static const float Uberlappenderlauf = 4; //Soviel mm (die Hälfte) wir der Schritt des Beins nach dem Umsetzen überlappend gelaufen static const signed int Radius_Min = 300; //kleinst möglicher Radius der als Kurve gelaufen werden kann. Hängt von Überlappenderlauf ab und beträgt bei 4mm = 272,16. Ansonsten soll sich der Roboter um die eigene Achse drehen const float Berechnungszeit_Kurve_Gerade = 2.1445; //Unterschiedliche Berechnungszeiten verursachen keine gleichmäßige Geschwindigkeit im Kurven/Geradeaus-Lauf const float Maximal_Geschwindigkeit = 600; // mm/s = 8Km/h (8km/h = 8000000mm/h = 2222.22mm/s) //ENDE Konstaten***************************************************************************** float Radius_Kurve; //Kurvenradius der gelaufen werden soll volatile float TEST[10]; //-------------------------------- Beinhöhe und Maximalauslenkung Bein -------------------- // float HiMe_MaxAUSLbein; //Hilfmerker Maximale Auslenkung Bein / berechnet MaximaleAuslenkung Bein unter beachtung der Höhe Global float Maxauslenkung_Bein; //Maximale auslenkung des Beins in mm ---> muß float sein, sonst wird die berechung (div/0) nicht durchgeführt ->32 float Hohe_Global; //Globale Roboter Höhe in Standart ist 12mm char Drehmodus = 0; //in diesem Modus wird der Roboter um seine Mitte gedreht char Drehmodus_Speicher; //Speichert den Drehmodus bei GRUNDSTELLUNG, um richtige Sollwertauswahl zu machen. char Servo_EIN; //Servos AUS oder EIN schalten //ENDE---------------------------- Beinhöhe und Maximalauslenkung Bein -------------------- //------------------------------- Bein Berechnungen ---------------------------------------- // float L_Bein_Horrizontal_IST[7] = {0,L_Bein_Horrizontal_Offset,L_Bein_Horrizontal_Offset,L_Bein_Horrizontal_Offset,L_Bein_Horrizontal_Offset,L_Bein_Horrizontal_Offset,L_Bein_Horrizontal_Offset}; //(Wirklänge Bein gesamt) = 90.036923440513637335512687671373 (eigentlich 90.05mm) / Horrizontaler Abstand Fußspitze zu Drehpunkt "Servo_HU" float L_Bein_Vertikal_IST[7]; //(Länge Sollweg D) / Soviel hat sich das Bein von der 0° Position entfernt, in mm float WINKEL_Bein[7]; //aktueller Beinwinkel für Berechnung (-90° - +90°) float WINKEL_OS[7]; //Winkel Oberschenkel float WINKEL_KNIE[7]; //Winkel Knie (Unterschenkel) char Vor_Ruck_Hub[7]; //Vorhub oder Rückhub vom Bein float Ruckhub_Faktor; //Rückhubgeschwindigkeit mit Synchrowinkel und Überlappenderlauf float Startwinkel; //berechnungshilfe für Überlappenden-Lauf / Offsetwert was Bein aus Mittelstellung verschoben ist char Ruckhub_Geschwindigkeit; //Faktor wie schnell sich das Bein zurück bewegen muß char Laufmodus = 51; //Anzahl der Beine im Vorhub-Rückhub 3-3 / 4-2 / 5-1 char Laufmodus_speicher; //Speichert den Laufmodus, vor desen Festlegu7ng (USART-EMPFANG) um eine Änderung mitzubekommen signed int Roboter_bewegung_mm; //Bewegung vom UART empfangen signet int float Synchro_Bein; //auf dieses Bein/Wert wird Synchronisiert (0-100% (-L_Bein_Vertikal) - (+L_Bein_Vertikal)) / SOLL-Wert float Schrittweite_MAX_Uber[7]; //Schrittweite_Max wurde überschritten und neuer Grenzwert wir hier gespeichert char Max_Anz_Beine_Ruckhub; //Maximale Anzahl an Beinen die sich im Rückhub befinden dürfen void Bein_Berechnungen (unsigned char y) // (Bein nummer die Berechnet wird) { float L_Bein_Vertikal_rech; //Vertikaler Abstand von Servo_HU zu Robotermitte in mm [für Berechnung positiv(vodere Beine) oder negativ(hintere Beine)] float L_Bein_Horizontal_rech; //Horizontaler Abstand von Servo_HU zu Robotermitte in mm [für Berechnung positiv(vodere Beine) oder negativ(hintere Beine)] float R_Fussspitze; //Tatsächlich zu laufender Radius pro Bein float R_Fussspitze_Ideal = 0; //Idealer R_Fußspitze bei Beinstellung 0° float HiMe_R_Fusspitze = 0; //Hilfsmerker von R_Fussspitze, hier wird entweder der aktuelle Radius Fußspitze oder der Ideale Radius (herangeführt) eingesezt float Winkel_R_zu_Fuss_IST; //Winkel von der Geraden (Radiusmittelpunkt zu Robotermitte) zur Geraden (Radiusmittelpunkt zu Fußspitze) float Bein_bewegung_mm = 0; //Bewegung des Beins in mm float Zentrierwinkel; //Zentrierwinkel für Vorwärtsbewegung, aus Vorwärtsbewegung in mm wird die Gerade (Radius-Mittelpunkt zu Fußspitze) um soviel Grad verschoben float Winkel_R_zu_Fuss_SOLL; //Winkel von der Geraden (Radiusmittelpunkt zu Robotermitte) zur Geraden (Radiusmittelpunkt zu Fußspitze) SOLL-Position float L_R_zu_Fuss_SOLL; //Horrizontaler Abstand von Drehpunkt Radius zu Fußspitze (SOLL position) float L_Bein_Horrizontal_SOLL; //Horrizontaler Abstand von "Servo_HU" zu Fußspitze (SOLL-Position) float Sehnenlange; //eine Gerade die im um "Winkel_R_zu_Fuss_SOLL" geneigt ist, und sich aus der Vorwärtsbewegung in mm ergibt. Entspricht der zwei geraden "R_Fußspitze" mit den "Winkel_R_zu_Fuss_SOLL" float L_Bein_Vertikal_anderung; //Länge um die sich L_Bein_Vertikal änder soll (Abstandsänderung von 0° Stellung Bein aus) float L_Bein_Vertikal_SOLL; //Längenänderung des Sollweg D (Vorwärtsbewegung) float L_BEIN_SOLL; //LA_BEIN_SOLL nach Änderung durch Bein_Bewegung_mm float L_Bein_Vertikal_SOLL_rech; //Ist Stellung Bein Vertikal immer POSITIV, wird benötigt um Höhe des Beines zu berechnen float HoheBein; //Bein Höhe -32768 bis +32767 signed short int float DIAGONALE; //Diagonale aus Drehpunkt Höhe OS und Länge Bein Soll float HILF_A; //Hilfslänge a für Höhe auf Diagonale float WINKEL_a; //Winkel ALPHA float Schrittweite_MAX = Maxauslenkung_Bein; //Maximale Schrittweite des Beins (Auslenkung) aus Maxauslenkung_Bein oder berechnet aus Kurvenradius float UberLauf_rech = Uberlappenderlauf; //umrechung Uberlappenderlauf in Kurvenmaße (geniegt) / Soviel mm (die Hälfte) wir der Schritt des Beins nach dem Umsetzen überlappend gelaufen float W_R_zu_FussSp; //Winkel von Drehpunkt-Radius zu Fußspitze @ 0° float W_MaxAuslenk_Bein; //Winkel-Verschiebung/Neigung durch Sehnenlänge (aus "Maxauslenkung_Bein") / Winkel von Bein @ 0° zu Maxauslenkung im Bezug zu Radiusmittelpunkt float Synchro_Faktor = 1; //Um diesen Betrag muß sich das zu Synchronisierende Bein schneller/langsamer bewegen float Synchro_Bein_IST; //Das zu Synchronisierende Bein hat diesen IST Wert float Synchro_Bein_SOLL; //Das zu Synchronisierende Bein sol diesen Wert haben float Synchro_Spreizung; //um diese Gradzahl sind die Beine im Laufmodus XX gespreizt signed char x = 1; //Hilfsvariable für Berechnung des Synchro-Soll-Wert (Anzahl der Spreizungswinkel) float L_Bein_Vertikal_IST_rech; //L_Bein_Horrizontal_IST immer positiv für eine berechnung char HiMe_Schrittweite_Max; //Schrittweite_MAX wurde überschritten und L_Bein_Vertikal wird um überschritt zurückgeführt, daher auch L_Bein_Horrizontal_SOLL ändern char SW_MAX_Verk; //Schrittweite_MAX wird verkürzt um wieder in Snchronisation zu kommen. Hilfsvariable um Formel richtig auszuwählen float HiMe_SW_MAX = Maxauslenkung_Bein; //speichert die berechnete Schrittweite_MAX um sie nach der Synchronisation vergleichen zu können char Synchro_Bein_SOLL_Rueckhub; //Synchrobein_SOLL ist ein Rückhubwert / zur Prüfung bei Synchro_Faktor bildung und Schrittweite_MAX einkürzung float Spreizung_MAX_warten; //Maximaler %-Wert den denn der Sollwert hinter den Istwert liegt darf, beim Synchronisieren char Ruckhub_Unterdruckung = 0; //Wenn mehr als 3(?) Beine im Rückhub, oder Längsseitig 2 benachbarte, keinen Rückhub zulassen //------------------------------- Schrittweite_MAX ----------------------------------------- //Schrittweite_MAX (umkehrpunkt für Bein) aus Kurven-Radius berechnen, da jedes Bein eine andere maximale Schrittweite benötigt, bei geradeaus gilt Maxauslenkung_Bein aus "void Hohe_geandert" // Schrittweite_MAX = (Winkel von Drehpunkt-Radius zu Fußspitze @ 0°) -+ (Winkel-Verschiebung/Neigung durch Sehnenlänge (aus "Maxauslenkung_Bein")) * (Gerade von Drehpunkt-Radius zu Fußspitze @ 0°) * (Winkel von Bein @ 0° zu Maxauslenkung_Bein vom Kurvenäußeren Bein mittig) // Winkel von Drehpunkt-Radius zu Fußspitze @ 0° = cos(((atan(L_Bein_Vertikal/((Radius_Kurve*-1)-L_Bein_Horizontal-L_Bein_Horrizontal_Offset))*180/M_PI) // Winkel-Verschiebung/Neigung durch Sehnenlänge (aus "Maxauslenkung_Bein") = -(90-((180-atan(Maxauslenkung_Bein/((Radius_Kurve*-1)+L_Bein_Horizontal_2+L_Bein_Horrizontal_Offset))*180/M_PI)/2)))*M_PI/180) // Gerade von Drehpunkt-Radius zu Fußspitze @ 0° = *( (sqrt(square((Radius_Kurve*-1)-L_Bein_Horizontal-L_Bein_Horrizontal_Offset)+square(L_Bein_Vertikal) )) // Winkel von Bein @ 0° zu Maxauslenkung_Bein vom Kurvenäußeren Bein mittig = * sin((atan(Maxauslenkung_Bein/((Radius_Kurve*-1)+L_Bein_Horizontal_2+L_Bein_Horrizontal_Offset))*180/M_PI)*M_PI/180)); // Nochmal Kopiert in Grundstellung if (Radius_Kurve != 0) //im Kurvenlauf berechnen { if (Drehmodus == 1) { if (y==4 || y==5) //Beine in der Mitte haben einen anderen Radius als die Äußeren 4 { W_MaxAuslenk_Bein = atan(Maxauslenkung_Bein/(L_Bein_Horizontal_2+L_Bein_Horrizontal_Offset))*180/M_PI; //Zentrierwinkel aus Bein @ 0° zu Beinstellung Maximalauslenkung (von "void Höhe Geändert") bezug ist Kurvenmittelpunkt UberLauf_rech = sin(((180-W_MaxAuslenk_Bein)/2)*M_PI/180) * Uberlappenderlauf; //Uberlappenderlauf aus Const umrechnen in geneiget L-Bein-Vertikal Schrittweite_MAX = (L_Bein_Horizontal_2+L_Bein_Horrizontal_Offset) * sin((W_MaxAuslenk_Bein)*M_PI/180); } else { W_MaxAuslenk_Bein = atan(Maxauslenkung_Bein/(L_Bein_Horizontal_2+L_Bein_Horrizontal_Offset))*180/M_PI; //Zentrierwinkel aus Bein @ 0° zu Beinstellung Maximalauslenkung (von "void Höhe Geändert") bezug ist Kurvenmittelpunkt 12,79° W_R_zu_FussSp = atan(L_Bein_Vertikal/(L_Bein_Horizontal+L_Bein_Horrizontal_Offset))*180/M_PI; //Winkel von "Robotermitte zu Kurvenmitte" zu Fußspitze @ 0° 35,54° if (((L_Bein_Vertikal_IST[y] < 0) && (y==1 || y==2)) || ((L_Bein_Vertikal_IST[y] >= 0) && (y==3 || y==6))) // { UberLauf_rech = sin((90-(W_R_zu_FussSp+W_MaxAuslenk_Bein)+(W_MaxAuslenk_Bein/2))*M_PI/180) * Uberlappenderlauf; //2,19mm (eigentlich 3,13mm) Schrittweite_MAX = cos((W_R_zu_FussSp+(W_MaxAuslenk_Bein/2))*M_PI/180)*((sqrt(square(L_Bein_Horizontal+L_Bein_Horrizontal_Offset)+square(L_Bein_Vertikal))) * sin((W_MaxAuslenk_Bein)*M_PI/180)); //32mm } else { UberLauf_rech = sin((90-(W_R_zu_FussSp-W_MaxAuslenk_Bein)-(W_MaxAuslenk_Bein/2))*M_PI/180) * Uberlappenderlauf; //Uberlappenderlauf aus Const umrechnen in geneiget L-Bein-Vertikal 3,79mm (eigentlich 3,72mm) Schrittweite_MAX = cos((W_R_zu_FussSp-(W_MaxAuslenk_Bein/2))*M_PI/180)*((sqrt(square(L_Bein_Horizontal+L_Bein_Horrizontal_Offset)+square(L_Bein_Vertikal))) * sin((W_MaxAuslenk_Bein)*M_PI/180));//38mm } } } else //wenn Kurvenlauf { if (Radius_Kurve < 0) //wenn Radius_Kurve negativ ist { W_MaxAuslenk_Bein = atan(Maxauslenkung_Bein/((Radius_Kurve*-1)+L_Bein_Horizontal_2+L_Bein_Horrizontal_Offset))*180/M_PI; //Zentrierwinkel aus Bein @ 0° zu Beinstellung Maximalauslenkung (von "void Höhe Geändert") bezug ist Kurvenmittelpunkt if (y==1 || y==3) { W_R_zu_FussSp = atan(L_Bein_Vertikal/((Radius_Kurve*-1)-L_Bein_Horizontal-L_Bein_Horrizontal_Offset))*180/M_PI; //Winkel von "Robotermitte zu Kurvenmitte" zu Fußspitze @ 0° if ((L_Bein_Vertikal_IST[y] > 0 && y==1 ) || (L_Bein_Vertikal_IST[y] <= 0 && y==3 )) //wenn Bein sich zu der Robotermitte hin bewegt { UberLauf_rech = sin((90-(W_R_zu_FussSp-W_MaxAuslenk_Bein)-(90-((180-W_MaxAuslenk_Bein)/2)))*M_PI/180) * Uberlappenderlauf; //Uberlappenderlauf aus Const umrechnen in geneiget L-Bein-Vertikal Schrittweite_MAX = cos(((W_R_zu_FussSp)-(90-((180-W_MaxAuslenk_Bein)/2)))*M_PI/180)*((sqrt(square((Radius_Kurve*-1)-L_Bein_Horizontal-L_Bein_Horrizontal_Offset)+square(L_Bein_Vertikal))) * sin((W_MaxAuslenk_Bein)*M_PI/180)); } if ((L_Bein_Vertikal_IST[y] <= 0 && y==1 ) || (L_Bein_Vertikal_IST[y] > 0 && y==3 )) //wenn Bein sich von der Robotermitte weg bewegt { UberLauf_rech = sin((90-(W_R_zu_FussSp+W_MaxAuslenk_Bein)+(90-((180-W_MaxAuslenk_Bein)/2)))*M_PI/180) * Uberlappenderlauf; //Uberlappenderlauf aus Const umrechnen in geneiget L-Bein-Vertikal Schrittweite_MAX = cos(((W_R_zu_FussSp)+(90-((180-W_MaxAuslenk_Bein)/2)))*M_PI/180)*((sqrt(square((Radius_Kurve*-1)-L_Bein_Horizontal-L_Bein_Horrizontal_Offset)+square(L_Bein_Vertikal))) * sin((W_MaxAuslenk_Bein)*M_PI/180)); } } if (y==2 || y==6) { W_R_zu_FussSp = atan(L_Bein_Vertikal/((Radius_Kurve*-1)+L_Bein_Horizontal+L_Bein_Horrizontal_Offset))*180/M_PI; //Winkel von "Robotermitte zu Kurvenmitte" zu Fußspitze @ 0° if ((L_Bein_Vertikal_IST[y] > 0 && y==2 ) || (L_Bein_Vertikal_IST[y] <= 0 && y==6 )) { UberLauf_rech = sin((90-(W_R_zu_FussSp-W_MaxAuslenk_Bein)-(90-((180-W_MaxAuslenk_Bein)/2)))*M_PI/180) * Uberlappenderlauf; //Uberlappenderlauf aus Const umrechnen in geneiget L-Bein-Vertikal Schrittweite_MAX = cos(((W_R_zu_FussSp)-(90-((180-W_MaxAuslenk_Bein)/2)))*M_PI/180)*((sqrt(square((Radius_Kurve*-1)+L_Bein_Horizontal+L_Bein_Horrizontal_Offset)+square(L_Bein_Vertikal))) * sin((W_MaxAuslenk_Bein)*M_PI/180)); } if ((L_Bein_Vertikal_IST[y] <= 0 && y==2 ) || (L_Bein_Vertikal_IST[y] > 0 && y==6 )) { UberLauf_rech = sin((90-(W_R_zu_FussSp+W_MaxAuslenk_Bein)+(90-((180-W_MaxAuslenk_Bein)/2)))*M_PI/180) * Uberlappenderlauf; //Uberlappenderlauf aus Const umrechnen in geneiget L-Bein-Vertikal Schrittweite_MAX = cos(((W_R_zu_FussSp)+(90-((180-W_MaxAuslenk_Bein)/2)))*M_PI/180)*((sqrt(square((Radius_Kurve*-1)+L_Bein_Horizontal+L_Bein_Horrizontal_Offset)+square(L_Bein_Vertikal))) * sin((W_MaxAuslenk_Bein)*M_PI/180)); } } if (y==4) { UberLauf_rech = sin(((180-W_MaxAuslenk_Bein)/2)*M_PI/180) * Uberlappenderlauf; //Uberlappenderlauf aus Const umrechnen in geneiget L-Bein-Vertikal Schrittweite_MAX = ((Radius_Kurve*-1)-L_Bein_Horizontal_2-L_Bein_Horrizontal_Offset) * sin((W_MaxAuslenk_Bein)*M_PI/180); } if (y==5) { UberLauf_rech = sin(((180-W_MaxAuslenk_Bein)/2)*M_PI/180) * Uberlappenderlauf; //Uberlappenderlauf aus Const umrechnen in geneiget L-Bein-Vertikal Schrittweite_MAX = ((Radius_Kurve*-1)+L_Bein_Horizontal_2+L_Bein_Horrizontal_Offset) * sin((W_MaxAuslenk_Bein)*M_PI/180); } } else //wenn Radius_Kurve positiv ist { W_MaxAuslenk_Bein = atan(Maxauslenkung_Bein/(Radius_Kurve+L_Bein_Horizontal_2+L_Bein_Horrizontal_Offset))*180/M_PI; //Zentrierwinkel aus Bein @ 0° zu Beinstellung Maximalauslenkung (von "void Höhe Geändert") bezug ist Kurvenmittelpunkt if (y==1 || y==3) { W_R_zu_FussSp = atan(L_Bein_Vertikal/(Radius_Kurve+L_Bein_Horizontal+L_Bein_Horrizontal_Offset))*180/M_PI; //Winkel von "Robotermitte zu Kurvenmitte" zu Fußspitze @ 0° if ((L_Bein_Vertikal_IST[y] > 0 && y==1 ) || (L_Bein_Vertikal_IST[y] <= 0 && y==3 )) { UberLauf_rech = sin((90-(W_R_zu_FussSp-W_MaxAuslenk_Bein)-(90-((180-W_MaxAuslenk_Bein)/2)))*M_PI/180) * Uberlappenderlauf; //Uberlappenderlauf aus Const umrechnen in geneiget L-Bein-Vertikal Schrittweite_MAX = cos(((W_R_zu_FussSp)-(90-((180-W_MaxAuslenk_Bein)/2)))*M_PI/180)*((sqrt(square(Radius_Kurve+L_Bein_Horizontal+L_Bein_Horrizontal_Offset)+square(L_Bein_Vertikal))) * sin((W_MaxAuslenk_Bein)*M_PI/180)); } if ((L_Bein_Vertikal_IST[y] <= 0 && y==1 ) || (L_Bein_Vertikal_IST[y] > 0 && y==3 )) { UberLauf_rech = sin((90-(W_R_zu_FussSp+W_MaxAuslenk_Bein)+(90-((180-W_MaxAuslenk_Bein)/2)))*M_PI/180) * Uberlappenderlauf; //Uberlappenderlauf aus Const umrechnen in geneiget L-Bein-Vertikal Schrittweite_MAX = cos(((W_R_zu_FussSp)+(90-((180-W_MaxAuslenk_Bein)/2)))*M_PI/180)*((sqrt(square(Radius_Kurve+L_Bein_Horizontal+L_Bein_Horrizontal_Offset)+square(L_Bein_Vertikal))) * sin((W_MaxAuslenk_Bein)*M_PI/180)); } } if (y==2 || y==6) { W_R_zu_FussSp = atan(L_Bein_Vertikal/(Radius_Kurve-L_Bein_Horizontal-L_Bein_Horrizontal_Offset))*180/M_PI; //Winkel von "Robotermitte zu Kurvenmitte" zu Fußspitze @ 0° if ((L_Bein_Vertikal_IST[y] > 0 && y==2 ) || (L_Bein_Vertikal_IST[y] <= 0 && y==6 )) { UberLauf_rech = sin((90-(W_R_zu_FussSp-W_MaxAuslenk_Bein)-(90-((180-W_MaxAuslenk_Bein)/2)))*M_PI/180) * Uberlappenderlauf; //Uberlappenderlauf aus Const umrechnen in geneiget L-Bein-Vertikal Schrittweite_MAX = cos(((W_R_zu_FussSp)-(90-((180-W_MaxAuslenk_Bein)/2)))*M_PI/180)*((sqrt(square(Radius_Kurve-L_Bein_Horizontal-L_Bein_Horrizontal_Offset)+square(L_Bein_Vertikal))) * sin((W_MaxAuslenk_Bein)*M_PI/180)); } if ((L_Bein_Vertikal_IST[y] <= 0 && y==2 ) || (L_Bein_Vertikal_IST[y] > 0 && y==6 )) { UberLauf_rech = sin((90-(W_R_zu_FussSp+W_MaxAuslenk_Bein)+(90-((180-W_MaxAuslenk_Bein)/2)))*M_PI/180) * Uberlappenderlauf; //Uberlappenderlauf aus Const umrechnen in geneiget L-Bein-Vertikal Schrittweite_MAX = cos(((W_R_zu_FussSp)+(90-((180-W_MaxAuslenk_Bein)/2)))*M_PI/180)*((sqrt(square(Radius_Kurve-L_Bein_Horizontal-L_Bein_Horrizontal_Offset)+square(L_Bein_Vertikal))) * sin((W_MaxAuslenk_Bein)*M_PI/180)); } } if (y==4) { UberLauf_rech = sin(((180-W_MaxAuslenk_Bein)/2)*M_PI/180) * Uberlappenderlauf; //Uberlappenderlauf aus Const umrechnen in geneiget L-Bein-Vertikal Schrittweite_MAX = (Radius_Kurve+L_Bein_Horizontal_2+L_Bein_Horrizontal_Offset) * sin((W_MaxAuslenk_Bein)*M_PI/180); } if (y==5) { UberLauf_rech = sin(((180-W_MaxAuslenk_Bein)/2)*M_PI/180) * Uberlappenderlauf; //Uberlappenderlauf aus Const umrechnen in geneiget L-Bein-Vertikal Schrittweite_MAX = (Radius_Kurve-L_Bein_Horizontal_2-L_Bein_Horrizontal_Offset) * sin((W_MaxAuslenk_Bein)*M_PI/180); } } //Ende else Kurve Positiv } //Ende else Kurvenlauf } //Ende else Drehmodus = 1 else // wenn geradeauslauf { Schrittweite_MAX = Maxauslenkung_Bein; //wenn geradeauslauf dann Maximale Schrittweite aus Roboterhöhenabhäniger Funktion Hohe geandert UberLauf_rech = Uberlappenderlauf; //wenn geradeauslauf dann Überlappenderlauf auch gerade } //Ende else Geradeauslauf //ENDE--------------------------- Schrittweite_MAX ----------------------------------------- //-------------------------------- Schrittweite_Max beim Umschalten festlegen-------------- if (L_Bein_Vertikal_IST[y] < 0) //falls IST-Wert negativ ist, diesen in Positiven wert kovertieren ist nötig für Prüfung ob Istwert schon über Schrittweite_MAX liegt { L_Bein_Vertikal_IST_rech = L_Bein_Vertikal_IST[y] * -1; } else { L_Bein_Vertikal_IST_rech = L_Bein_Vertikal_IST[y]; } //Wenn beim Umschalten von Geradeauslauf zu Kurvenlauf die Maximale Beinauslenken bereits überschritten wurde. Bedingt durch zu kleinen Schrittweite_MAX. Dann erweiterten Grenzwert festlegen. if (Schrittweite_MAX < L_Bein_Vertikal_IST_rech) //prüfung ob der Aktuelle Istwert großer ist als Grenzwert des Beins. (HoheBein[y] == Hohe_Global) verhindert ständiges neuberechnen/hochzählen des Grenzwerts { if ((Schrittweite_MAX_Uber[y] > 0) && (Schrittweite_MAX_Uber[y] >= L_Bein_Vertikal_IST_rech)) //wenn bereits eine Schrittweite (durch überschreitung Istwert < Schrittweite_MAX) neu berechnet wurde. Und Prüfung ob Schrittweite_MAX_Uber[y] nochmals überschritten wurde { Schrittweite_MAX = Schrittweite_MAX_Uber[y]; } else { if (Vor_Ruck_Hub[y] == 1) { Schrittweite_MAX = L_Bein_Vertikal_IST_rech; //neuer Grenzwert = Istwert, da eh sofort auf Vorhub umgeschalten wird / Rückhub } else { Schrittweite_MAX = L_Bein_Vertikal_IST_rech + (UberLauf_rech/2); //neuer Grenzwert = Istwert + Überlappenderlauf / Vorhub } Schrittweite_MAX_Uber[y] = Schrittweite_MAX; //neuen Grenzwert speichern } } //ENDE---------------------------- Schrittweite_Max beim Umschalten festlegen--------------
Bitte mal Irgendwer irgendwas schreiben, es wird immer nur der letzte Post aktualisiert. Somit komm ich wieder ins Zeichenlimit.
aber gern!
- - - Aktualisiert - - -Code://------------------------------- Synchronisation ------------------------------------------ //Beim Umschalten von Kurvenlauf, Drehmodus, Geradeaus oder Laufmodus müssen die Beine wieder synchronisiert werden //------------------------->SOLL Position in % errechen, siehe Laufmodusfestlegung (-Maxauslenkung zu +Maxauslekung ergeben 0-100%), in Bezug auf Maxauslenkung im Kurvenlau/Geradeaus zu aktuellen IST-Wert //------------------------->IST Positiion in % errechnen //------------------------->Bei Rückhub über Synchro_Faktor die Geschwindigkeit manipulieren um wieder in "Takt" zu kommen (aufholen, warten oder Schrittweite einkürzen) //-------------------------> //Sollpoisition bestimmen //Hilfsvariable x wird die Spreizung (Winkelunterschied) der Beine beschrieben. //im Laufmodus 3-3 ist dieser immer genau 50% versetzt. if (Laufmodus == 33) //5 = 50 % (Spreizung 100%) { if (y==1 || y==3 || y==5) { Synchro_Spreizung = 100 - (100*Uberlappenderlauf/Maxauslenkung_Bein); //90,48 eventuell für geradeaus und kurve wieder /Schrittweite_MAX } else { Synchro_Spreizung = 0 ; //80,96 } } else { if (Laufmodus == 42) //5 = 54,76 % (Spreizung 45,24%) { if (y==2 || y==4) //9,52% { x = 2; } if (y==3 || y==5) //54,76 % { x = 1; } if (y==1) //100 % { x = 0; } } if (Laufmodus == 51) //5 = 63,80 % (Spreizung 18,1%) { if (y==1) { x = 1; //81,90 % } if (y==2) //27,61 % { x = 4; } if (y==3) //45,71 % { x = 3; } if (y==4) //9,52 % { x = 5; } if (y==5) //63,80 % { x = 2; } } Synchro_Spreizung = ((100/Ruckhub_Geschwindigkeit) - ((100*Uberlappenderlauf/Maxauslenkung_Bein)/Ruckhub_Geschwindigkeit)) * x; //TEST ohne Schrittweite MAX - errechnet die Spreizung des akutell zu berechnenden Beins } //%-Wert von Bein6 (Synchrobein) bestimmen if (y == 6) //auf Bein 6 wird Synchronisiert { if (Vor_Ruck_Hub[6]==1) //im Rückhub { Synchro_Bein = (((L_Bein_Vertikal_IST[6] + Schrittweite_MAX) * 100 ) / (2 * Schrittweite_MAX)) / Ruckhub_Faktor; //IST-Wert des SynchrobBeins, Rückhubfaktor bereinigt } else { Synchro_Bein = ((L_Bein_Vertikal_IST[6] + Schrittweite_MAX) * 100 ) / (2 * Schrittweite_MAX); //IST-Wert des SynchrobBeins } Synchro_Faktor = 1; //das Synchro-Bein darf sich nicht selbst verstellen } else //nur wenn nicht Bein == 6 dann Synchronisation durchführen { //den Aktuellen IST-Wert errechnen if (Vor_Ruck_Hub[y]==1) //im Rückhub { Synchro_Bein_IST = (((L_Bein_Vertikal_IST[y] + Schrittweite_MAX) * 100 ) / (2 * Schrittweite_MAX)) / Ruckhub_Faktor; //IST-Wert des zu Synchronisierenden Beins, Rückhubfaktor bereinigt } else { Synchro_Bein_IST = ((L_Bein_Vertikal_IST[y] + Schrittweite_MAX) * 100 ) / (2 * Schrittweite_MAX); //IST-Wert des zu Synchronisierenden Beins } //Beim Umschalten Geradeaus, Kurve, Drehmodus kann es sein das einmalig Synchro_Bein_IST auserhalb des gültigen Bereichs ist (0-100). if ((Synchro_Bein_IST < 0) || (Synchro_Bein_IST > 100)) { Synchro_Faktor = 1; goto negativen_Synchro_Bein_IST_ignorieren; //Gesamte Synchronisierung bei unglültigen Wert überspringen. } //Es gibt 4 Varianten(Stellungen) der Beine (Synchro Bein und zu Synchronisierendes Bein) //1. Synchro Bein ist im Wert (0-100%) Größer als zu Synchronisierendes Bein //2. zu Synchronisieredes Bein befindest sich im Rückhub //3. beide Beine sind sind wieder im Vorhub, aber zu Synchronisieredes Bein ist im Wert (0-100%) Größer als Synchrobein //4. Synchro Bein ist im Rückhub // Es gibt einige Ausnahmen, wenn das zu Synchronisieredes Bein den gleichen Wert (0-100%) hat wie das Synchrobein (4-2 Bein 1) und (3-3 Bein 2 & 4) // Es gibt eine Liste im Excel (Schrittfolge in%) in der die Werte in % beschrieben sind if ((Drehmodus == 1) && (y==1 || y==3 || y==4)) //1 { Synchro_Bein_SOLL = Synchro_Spreizung - Synchro_Bein; //Drehmodus 5-1 Bein 1 } else { Synchro_Bein_SOLL = Synchro_Bein - Synchro_Spreizung; //Beide Vorhub 1 (5-1 / 1) } if (Vor_Ruck_Hub[6] == 1) // Synchro Bein Rückhub 4 { if ((Laufmodus == 33) && (Synchro_Spreizung > 0)) //Ausnahme für 3-3 nicht für Bein 4 { if ((Drehmodus == 1) && (y==1 || y==3)) { Synchro_Bein_SOLL = Synchro_Spreizung - (100 / Ruckhub_Faktor) + Synchro_Bein; } else { Synchro_Bein_SOLL = Synchro_Spreizung - Synchro_Bein; } } else { if ((((Synchro_Spreizung == 0) && (Drehmodus == 0)) && (Laufmodus == 42)) || (Laufmodus == 33)) //Ausnahme da Bein 1 gleich Synchrobein @ Laufmodus 4-2 { if ((Drehmodus == 1) && (y==4) && (Laufmodus==33)) { Synchro_Bein_SOLL = (100/Ruckhub_Faktor) - Synchro_Bein; } else { Synchro_Bein_SOLL = Synchro_Bein; } } else { if ((Drehmodus == 1) && (y==1 || y==3 || y==4)) { Synchro_Bein_SOLL = Synchro_Spreizung - (100 / Ruckhub_Faktor) + Synchro_Bein; if (Synchro_Bein_SOLL < 0) //Negativen Wert bei Synchro_Spreizung = 0 verhindern { Synchro_Bein_SOLL *= -1; } } else { if (Synchro_Bein > (100/Ruckhub_Faktor)) { Synchro_Bein_SOLL = Synchro_Bein - Synchro_Spreizung; //wurde eingefügt weil Synchnchro_Bein_SOLL negativ wurde wenn Synchrobein > (100/Ruckhub_Faktor) } else { Synchro_Bein_SOLL = 100 - Synchro_Spreizung - Synchro_Bein + (100 / Ruckhub_Faktor); } } } } } else //2 & 3 { if ((Drehmodus == 1) && (y==1 || y==3 || y==4) && (Synchro_Bein_SOLL < 0)) { if (Synchro_Bein > Synchro_Spreizung) //wenn Synchrobein kleiner 78,67 3 > 21,51 (Synchro_Bein > (Synchro_Spreizung + (100 / Ruckhub_Faktor[y]))) { Synchro_Bein_SOLL = (100 + Synchro_Spreizung) - Synchro_Bein; //geändert 17.10.15 } else { Synchro_Bein_SOLL = Synchro_Bein - Synchro_Spreizung; //Drehmodus 5-1 Bein 1 IST-Bein im Rückhub 2 } } else //Variante 2 zu Synchronisieredes Bein befindest sich im Rückhub { if ((Drehmodus == 1) && (y==1 || y==3 || y==4)) { Synchro_Bein_SOLL = ((Synchro_Spreizung - (100 / Ruckhub_Faktor)) * -1) + Synchro_Bein; if (Synchro_Bein_SOLL < 0) //Wenn Rückhub IST-Bein abgeschlossen, dann würde Wert Negativ werden { Synchro_Bein_SOLL *= -1; } } else { if (Synchro_Bein_SOLL < 0) // IST-Bein im Rückhub 2 { Synchro_Bein_SOLL *= -1; if (Synchro_Bein_SOLL > (100/Ruckhub_Faktor)) //Maximaler Rückhubweg überschritten ((4-2 = 35,48) (5-1 = 70,84)) Beide Beine im Vorhub 3 { Synchro_Bein_SOLL = 100 - ((((Synchro_Spreizung*Ruckhub_Faktor)-100)/Ruckhub_Faktor)-Synchro_Bein); //100 - 14,66 - Synchro_Bein (5-1 / 1) / (Synchro_Spreizung - (100/Rückhubfaktor)) } } } } } //"Synchro_Bein_SOLL_Rueckhub" gibt an wann "Synchro_Bein_SOLL" ein Rückhubwert ist (==1) if ( ((Laufmodus==42 && y==1) || (Laufmodus==33 && (y==2 || y==4))) && (Vor_Ruck_Hub[6] == 1) ) //im Laufmodus 4-2 ist Bein 1 Synchron zum Synchrobein(6) / auch für Laufmodus 3-3 Bein 2 & 4 { Synchro_Bein_SOLL_Rueckhub = 1; //"Synchro_Bein_SOLL" ein Rückhubwert } else { if ( (Synchro_Bein <= Synchro_Spreizung) && (Synchro_Bein >= (Synchro_Spreizung - (100 / Ruckhub_Faktor))) && (Vor_Ruck_Hub[6] == 0) ) //Laufmodus 3-3, 4-2, 5-1 { Synchro_Bein_SOLL_Rueckhub = 1; //"Synchro_Bein_SOLL" ein Rückhubwert } else { Synchro_Bein_SOLL_Rueckhub = 0; //"Synchro_Bein_SOLL" ein Vorhubwert } } //berechnet die Bewegungsgeschwindigkeit um wieder zur gewünschten Bein-Spreizung zu gelangen (verarbeitung in "Bein_bewegung_mm") //wenn schon über 50% gefahren dann Schrittweite einkürzen, weil in den ersten 50% Fahrweg der Horrizontale Beinabstand normalisiert wird if (Vor_Ruck_Hub[y] == 1) //nur wenn Bein im Rückhub, da im Vorhub nicht Synchronisiert wird. { HiMe_SW_MAX = Schrittweite_MAX; //Schrittweite_MAX merken um sich nach der Synchronisation auf Fehler überprüfen zu können if (Laufmodus == 33) //Maximaler %-Wert den denn der Sollwert hinter den Istwert liegt darf, beim Synchronisieren { // Da es vorkommt das mehr als 3 Beine gleichzeigt in der Luft sind, muß das unterbunden werden (sonst kippt der Roboter). Möglicherweiße muß noch Programm zur Neuaufstellung (Grundstellung) geschrieben werden // auch noch für 4-2 und 5-1 einfügen, ist aber nicht ganz so Kritisch / Möglicherweiße reicht /2 schon aus if ((Vor_Ruck_Hub[1]+Vor_Ruck_Hub[2]+Vor_Ruck_Hub[3]+Vor_Ruck_Hub[4]+Vor_Ruck_Hub[5]+Vor_Ruck_Hub[6]) <= 3) { Spreizung_MAX_warten = (100 - (100*Uberlappenderlauf/Maxauslenkung_Bein)) - (100/Ruckhub_Faktor) / 2; } else { Spreizung_MAX_warten = 0; } } else { Spreizung_MAX_warten = (((100/Ruckhub_Geschwindigkeit) - ((100*Uberlappenderlauf/Maxauslenkung_Bein)/Ruckhub_Geschwindigkeit)) - (100/Ruckhub_Faktor)) / 2; } //Für Schrittweitenverkürzung Formel aussuchen, welche berücksichtigt ob über oder unter 50% gefahren wurde [je nach bewegungsrichtung des Beins (von 100% zu 0% oder 0% zu 100%)] SW_MAX_Verk = 1; //Hilfvariable für Synchro_Faktor (Schrittweiten-Verkürzung) berechung, vor jeder berechung auf 1 setzen if ((Drehmodus==1) && (((Radius_Kurve > 0) && (y==2 || y==5 || y==6)) || ((Radius_Kurve < 0) && (y==1 || y==3 || y==4)))) // "EXCEL Schrittfolge in %" { SW_MAX_Verk = 0; } if (Roboter_bewegung_mm < 0) //bei Rückwärtsbewegung invertieren { if (SW_MAX_Verk == 1) //invertierung { SW_MAX_Verk = 0; } else { SW_MAX_Verk = 1; } } //Auswahl und Berechung ob Schrittweite verkürzt wird, Bein wartet oder aufholt if (SW_MAX_Verk == 0) //Bein läuft "Rückwärts"(100% -> 0%), es müßen 50% UNTERschritten werden um SW_MAX einzukürzen { if ( ((Synchro_Bein_IST * Ruckhub_Faktor) < 50) && Synchro_Bein_SOLL_Rueckhub == 0 ) //Schrittweite_MAX einkürzen, wenn Sollwert schon wieder im Vorhub ist { if (Synchro_Bein_SOLL > 50) //wenn Sollwert über 50% dann Rückhub sofort abbrechen um maximal aufholen zu können. { Schrittweite_MAX = L_Bein_Vertikal_IST_rech; //sofort auf Vorhub umschalten. Synchro_Faktor = 1; } else { Schrittweite_MAX *= ((100 - (2 * Synchro_Bein_SOLL)) / 100); //Schrittweite_Max auf aktuellen Vorhubwert begrenzen Synchro_Faktor = 1; } } else { if ( (((Synchro_Bein_SOLL > Synchro_Bein_IST) && ((Synchro_Bein_SOLL - Synchro_Bein_IST) <= Spreizung_MAX_warten) ) || (Synchro_Bein_SOLL < Synchro_Bein_IST)) && (Synchro_Bein_SOLL_Rueckhub == 1) ) // (warten: wenn SOLL > IST && Abstand nur 10%) || (Einholen wenn SOLL < IST) / "((Synchro_Bein_SOLL - Synchro_Bein_IST) <= (Synchro_Spreizung - (100 / Ruckhub_Faktor[y]) / Ruckhub_Faktor[y]))" beschreibt die Maximale % Zahl die gewartet werden kann (Ende Rückhub zu Synchronisierendes Bein bis Anfang Rückhub nächstes Bein) { Synchro_Faktor = (Synchro_Bein_IST / Synchro_Bein_SOLL); } else { if ((Synchro_Bein_SOLL > Synchro_Bein_IST) && (Synchro_Bein_SOLL_Rueckhub == 1))//nur falls warten nicht möglich (z.B. Abstand größer 10%) dann aufholen { Synchro_Faktor = (Synchro_Bein_SOLL / Synchro_Bein_IST); } } } } else //Bein läuft "Vorwärts"(0% -> 100%), es müßen 50% ÜBERschritten werden um SW_MAX einzukürzen { if ( ((Synchro_Bein_IST * Ruckhub_Faktor) > 50) && Synchro_Bein_SOLL_Rueckhub == 0 ) //Schrittweite_MAX einkürzen, wenn Sollwert schon wieder im Vorhub ist { if (Synchro_Bein_SOLL <= 50) //wenn Sollwert unter 50% dann Rückhub sofort abbrechen um maximal aufholen zu können. { Schrittweite_MAX = L_Bein_Vertikal_IST_rech; //sofort auf Vorhub umschalten. Synchro_Faktor = 1; } else { Schrittweite_MAX *= ((2 * Synchro_Bein_SOLL) - 100) / 100; //Schrittweite_Max auf aktuellen Vorhubwert begrenzen Synchro_Faktor = 1; } } else { if ( (((Synchro_Bein_SOLL < Synchro_Bein_IST) && ((Synchro_Bein_IST - Synchro_Bein_SOLL) <= Spreizung_MAX_warten) ) || (Synchro_Bein_SOLL > Synchro_Bein_IST)) && (Synchro_Bein_SOLL_Rueckhub == 1) ) // (warten: wenn SOLL < IST && Abstand nur 10%) || (Einholen wenn SOLL > IST) / (((Synchro_Spreizung / x) - (100 / Ruckhub_Faktor[y])) / Ruckhub_Faktor[y]) { Synchro_Faktor = (Synchro_Bein_SOLL / Synchro_Bein_IST); } else { if ((Synchro_Bein_SOLL < Synchro_Bein_IST) && (Synchro_Bein_SOLL_Rueckhub == 1))//nur falls warten nicht möglich (z.B. Abstand größer 10%) dann aufholen { Synchro_Faktor = (Synchro_Bein_IST / Synchro_Bein_SOLL); } } } } //Fehler überprüfungen if (Schrittweite_MAX < 0) //dürfte nie berechnet werden, aber falls doch einen negativen Wert verhindern { Schrittweite_MAX = 0; } if (Schrittweite_MAX > HiMe_SW_MAX) //dürfte nie berechnet werden, aber falls doch einen zu großen Wert verhindern { Schrittweite_MAX = HiMe_SW_MAX; } } else { Synchro_Faktor = 1; } negativen_Synchro_Bein_IST_ignorieren: //Einsprung-Punkt falls Synchro_Bein_IST beim Umschalten von Geradeaus auf Kurve/Drehmodus kleiner 0% oder Größer 100% ist. //Fehler überprüfungen if (Synchro_Faktor > 1.5) //Bein darf Maximal mit der 1,5fachen Geschwindigkeit fahren (synchronisiert werden), sonst kommt es zum Fehler (Bewegung des Beins pro Zyklus über 135mm) { Synchro_Faktor = 1.5; } if (Synchro_Faktor <= 0) //negativewerte und NULL verhindern { Synchro_Faktor = 1; } } //ENDE (y==6) //ENDE--------------------------- Synchronisation ------------------------------------------ //Bewegung des Beins/Roboters in mm festlegen if (Vor_Ruck_Hub[y] == 1) //Rückhub { Bein_bewegung_mm = (((((float)Roboter_bewegung_mm * Zykl_Zeit_Multiplik) / Geschwindigkeit ) / Ruckhub_Faktor) * Ruckhub_Faktor * Synchro_Faktor); //festlegung der Geschwindigkeit * Überlappender-Lauf aus Synchro_Winkel (Faktor für Rückhub) (((float)Roboter_bewegung_mm / Geschwindigkeit) * Ruckhub_Faktor * Synchro_Faktor) } else //Vorhub { Bein_bewegung_mm = (((((float)Roboter_bewegung_mm * Zykl_Zeit_Multiplik) / Geschwindigkeit ) / Ruckhub_Faktor) * -1); //festlegung der Geschwindigkeit (((float)Roboter_bewegung_mm / Geschwindigkeit) * -1) } if ((Radius_Kurve > 0) && (Drehmodus == 1)) //um in richtige Richtung bei Drehmodus zu drehen (je nach Joystick eingabe) { Bein_bewegung_mm *= -1; } //ENDE Bewegung des Beins in mm if (Radius_Kurve == 0) //wenn Geradeauslauf { L_Bein_Vertikal_anderung = Bein_bewegung_mm; //Vorwärts/Rückwärts-Bewegung addieren //Werte werden sonst bei Geradeauslauf undefiniert sein Sehnenlange = 0; L_R_zu_Fuss_SOLL = 0; Winkel_R_zu_Fuss_SOLL = 0; Zentrierwinkel = 0; Winkel_R_zu_Fuss_IST = 0; R_Fussspitze = 0; L_Bein_Horizontal_rech = 0; L_Bein_Vertikal_rech = 0; //L_Bein_Horrizontal_SOLL wieder auf L_Bein_Horrizontal_Offset heranführen. Damit nach Kurvenlauf die Beine wieder den gleichen/normalen Abstand zum Körper haben if ( (Vor_Ruck_Hub[y] == 1) && (L_Bein_Vertikal_IST_rech != 0) && (L_Bein_Horrizontal_IST[y] != L_Bein_Horrizontal_Offset) ) //nur Berechnen wenn Bein im Rückhub, L_Bein_Vertikal_IST_rech nicht 0 (sonst Division /0) { if (Bein_bewegung_mm < 0) //bei negativer Bewegung (Rückwärts) Bein_bewegung_mm negieren { L_Bein_Horrizontal_SOLL = L_Bein_Horrizontal_IST[y] + ((L_Bein_Horrizontal_Offset - L_Bein_Horrizontal_IST[y]) * ((Bein_bewegung_mm * -1)/ L_Bein_Vertikal_IST_rech)); } else { L_Bein_Horrizontal_SOLL = L_Bein_Horrizontal_IST[y] + ((L_Bein_Horrizontal_Offset - L_Bein_Horrizontal_IST[y]) * (Bein_bewegung_mm / L_Bein_Vertikal_IST_rech)); } } else { L_Bein_Horrizontal_SOLL = L_Bein_Horrizontal_IST[y]; //im Vorhub, oder wenn L_Bein_Vertikal_IST_rech gleich 0, oder wenn L_Bein_Horrizontal_IST[y] = L_Bein_Horrizontal_Offset dann Horrizontalen Beinabstand aus IST-Wert mit nehmen. } } else //wenn Kurvenlauf { //IST-Position berechnen----------------------------------------------------------------------------------------------------------------------------------------- //Es wird die IST-Position des Beins berechnet, Grundlage sind "L_Bein_Vertikal_IST" (abstand Fußspitze IST-Drehwinkel zu 0°) und "L_Bein_Horrizontal_IST" (Abstand Fußspitze zu Servo_HU [Horrizontal]) //Um Dreicke von Kurvenradius-Mittelpunkt zu Fußspitze zu berechnen, werden die Katehte und Ankatehte je Negativ oder Positiv gerechnet. //Dies ist wichtig um die Servo-Drehrichtung zu beachten. //Die Mittleren Beine (4 & 5) stehen Horrizontal weiter ab als die anderen Beine if (y == 1) //Bein 1 hinten { L_Bein_Vertikal_rech = L_Bein_Vertikal * -1; //123mm negativ rechnen L_Bein_Horizontal_rech = L_Bein_Horizontal; //70mm positiv rechnen } if (y == 2) //Bein 2 hinten { if (Drehmodus == 1) { L_Bein_Vertikal_rech = L_Bein_Vertikal * -1; //123mm negativ rechnen } else { L_Bein_Vertikal_rech = L_Bein_Vertikal; //123mm positiv rechnen } L_Bein_Horizontal_rech = L_Bein_Horizontal * -1; //70mm negativ rechnen } if (y == 3) //Bein 3 vorn { L_Bein_Vertikal_rech = L_Bein_Vertikal; //123 positiv rechnen L_Bein_Horizontal_rech = L_Bein_Horizontal; //70mm positiv rechnen } if (y == 4) //Bein 4 Mitte { L_Bein_Vertikal_rech = 0; //0 rechnen L_Bein_Horizontal_rech = L_Bein_Horizontal_2; //95mm positiv rechnen } if (y == 5) //Bein 5 Mitte { L_Bein_Vertikal_rech = 0; //0 rechnen L_Bein_Horizontal_rech = L_Bein_Horizontal_2 * -1; //95mm negativ rechnen } if (y == 6) //Bein 6 vorn { if (Drehmodus == 1) { L_Bein_Vertikal_rech = L_Bein_Vertikal; //123mm positiv rechnen } else { L_Bein_Vertikal_rech = L_Bein_Vertikal * -1; //123mm negativ rechnen } L_Bein_Horizontal_rech = L_Bein_Horizontal * -1; //70mm negativ rechnen } //Realen Radius von Fußspitze zu Kurvenmittelpunkt berechnen if (Drehmodus == 1) { if (y==4 || y==5) //r=185,05 @ Bein 0° { R_Fussspitze = sqrt( square(L_Bein_Horizontal_2 + L_Bein_Horrizontal_IST[y]) + square(L_Bein_Vertikal_IST[y])); // R_Fußspitze in IST-Position zur Robotermitte (Mittlere Beine) } else //r=201,85 @ Bein 0° { R_Fussspitze = sqrt( square(L_Bein_Horizontal + L_Bein_Horrizontal_IST[y]) + square(L_Bein_Vertikal_rech + L_Bein_Vertikal_IST[y])); // R_Fußspitze in IST-Position zur Robotermitte (Äußere Beine) } } else //wenn Kurvenlauf { if (y==2 || y==5 || y==6) //Wenn BEIN LINKS { R_Fussspitze = sqrt( square(Radius_Kurve + L_Bein_Horizontal_rech - L_Bein_Horrizontal_IST[y]) + square(L_Bein_Vertikal_rech - L_Bein_Vertikal_IST[y])); } else { R_Fussspitze = sqrt( square(Radius_Kurve + L_Bein_Horizontal_rech + L_Bein_Horrizontal_IST[y]) + square(L_Bein_Vertikal_rech + L_Bein_Vertikal_IST[y])); //Abstannd Fußspitze zu Roboter Addieren "+ L_Bein_Horrizontal_IST[y]" } } //Winkel von der Geraden (Radiusmittelpunkt zu Robotermitte) zur Geraden (Radiusmittelpunkt zu Fußspitze) berechnen if (Drehmodus == 1) { if (y==4 || y==5) { Winkel_R_zu_Fuss_IST = acos((L_Bein_Horizontal_2 + L_Bein_Horrizontal_IST[y]) / R_Fussspitze)*180/M_PI; //Winkel von Horrizontalen durch den Roboter zur Fußspitze IST-Position (Mittlere Beine) } else { Winkel_R_zu_Fuss_IST = acos((L_Bein_Horizontal + L_Bein_Horrizontal_IST[y]) / R_Fussspitze)*180/M_PI; //Winkel von Horrizontalen durch den Roboter zur Fußspitze IST-Position (Äußere Beine) } } else //wenn Kurvenlauf { if (y==2 || y==5 || y==6) //für Linke Seite { if (Radius_Kurve > 0) { Winkel_R_zu_Fuss_IST = acos((Radius_Kurve + L_Bein_Horizontal_rech - L_Bein_Horrizontal_IST[y]) / R_Fussspitze)*180/M_PI; } else { Winkel_R_zu_Fuss_IST = acos(((Radius_Kurve*-1) - L_Bein_Horizontal_rech + L_Bein_Horrizontal_IST[y]) / R_Fussspitze)*180/M_PI; } } else { if (Radius_Kurve > 0) { Winkel_R_zu_Fuss_IST = acos((Radius_Kurve + L_Bein_Horizontal_rech + L_Bein_Horrizontal_IST[y]) / R_Fussspitze)*180/M_PI; } else { Winkel_R_zu_Fuss_IST = acos(((Radius_Kurve*-1) - L_Bein_Horizontal_rech - L_Bein_Horrizontal_IST[y]) / R_Fussspitze)*180/M_PI; } } } //ENDE IST-Position berechnen------------------------------------------------------------------------------------------------------------------------------------ //aus Vorwärtsbewegung in mm wird die Gerade (Radius-Mittelpunkt zu Fußspitze) um soviel Grad verschoben if (Bein_bewegung_mm != 0) //diffision durch 0 verhindern { if (Drehmodus == 1) { Zentrierwinkel = atan(Bein_bewegung_mm/(L_Bein_Horrizontal_Offset + L_Bein_Horizontal_2)) *180/M_PI; //Vorwärtsbewegung mit Mittlerenbei festlegen if ((Drehmodus == 1) && (y==1 || y==3 || y==4)) //um Radius-wöllbung zum Roboter hin geneigt zu behalten, bei Drehrichtungsumkehr für Drehmodus { Zentrierwinkel *= -1; //negieren } } else //wenn Kurvenlauf { if (Radius_Kurve < 0) { Zentrierwinkel = atan(Bein_bewegung_mm/(Radius_Kurve*-1)) *180/M_PI; //mit positiven Radius_Kurve rechnen } else { Zentrierwinkel = atan(Bein_bewegung_mm/Radius_Kurve) *180/M_PI; } } } else { Zentrierwinkel = 0; //diffision durch 0 verhindern (Zentriewinkel Berechnung) } //Winkel von der Geraden (Radiusmittelpunkt zu Robotermitte) zur Geraden (Radiusmittelpunkt zu Fußspitze) SOLL-Position berechnen (nach der Bewegung in mm) if (y==1 || y==2 || (y==4 && L_Bein_Vertikal_IST[4] <= 0) || (y==5 && L_Bein_Vertikal_IST[5] <= 0) ) //wenn BEIN 1 oder 2 dann muß der Zentrierwinkel subtrahiert werden, legt die bewegungsrichtung fest / (Bein 4 oder 5 mit Negativen L_Bein_Vertikal) verhindert das R_Fußspitze immer Größer wird, bei negativen L_Bein_Vertikal (Winkel_R_zu_Fuss_SOLL darf an Bein 4&5 nicht negativ werden) { Zentrierwinkel *= -1; } Winkel_R_zu_Fuss_SOLL = Winkel_R_zu_Fuss_IST + Zentrierwinkel; //Horrizontaler Abstand von Drehpunkt Radius zu Fußspitze (SOLL position) L_R_zu_Fuss_SOLL = cos(Winkel_R_zu_Fuss_SOLL * M_PI/180) * R_Fussspitze; //Horrizontaler Abstand von "Servo_HU" zu Fußspitze (SOLL-Position) if (Drehmodus == 1) { if (y==4 || y==5) { L_Bein_Horrizontal_SOLL = L_R_zu_Fuss_SOLL - L_Bein_Horizontal_2; } else { L_Bein_Horrizontal_SOLL = L_R_zu_Fuss_SOLL - L_Bein_Horizontal; } } else { if (y == 1 || y == 3 || y == 4) { if (Radius_Kurve < 0) { L_Bein_Horrizontal_SOLL = (Radius_Kurve + L_Bein_Horizontal_rech + L_R_zu_Fuss_SOLL)*-1; } else { L_Bein_Horrizontal_SOLL = L_R_zu_Fuss_SOLL - Radius_Kurve - L_Bein_Horizontal_rech; } } else { if (Radius_Kurve < 0) { L_Bein_Horrizontal_SOLL = L_R_zu_Fuss_SOLL + Radius_Kurve + L_Bein_Horizontal_rech; } else { L_Bein_Horrizontal_SOLL = Radius_Kurve + L_Bein_Horizontal_rech - L_R_zu_Fuss_SOLL; } } } //eine Gerade die im um "Winkel_R_zu_Fuss_SOLL" geneigt ist, und sich aus der Vorwärtsbewegung in mm ergibt. Entspricht der zwei geraden "R_Fußspitze" mit den "Winkel_R_zu_Fuss_SOLL" if (Bein_bewegung_mm != 0) { if (y == 1 || y == 2) { Sehnenlange = R_Fussspitze * sin(Zentrierwinkel * M_PI/180); } else { Sehnenlange = R_Fussspitze * sin((Zentrierwinkel*-1) * M_PI/180); } } else { Sehnenlange = 0; } //Berechnet den "Weg in mm" den sich das Bein Vertikal bewegen soll if (Bein_bewegung_mm != 0) //Wenn Bewegung BEIN/Roboter größer 0 ist { L_Bein_Vertikal_anderung = sqrt(square(Sehnenlange) - square(L_Bein_Horrizontal_SOLL - L_Bein_Horrizontal_IST[y])); // if (Bein_bewegung_mm < 0) //wenn Rückwärts Bewegung { L_Bein_Vertikal_anderung *= -1; //negieren } if ((Drehmodus == 1) && (y==1 || y==3 || y==4)) //wenn Drehmodus um Beine entgegengesetzt laufen zu lassen { L_Bein_Vertikal_anderung *= -1; //negieren } } else { L_Bein_Vertikal_anderung = 0; //Wenn "Bein_bewegung_mm = 0" dann auf 0 setzen, da sonst Rechenfehler in L_BEIN_SOLL } }//ENDE Kurvenlauf
bitte noch einmal etwas nutzlosen posten
aber na klar
- - - Aktualisiert - - -Code://Rückhub Unterdrückung durch Maximale Anzahl, oder Einseitigkeit, der Beine im Rückhub //sind mehr als (3-3 & 4-2 = 3 / 5-1 = 2) Beine im Rückhub, oder ist Rückhub auf 2 benachbarten Beinen (Längseitig) dann keinen Rückhub zulassen //es werden auch Rückhub-Kombinationen beide vorderen/hinteren Beine unterdrückt if ( ((Vor_Ruck_Hub[1]+Vor_Ruck_Hub[2]+Vor_Ruck_Hub[3]+Vor_Ruck_Hub[4]+Vor_Ruck_Hub[5]+Vor_Ruck_Hub[6]) >= Max_Anz_Beine_Ruckhub) || ( ((y==1) && (Vor_Ruck_Hub[4]==1 || Vor_Ruck_Hub[2]==1)) || ((y==4) && (Vor_Ruck_Hub[1]==1 || Vor_Ruck_Hub[3]==1)) || ((y==3) && (Vor_Ruck_Hub[4]==1 || Vor_Ruck_Hub[6]==1)) || ((y==2) && (Vor_Ruck_Hub[5]==1 || Vor_Ruck_Hub[1]==1)) || ((y==5) && (Vor_Ruck_Hub[2]==1 || Vor_Ruck_Hub[6]==1)) || ((y==6) && (Vor_Ruck_Hub[5]==1 || Vor_Ruck_Hub[3]==1)) ) ) { Ruckhub_Unterdruckung = 1; //keinen Rückhub zulassen } else { Ruckhub_Unterdruckung = 0; //Rückhub zulassen } //ENDE Rückhub Unterdrückung //----Vor_Ruck_Hub Umschalten //über "Ziel_Drehwinkel_Bein" festgelegte Gradzahl wird Bein-Maximal-Auslenkung in mm berechnet (Schrittweite_MAX) //aus Aktuellen Ist-Auslenkung in mm (LA_SOLL_WEG_D) wird der maximal fahrbare Weg berechnet, und bei Überschreiten von "Schrittweite_MAX" wird Vor_Ruck_Hub[y] umgeschalten //dabei wird ein Vorwärts oder Rückwärts fahren beachtet //unter der beachtung von "Ruckhub_Faktor" wird ein Positionsverlust, durch überschreiten des "Ziel_Drehwinkel_Bein" vermieden //daher muß "Ruckhub_Faktor" alias "Winkel_Bein[0]" zuerst berechnet werden. //da sich "Ziel_Drehwinkel_Bein"/"Schrittweite_MAX" durch die "Hohe_Global" ändert, ist bei änderung der Roboterhöhe eine Neuberechnung notwendig "HiMe_Hohe_geandert = Hohe_Global" HiMe_Schrittweite_Max = 0; //Muß vor dem Setzen rückgesetzt werden if (WINKEL_Bein[y] < 0) //wenn Bein-Winkel Negativ { if (Vor_Ruck_Hub[y] == 0) //wenn Vorhub EIN { if (((Schrittweite_MAX + L_Bein_Vertikal_IST[y]) < (L_Bein_Vertikal_anderung*-1)) && Ruckhub_Unterdruckung == 0) //Zielwert überschritten (bei Vorwärtsbewegung) / Nur in Rückhub gehen wenn es möglich ist { L_Bein_Vertikal_SOLL = ((Schrittweite_MAX*-1) - ((L_Bein_Vertikal_anderung + (Schrittweite_MAX + L_Bein_Vertikal_IST[y])) * Ruckhub_Faktor)); //Überschrittenen Wert (größer Schrittweite_MAX wieder abziehen) Vor_Ruck_Hub[y] = 1; //und Vor_Ruck_Hub umschalten Schrittweite_MAX_Uber[y] = 0; //Gespeicherten erweiterten Grenzwert (durch überschreitung Istwert > Schrittweite_MAX) löschen HiMe_Schrittweite_Max = 1; //Schrittweite_Max wurde überschritten und L_Bein_Horrizontal_SOLL muß neu berechnet werden if (y==6) //%-Wert von Synchrobein muß neu berechnet werden. %-Wert ist noch eine Rückhub Angabe. Somit würde ein Fehler auftreten, in Synchronisation der Beine (1-5) { Synchro_Bein = (((L_Bein_Vertikal_IST[6] + Schrittweite_MAX) * 100 ) / (2 * Schrittweite_MAX)) / Ruckhub_Faktor; //IST-Wert des SynchrobBeins } } else { L_Bein_Vertikal_SOLL = L_Bein_Vertikal_IST[y] + L_Bein_Vertikal_anderung; //normale Bein-Bewegung bei nicht überschritten } } else //wenn Rückhub EIN { if ((Schrittweite_MAX + L_Bein_Vertikal_IST[y]) < (L_Bein_Vertikal_anderung*-1)) //Zielwert überschritten (bei Vorwärtsbewegung) { L_Bein_Vertikal_SOLL = (Schrittweite_MAX - (((L_Bein_Vertikal_anderung*-1) - (Schrittweite_MAX + L_Bein_Vertikal_IST[y])) / Ruckhub_Faktor / Synchro_Faktor))*-1; //Überschrittenen Wert (größer Schrittweite_MAX wieder abziehen) Vor_Ruck_Hub[y] = 0; //und Vor_Ruck_Hub umschalten Schrittweite_MAX_Uber[y] = 0; //Gespeicherten erweiterten Grenzwert (durch überschreitung Istwert > Schrittweite_MAX) löschen HiMe_Schrittweite_Max = 1; //Schrittweite_Max wurde überschritten und L_Bein_Horrizontal_SOLL muß neu berechnet werden if (y==6) //%-Wert von Synchrobein muß neu berechnet werden. %-Wert ist noch eine Rückhub Angabe. Somit würde ein Fehler auftreten, in Synchronisation der Beine (1-5) { Synchro_Bein = ((L_Bein_Vertikal_IST[6] + Schrittweite_MAX) * 100 ) / (2 * Schrittweite_MAX); //IST-Wert des SynchrobBeins } } else { L_Bein_Vertikal_SOLL = L_Bein_Vertikal_IST[y] + L_Bein_Vertikal_anderung; //normale Bein-Bewegung bei nicht überschritten } } } else //wenn Bein-Winkel Positiv { if (Vor_Ruck_Hub[y] == 0) //wenn Vorhub EIN { if (((Schrittweite_MAX - L_Bein_Vertikal_IST[y]) < L_Bein_Vertikal_anderung) && Ruckhub_Unterdruckung == 0) //Zielwert überschritten (bei Rückwärtsbewegung) / Nur in Rückhub gehen wenn es möglich ist { L_Bein_Vertikal_SOLL = (Schrittweite_MAX - ((L_Bein_Vertikal_anderung - (Schrittweite_MAX - L_Bein_Vertikal_IST[y])) * Ruckhub_Faktor)); //Überschrittenen Wert (größer Schrittweite_MAX wieder abziehen) Vor_Ruck_Hub[y] = 1; //und Vor_Ruck_Hub umschalten Schrittweite_MAX_Uber[y] = 0; //Gespeicherten erweiterten Grenzwert (durch überschreitung Istwert > Schrittweite_MAX) löschen HiMe_Schrittweite_Max = 1; //Schrittweite_Max wurde überschritten und L_Bein_Horrizontal_SOLL muß neu berechnet werden if (y==6) //%-Wert von Synchrobein muß neu berechnet werden. %-Wert ist noch eine Rückhub Angabe. Somit würde ein Fehler auftreten, in Synchronisation der Beine (1-5) { Synchro_Bein = (((L_Bein_Vertikal_IST[6] + Schrittweite_MAX) * 100 ) / (2 * Schrittweite_MAX)) / Ruckhub_Faktor; //IST-Wert des SynchrobBeins } } else { L_Bein_Vertikal_SOLL = L_Bein_Vertikal_IST[y] + L_Bein_Vertikal_anderung; //normale Bein-Bewegung bei nicht überschritten } } else //wenn Rückhub EIN { if ((Schrittweite_MAX - L_Bein_Vertikal_IST[y]) < L_Bein_Vertikal_anderung) //Zielwert überschritten (bei Rückwärtsbewegung) { L_Bein_Vertikal_SOLL = (Schrittweite_MAX - ((L_Bein_Vertikal_anderung - (Schrittweite_MAX - L_Bein_Vertikal_IST[y])) / Ruckhub_Faktor / Synchro_Faktor)); //Überschrittenen Wert (größer Schrittweite_MAX wieder abziehen) Vor_Ruck_Hub[y] = 0; //und Vor_Ruck_Hub umschalten Schrittweite_MAX_Uber[y] = 0; //Gespeicherten erweiterten Grenzwert (durch überschreitung Istwert > Schrittweite_MAX) löschen HiMe_Schrittweite_Max = 1; //Schrittweite_Max wurde überschritten und L_Bein_Horrizontal_SOLL muß neu berechnet werden if (y==6) //%-Wert von Synchrobein muß neu berechnet werden. %-Wert ist noch eine Rückhub Angabe. Somit würde ein Fehler auftreten, in Synchronisation der Beine (1-5) { Synchro_Bein = ((L_Bein_Vertikal_IST[6] + Schrittweite_MAX) * 100 ) / (2 * Schrittweite_MAX); //IST-Wert des SynchrobBeins } } else { L_Bein_Vertikal_SOLL = L_Bein_Vertikal_IST[y] + L_Bein_Vertikal_anderung; //normale Bein-Bewegung bei nicht überschritten } } } //Da bei einer Überschreitung von Schrittweite_MAX L_Bein_Vertikal_SOLL neu berechnet wird, ist auch eine Neuberechung von L_Bein_Horrizontal_SOLL notwendig //Oder es wurde in den Kurvenlauf/Drehmodus umgeschalten und das Bein befindet sich nicht auf dem Idealen Radius (ergibt sich bei Bein 0°), sonst funktioniert die Synchronisation der Beine nicht, da Schrittweite_Max sich aus den idealen Radien ergibt. //Heranführen von L_Bein_Horrizontal an Offsetwert im geradeauslauf geschiet weiter oben im Programm (ca Z534) //im Vorhub ist aktueller R_Fussspitze zu beachten //im Rückhub wird auf idealen R_Fussspitze herangeführt if (Radius_Kurve != 0) //nur im Kurvenlauf zu berechnen { if ((HiMe_Schrittweite_Max == 1) && (Vor_Ruck_Hub[y] == 0)) //für Schrittweite_Max überschritten im Vorhub { HiMe_R_Fusspitze = R_Fussspitze; } else //für umschalten in den Drehmodus/Kurvenlauf um R_Fusspitze auf Idealwert (Bein @ 0°) heranzuführen { if (Drehmodus == 1) { if (y==4 || y==5) { R_Fussspitze_Ideal = L_Bein_Horizontal_2 + L_Bein_Horrizontal_Offset; // R_Fußspitze in IST-Position zur Robotermitte (Mittlere Beine) } else { R_Fussspitze_Ideal = sqrt( square(L_Bein_Horizontal + L_Bein_Horrizontal_Offset) + square(L_Bein_Vertikal_rech)); } } else //wenn Kurvenlauf { if (y==2 || y==5 || y==6) //Wenn BEIN LINKS { R_Fussspitze_Ideal = sqrt( square(Radius_Kurve + L_Bein_Horizontal_rech -L_Bein_Horrizontal_Offset) + square(L_Bein_Vertikal_rech)); } else { R_Fussspitze_Ideal = sqrt( square(Radius_Kurve + L_Bein_Horizontal_rech + L_Bein_Horrizontal_Offset) + square(L_Bein_Vertikal_rech)); } } } if ( ((HiMe_Schrittweite_Max == 1) && (Vor_Ruck_Hub[y] == 0)) || ((Vor_Ruck_Hub[y] == 1) && (R_Fussspitze != R_Fussspitze_Ideal)) ) { if ((R_Fussspitze != R_Fussspitze_Ideal) && (Vor_Ruck_Hub[y] == 1)) //R_Fussspitze_Ideal schrittweiße heranführen um Sprung in der Bewegung zu vermeiden. { HiMe_R_Fusspitze = R_Fussspitze + ((R_Fussspitze_Ideal - R_Fussspitze) * (1 - (L_Bein_Vertikal_IST_rech / Schrittweite_MAX))); } //L_Bein_Horrizontal_SOLL neu berechnen (Schrittweite_MAX wurde überschritten oder Bein befindet sich im Kurvenlauf/Drehmodus nicht auf idealen Radius) if (Drehmodus == 1) { if (y==4 || y==5) //r=185,05 @ Bein 0° { L_Bein_Horrizontal_SOLL = sqrt( square(HiMe_R_Fusspitze) - square(L_Bein_Vertikal_SOLL)) - L_Bein_Horizontal_2; } else //r=201,85 @ Bein 0° { L_Bein_Horrizontal_SOLL = sqrt( square(HiMe_R_Fusspitze) - square(L_Bein_Vertikal_rech + L_Bein_Vertikal_SOLL)) - L_Bein_Horizontal; } } else //wenn Kurvenlauf { if (y==2 || y==5 || y==6) //Wenn BEIN LINKS { if (Radius_Kurve < 0) { L_Bein_Horrizontal_SOLL = sqrt( square(HiMe_R_Fusspitze) - square(L_Bein_Vertikal_rech - L_Bein_Vertikal_SOLL)) + Radius_Kurve + L_Bein_Horizontal_rech; } else { L_Bein_Horrizontal_SOLL = Radius_Kurve - sqrt( square(HiMe_R_Fusspitze) - square(L_Bein_Vertikal_rech - L_Bein_Vertikal_SOLL)) + L_Bein_Horizontal_rech; } } else { if (Radius_Kurve < 0) { L_Bein_Horrizontal_SOLL = (Radius_Kurve * -1) - sqrt( square(HiMe_R_Fusspitze) - square(L_Bein_Vertikal_rech + L_Bein_Vertikal_SOLL)) - L_Bein_Horizontal_rech; } else { L_Bein_Horrizontal_SOLL = sqrt( square(HiMe_R_Fusspitze) - square(L_Bein_Vertikal_rech + L_Bein_Vertikal_SOLL)) - Radius_Kurve - L_Bein_Horizontal_rech; } } } HiMe_Schrittweite_Max = 0; //berechnung erfolgt } } //----ENDE Vor_Ruck_Hub //Solllänge Bein berechnen L_BEIN_SOLL = sqrt(square(L_Bein_Horrizontal_SOLL) + square(L_Bein_Vertikal_SOLL)); //Drehwinkel Bein berechnen WINKEL_Bein[y] = asin(L_Bein_Vertikal_SOLL / L_BEIN_SOLL) * 180/M_PI; //berechnet die Höhe des Beins, inklusive Offset für Überlappenden Lauf //die Höhe des Beins (-51mm bis 67mm Roboter Unterkante über Grund) wird für den Rückhub in einer Sinusfunktion berechnet. //Bei Rückhub wird um den Wert "Synchro_Winkel" über "Zielwert_Drehwinkel_Bein" hinausgefahren //Bei Vorhub wird um den Synchrowinkel Versatz die Höhe mit "Hohe_Offset" angehoben //Synchrowinkel/2 bewirkt den Überlappenden Lauf if (L_Bein_Vertikal_SOLL < 0) //WINKEL_Bein[y] immer Positiv machen { L_Bein_Vertikal_SOLL_rech = L_Bein_Vertikal_SOLL * -1; } else { L_Bein_Vertikal_SOLL_rech = L_Bein_Vertikal_SOLL; } if (Vor_Ruck_Hub[y] == 0) //wenn Vorhub { if ((L_Bein_Vertikal_SOLL_rech >= Schrittweite_MAX-(UberLauf_rech/2)) && Ruckhub_Unterdruckung == 0) //wenn Zielwert_Drehwinkel_Bein noch nicht erreicht (ANFANG Vorhub) / wenn Rückhub gesperrt, dann kein Anheben des Beins durch Hohe_Offset { HoheBein = (Hohe_Global - (Hohe_Offset * ((L_Bein_Vertikal_SOLL_rech - (Schrittweite_MAX-(UberLauf_rech/2))) / (UberLauf_rech/2)))); //wenn Bein in Bereich Überlappendenlauf kommt, um Offsetwert anheben (dieser wird Schrittweiten abhänig angepasst, geradlinig, um ein "Stampfen zu verhindern), dies ermöglich das Aus- und Ein- Schwingen des Beines, um ein Abbremsen des Roboters zu verhindern (Bein ist im Rückhub sehr schnell, und braucht etwas zur Richtungsumkehr, daher Uberlauf_rech) } else { HoheBein = Hohe_Global; //sonst Beinhöhe Global } } else //wenn Rückhub dann Beinhöhe über Sinusfunktion { HoheBein = (Hohe_Global-Hohe_Offset)-(sin(((Schrittweite_MAX-L_Bein_Vertikal_SOLL_rech)*(90/Schrittweite_MAX))/180*M_PI)*(Hohe_Global-Hohe_Offset)); //(Hohe_Offset+((Hohe_Global-Hohe_Offset)-(sin((90-(WINKEL_Bein_hohe[c]/((Ziel_Drehwinkel_Bein+Synchro_Winkel)/90)))/180*M_PI)*(Hohe_Global-Hohe_Offset)))* 10) + 510; } //ENDE Höhe Bein DIAGONALE = sqrt(((HoheBein + FIX_HOHE) * (HoheBein + FIX_HOHE)) + square(L_BEIN_SOLL)); // HILF_A = ((BEIN_OS * BEIN_OS) - (BEIN_US * BEIN_US) + (DIAGONALE * DIAGONALE))/(2 * DIAGONALE); // WINKEL_a = acos((HILF_A / BEIN_OS)) * 180/M_PI; // WINKEL_OS[y] = (asin(L_BEIN_SOLL / DIAGONALE) * 180/M_PI) + WINKEL_a - 90; //Übergabewert Servo_OS WINKEL_KNIE[y] = (asin((DIAGONALE - HILF_A) / BEIN_US) * 180/M_PI) + (90 - WINKEL_a) - 90; //Übergabewert Servo_US //die errechneten Werte noch für die nächste Berechnung mit übernehmen L_Bein_Horrizontal_IST[y] = L_Bein_Horrizontal_SOLL; //nur wichtig bei Radiuslauf L_Bein_Vertikal_IST[y] = L_Bein_Vertikal_SOLL; } //ENDE--------------------------- Bein Berechnungen ---------------------------------------- short int HiMe_Hohe_geandert; //Höhe wurde geändert, somit muss Ruckhub_Faktor und Zielwert_Drehwinkel_Bein neu berechent werden float Hohe_Global_Speicher; //Speichert den Wert Höhe Global um ihn später wieder anfahren zu können void Hohe_geandert (void) //Höhe wurde geändert, somit muss Ruckhub_Faktor und Zielwert_Drehwinkel_Bein neu berechent werden { //Zielwert_Drehwinkel_Bein //berechnet sich aus der Beinhöhe, um Kollision der Beine zu verhindern if (Hohe_Global > -15) //gilt nur bei Höhe größer -15mm (bei weniger Zielwert Drehwinkel Bein = 42°) { HiMe_MaxAUSLbein = L_Bein_Horrizontal_Offset * tan((42-((Hohe_Global+15)/2.05)) * M_PI/180); //Maximalen Auslenkung aus Höhe Berechen------------------------------------->HIER kann die Automatische Auslenkung geändert werden if(HiMe_MaxAUSLbein < 36) //Mindestwert Auslenkung ist ist 36mm { HiMe_MaxAUSLbein = 36; } if (HiMe_MaxAUSLbein < Maxauslenkung_Bein_Eingabe) //den kleineren der Beiden Werte als Maximalwinkel setzen { Maxauslenkung_Bein = HiMe_MaxAUSLbein; } else { Maxauslenkung_Bein = Maxauslenkung_Bein_Eingabe; } } else { if(Maxauslenkung_Bein_Eingabe < 81) //bei Höhe kleiner -15mm, dann Maximalauslenkung 42° oder kleinere Eingabe { Maxauslenkung_Bein = Maxauslenkung_Bein_Eingabe; } else { Maxauslenkung_Bein = 81; //Maximalauslenkung 42° } } //-------------------------------- Rückhubfaktor festlegen--------------------------------- //Rückhubfaktor festlegen, bei 3-3 gibt es keinen Startwinkel (was Bein von 0° versetzt ist) if (Laufmodus == 33) //bei Laufmodus 3-3 ist der Startwinkel 0°, deswegen separate Berechnung { Ruckhub_Faktor = (Maxauslenkung_Bein) / (Maxauslenkung_Bein - (2*Uberlappenderlauf)); } else { Ruckhub_Faktor = (Maxauslenkung_Bein * Ruckhub_Geschwindigkeit) / ((Maxauslenkung_Bein - (2*Uberlappenderlauf)) - (Maxauslenkung_Bein-((Maxauslenkung_Bein+(Maxauslenkung_Bein-(Uberlappenderlauf*2)))/Ruckhub_Geschwindigkeit))); } //ENDE---------------------------- Rückhubfaktor festlegen--------------------------------- HiMe_Hohe_geandert = Hohe_Global; //Wert für Vergleich übernehmen } //------------------------------- Grundstellung -------------------------------------------- //Beim erstmaligen Programmstart müssen die Beine in eine definierte Position gebracht werden. //Auch bei Fehlern nötig, wenn z.B. L_Bein_Vertikal oder L_Bein_Horrizontal Werte überschreiten, die Hardwaremäßig (zuviel mm) nicht ausführbar sind //USART-Empfang sperren, und Bewegungsinfos auf 0 setzen, Drehmodus auf 0 //Roboterhöhe Speichern, und Roboter absenken, das Beine frei schweben //Sollwerte berechnen, aus LAufmosi //L_Bein_Horrizontal auf Offsetwert anpssen, falls Wert überlaufen war (<0 oder Doppelter Offsetwert) auf Offsetwert springen //L_Bein_Vertikal auf Offsetwert anpssen, falls Wert überlaufen war (<0 oder Doppelter Maximalauslenkungswert) auf Zielwert springen //Wenn alle 6 Beine auf Zielwerten stehen, dann Roboter_Höhe wieder auf ausgangswert anheben und Programm beenden. char Grundstellung_ausfuhren = 1; //Grundstellung wird ausgeführt, es wird USART-Empfang und Grundstellung ignoriert void Grundstellung (void) //Beine in Grundstellung bringen (ist beim erstmaligen Einschlten, Laufmoduswechsel???, oder Fehlern in der Berechnung der Beine nötig) { float L_Bein_Vertikal_Grundstellung[7]; //Startwert für Bein in mm (wie L_Bein_Vertikal_IST/SOLL) short int Sollpositionen_erreicht; //L_Bein_vertikal_IST und L_Bein_Horrizontak_IST sind auf Startwerte erreicht int i = 1; //Hilfbit für Schleife if (Grundstellung_ausfuhren == 0) //Roboterbewegung verhindern, durch Überspringen des USART-Empfang in Hauptschleife { Roboter_bewegung_mm = 0; Radius_Kurve = 0; Drehmodus_Speicher = Drehmodus; //um richtigen Sollwert für Drehmodus auszuwählen. Sonst Fehler bei Drehmodus und 3-3 Drehmodus = 0; Grundstellung_ausfuhren = 1; //Die Beine werden in Grundstellung gebracht, und verriegeln den USART-Empfang, die Berechnung zum Drehmodus, ...? Hohe_Global_Speicher = Hohe_Global; //nach erlogten Startwert/Offsetwert anfahren, die ursprüngliche Roboter-Höhe wieder anfahren Sollpositionen_erreicht = 0; //einmalig initialisieren, pro Funktionsaufruf //alle Beine in Vorhub, für alle Laufmodi Vor_Ruck_Hub[1] = 0; Vor_Ruck_Hub[2] = 0; Vor_Ruck_Hub[3] = 0; Vor_Ruck_Hub[4] = 0; Vor_Ruck_Hub[5] = 0; Vor_Ruck_Hub[6] = 0; } //Roboter absetzen und Fußspitzen auf 3mm über Boden anheben, damit Beine frei zum bewegen sind. if (Grundstellung_ausfuhren == 1) { if (Hohe_Global > (Hohe_Offset * -1)) //Roboter Höhe absenken, bis Fußspitzen 3mm über Boden schweben { if (Hohe_Global <= ((Hohe_Offset * -1) + 0.1)) //Wenn Grenzwert fast erreicht, und durch rechnen nicht mehr erreicht werden kann. { Hohe_Global = (Hohe_Offset * -1); } else { Hohe_Global -= 0.1; //Höhe pro Schleifendurchgang anpassen } } else { if (Hohe_Global >= ((Hohe_Offset * -1) - 0.1)) //Wenn Grenzwert fast erreicht, und durch rechnen nicht mehr erreicht werden kann. { Hohe_Global = (Hohe_Offset * -1); } else { Hohe_Global += 0.1; //Höhe pro Schleifendurchgang anpassen } } if (Hohe_Global == (Hohe_Offset * -1)) //Fußspitzen berühren nicht mehr den Boden und können bewegt werden { Grundstellung_ausfuhren = 2; } } //Sollposition bestimmen, Vertikal aus Laufmodi und Maximalauslenkung Bein. Horrizontal aus Offsetwert errechnen //L_Bein_Vertikal aus Laufmodus bestimmen. Es wird der Startwert festgelegt (Beinspreizung beginnent mit Bein6 @100%) //verrechnen das (100% = 1 / 50# = 0 / 0% = -1) ergeben, und mit Maxauslenkung_Bein multiplizieren if (Laufmodus == 33) { Ruckhub_Geschwindigkeit = 1; Hohe_geandert(); //muß ausgeführt werden, das sich Rückhubgeschwindigkeit ändert, und somit eine neue Rückhub_Faktor ergibt L_Bein_Vertikal_Grundstellung[2] = Maxauslenkung_Bein; L_Bein_Vertikal_Grundstellung[5] = ((0.5 - ((100*Uberlappenderlauf/Maxauslenkung_Bein)/100)) * -2) * Maxauslenkung_Bein; L_Bein_Vertikal_Grundstellung[6] = Maxauslenkung_Bein; //Bei Drehmodus müßen für Bein 1,3,4 andere Sollwerte angefahren werden.(Theoretisch auch L_Bein_Horrizontal. / und * Schrittweite_MAX) if (Drehmodus_Speicher == 0) //Geradeaus { L_Bein_Vertikal_Grundstellung[1] = ((0.5 - (1 - (((100/Ruckhub_Geschwindigkeit) - ((100*Uberlappenderlauf/Maxauslenkung_Bein)/Ruckhub_Geschwindigkeit))/100)))* -2) * Maxauslenkung_Bein; L_Bein_Vertikal_Grundstellung[3] = ((0.5 - ((100*Uberlappenderlauf/Maxauslenkung_Bein)/100)) * -2) * Maxauslenkung_Bein; L_Bein_Vertikal_Grundstellung[4] = Maxauslenkung_Bein; } else //Drehmodus { L_Bein_Vertikal_Grundstellung[1] = ((0.5 - ((((100/Ruckhub_Geschwindigkeit) - ((100*Uberlappenderlauf/Maxauslenkung_Bein)/Ruckhub_Geschwindigkeit))/100)))* -2) * Maxauslenkung_Bein; L_Bein_Vertikal_Grundstellung[3] = L_Bein_Vertikal_Grundstellung[1]; L_Bein_Vertikal_Grundstellung[4] = Maxauslenkung_Bein * -1;//0% } Max_Anz_Beine_Ruckhub = 3; //Maximale Anzahl an Beinen die sich im Rückhub befinden dürfen. Wird in Bein_Berechnungen bei der Umschaltung Vorhub_Rückhub berücksichtigt (Ruckhub_Unterdruckung), und ist für jeden Laufmodi anders. } if (Laufmodus == 42) { Ruckhub_Geschwindigkeit = 2; //Faktor den sich das Bein bei Rückhub schneller bewegt also Vorhub Hohe_geandert(); //muß ausgeführt werden, das sich Rückhubgeschwindigkeit ändert, und somit eine neue Rückhub_Faktor ergibt L_Bein_Vertikal_Grundstellung[1] = Maxauslenkung_Bein;//((0,5 - ((((100/Ruckhub_Geschwindigkeit) - ((100*Uberlappenderlauf/Maxauslenkung_Bein)/Ruckhub_Geschwindigkeit))/100) * 0)) * -2 ); L_Bein_Vertikal_Grundstellung[2] = ((0.5 - (1 - (((100/Ruckhub_Geschwindigkeit) - ((100*Uberlappenderlauf/Maxauslenkung_Bein)/Ruckhub_Geschwindigkeit)) * 2/100))) * -2) * Maxauslenkung_Bein; L_Bein_Vertikal_Grundstellung[3] = ((0.5 - (1 - (((100/Ruckhub_Geschwindigkeit) - ((100*Uberlappenderlauf/Maxauslenkung_Bein)/Ruckhub_Geschwindigkeit))/100)))* -2) * Maxauslenkung_Bein; L_Bein_Vertikal_Grundstellung[4] = L_Bein_Vertikal_Grundstellung[2]; //ist gleich wie Bein 2 L_Bein_Vertikal_Grundstellung[5] = L_Bein_Vertikal_Grundstellung[3]; //ist gleich wie Bein 3 L_Bein_Vertikal_Grundstellung[6] = Maxauslenkung_Bein; //ist gleich wie Bein 1 Max_Anz_Beine_Ruckhub = 3; //Maximale Anzahl an Beinen die sich im Rückhub befinden dürfen. Wird in Bein_Berechnungen bei der Umschaltung Vorhub_Rückhub berücksichtigt (Ruckhub_Unterdruckung), und ist für jeden Laufmodi anders. } if (Laufmodus == 51) { Ruckhub_Geschwindigkeit = 5; //Faktor den sich das Bein bei Rückhub schneller bewegt also Vorhub Hohe_geandert(); //muß ausgeführt werden, das sich Rückhubgeschwindigkeit ändert, und somit eine neue Rückhub_Faktor ergibt L_Bein_Vertikal_Grundstellung[1] = ((0.5 - (1 - ((100/Ruckhub_Geschwindigkeit) - ((100*Uberlappenderlauf/Maxauslenkung_Bein)/Ruckhub_Geschwindigkeit))/100)) * -2) * Maxauslenkung_Bein; L_Bein_Vertikal_Grundstellung[2] = ((0.5 - (1 - (((100/Ruckhub_Geschwindigkeit) - ((100*Uberlappenderlauf/Maxauslenkung_Bein)/Ruckhub_Geschwindigkeit)) * 4/100))) * -2) * Maxauslenkung_Bein; L_Bein_Vertikal_Grundstellung[3] = ((0.5 - (1 - (((100/Ruckhub_Geschwindigkeit) - ((100*Uberlappenderlauf/Maxauslenkung_Bein)/Ruckhub_Geschwindigkeit)) * 3/100))) * -2) * Maxauslenkung_Bein; L_Bein_Vertikal_Grundstellung[4] = ((0.5 - (1 - (((100/Ruckhub_Geschwindigkeit) - ((100*Uberlappenderlauf/Maxauslenkung_Bein)/Ruckhub_Geschwindigkeit)) * 5/100))) * -2) * Maxauslenkung_Bein; L_Bein_Vertikal_Grundstellung[5] = ((0.5 - (1 - (((100/Ruckhub_Geschwindigkeit) - ((100*Uberlappenderlauf/Maxauslenkung_Bein)/Ruckhub_Geschwindigkeit)) * 2/100))) * -2) * Maxauslenkung_Bein; L_Bein_Vertikal_Grundstellung[6] = Maxauslenkung_Bein; //Bein 6 ist bei 100%, deshalb ist Fahktor für Schrittweite_MAX verrechnung auch 1. Max_Anz_Beine_Ruckhub = 2; //Maximale Anzahl an Beinen die sich im Rückhub befinden dürfen. Wird in Bein_Berechnungen bei der Umschaltung Vorhub_Rückhub berücksichtigt (Ruckhub_Unterdruckung), und ist für jeden Laufmodi anders. } //Beinabstände zum Roboter auf Offsetwert/Startwert herranführen if (Grundstellung_ausfuhren == 2) { i = 1; //sonst wird die for Schleife beim nächsten Durchlauf nicht abgeabreitet for (i=1; i<=6 ; i++) { if (L_Bein_Horrizontal_IST[i] != L_Bein_Horrizontal_Offset) //nur L_Bein_Horrizontal_IST herranführen wenn es eine Abweichung zu L_Bein_Horrizontal_Offset gibt, sonst als O.K. markieren { if ((L_Bein_Horrizontal_IST[i] <= 0) || (L_Bein_Horrizontal_IST[i] >= (2*L_Bein_Horrizontal_Offset))) //Falls Horrizontaler Beinabstand kleiner 0, oder Doppelter Offsetwert ist, dann sofort auf Offsetwert / FEHLERPRÜFUNG { L_Bein_Horrizontal_IST[i] = L_Bein_Horrizontal_Offset; } // L_Bein_Horrizontal_IST an Offsetwert herranführen if (L_Bein_Horrizontal_IST[i] <= L_Bein_Horrizontal_Offset) //Fußspitze nach Außen bewegen { if (L_Bein_Horrizontal_IST[i] >= (L_Bein_Horrizontal_Offset - 0.5)) //wenn Angleichung an Offsetwert nicht per Schleifendruchlauf angepasst werden kann { L_Bein_Horrizontal_IST[i] = L_Bein_Horrizontal_Offset; } else { L_Bein_Horrizontal_IST[i] += 0.5; } } else { if (L_Bein_Horrizontal_IST[i] <= (L_Bein_Horrizontal_Offset + 0.5)) //wenn Angleichung an Offsetwert nicht per Schleifendruchlauf angepasst werden kann { L_Bein_Horrizontal_IST[i] = L_Bein_Horrizontal_Offset; } else { L_Bein_Horrizontal_IST[i] -= 0.5; } } } else { Sollpositionen_erreicht |= 1<<(i-1); //wenn L_Bein_Horrizontal_IST[i] herrangeführt dann bit markieren (0000 0000 00xx xxxx) } // L_Bein_Vertikal_IST an Startwert herranführen if (L_Bein_Vertikal_IST[i] != L_Bein_Vertikal_Grundstellung[i]) //nur L_Bein_Vertikal_IST herranführen wenn es eine Abweichung zu L_Bein_Vertikal_Grundstellung gibt, sonst als O.K. markieren { if ((L_Bein_Vertikal_IST[i] < (Maxauslenkung_Bein * -2)) || (L_Bein_Vertikal_IST[i] > (Maxauslenkung_Bein * 2))) //Falls Vertikale Auslenkung mehr als Doppelt so Groß ist / FEHLERPRÜFUNG { L_Bein_Vertikal_IST[i] = 0; } if (L_Bein_Vertikal_IST[i] <= L_Bein_Vertikal_Grundstellung[i]) { if (L_Bein_Vertikal_IST[i] >= (L_Bein_Vertikal_Grundstellung[i] - 0.5)) //wenn Angleichung an Offsetwert nicht per Schleifendruchlauf angepasst werden kann { L_Bein_Vertikal_IST[i] = L_Bein_Vertikal_Grundstellung[i]; } else { L_Bein_Vertikal_IST[i] += 0.5; } } else { if (L_Bein_Vertikal_IST[i] <= (L_Bein_Vertikal_Grundstellung[i] + 0.5)) //wenn Angleichung an Offsetwert nicht per Schleifendruchlauf angepasst werden kann { L_Bein_Vertikal_IST[i] = L_Bein_Vertikal_Grundstellung[i]; } else { L_Bein_Vertikal_IST[i] -= 0.5; } } } else { Sollpositionen_erreicht |= 1<<(i+7); //wenn L_Bein_Vertikal_IST[i] herrangeführt dann bit markieren (00xx xxxx 0000 0000) } } if (Sollpositionen_erreicht == 0b0011111100111111) //wenn alle Beine auf Startwerte herangeführt dann fertig für nächsten Schritt 0b0011111100111111 = 16191 { Grundstellung_ausfuhren = 3; } } //nach erlogten Startwert/Offsetwert anfahren, die ursprüngliche Roboter-Höhe wieder anfahren if (Grundstellung_ausfuhren == 3) { if (Hohe_Global > Hohe_Global_Speicher) //Roboter Höhe absenken auf gespeicherten Wert (vor Grundstellung) { if (Hohe_Global <= (Hohe_Global_Speicher + 0.1)) //Wenn Grenzwert fast erreicht, und durch rechnen nicht mehr erreicht werden kann. { Hohe_Global = Hohe_Global_Speicher; } else { Hohe_Global -= 0.1; //Höhe pro Schleifendurchgang anpassen } } else { if (Hohe_Global >= (Hohe_Global_Speicher - 0.1)) //Wenn Grenzwert fast erreicht, und durch rechnen nicht mehr erreicht werden kann. { Hohe_Global = Hohe_Global_Speicher; } else { Hohe_Global += 0.1; //Höhe pro Schleifendurchgang anpassen } } if (Hohe_Global == Hohe_Global_Speicher) //gespeicherte Höhe, von vor Grundstellungsprogrogramm ist wieder hergestellt { Grundstellung_ausfuhren = 0; } } } //ENDE--------------------------- Grundstellung --------------------------------------------
das waren dann alle 3 Teile
Die Funktion "Bein Berechnungen" wird dann in der Hauptschleife 6 mal aufgerufen. Von 6 bis 1, das liegt daran das die Synchronisation der Beinspreizung (Winkel/Stellungen zu einander) vom Bein 6 aus beginnt.
Ich weiß das man es bestimmt geordneter schreiben kann, aber für mich war das der Weg meines 1. C Projekts.
danke, aber - puh - das ist ja deutlich, ja sogar extrem mehr Aufwand als ich je vermutet hätte!
Berechtigungen
Zitieren
Lesezeichen