Das muss ich mal wieder korrigieren. Der Chip im Teensy ist ein K20, der hat einen Cortex M4 ohne FPU. Der kommende Teensy wird einen K66 haben, der hat einen Cortex M4F mit FPU (und 180 MHz).Zitat von HaWe
Das muss ich mal wieder korrigieren. Der Chip im Teensy ist ein K20, der hat einen Cortex M4 ohne FPU. Der kommende Teensy wird einen K66 haben, der hat einen Cortex M4F mit FPU (und 180 MHz).
ok, wieder dazugelernt.
Ich bleibe aber doch lieber beim Due, außer es ist ein Mega als reines UART / I2C-Extension-Board für den Raspi.
Sorry aus Code wird so nix. Es wird mit folgen Fehler abgebrochen:
"Der Text, den du eingegeben hast, besteht aus 70966 Zeichen und ist damit zu lang. Bitte kürze den Text auf die maximale Länge von 34000 Zeichen."
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
Berechtigungen
Zitieren
Lesezeichen