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.
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--------------
- - - Aktualisiert - - -
Bitte mal Irgendwer irgendwas schreiben, es wird immer nur der letzte Post aktualisiert. Somit komm ich wieder ins Zeichenlimit.
Zitat von HaWe
Berechtigungen
Zitieren
Lesezeichen