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