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