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.
Zitieren
Lesezeichen