Ahnugslos zum erstem Bot

[Tomy83]

Hallo Community,

ich stöbere ja schon etwas länger bei euch im Forum rum. Hier findet man ja allerhand nützliche Infos, die mir bei meinem Erstlingswerk schon enorm weiter geholfen haben.

Es soll ein Hexabod werden, und wie ich mir damals dachte, aus Alu natürlich. Nun, jetzt weiß ich wie schwer das werden kann (geworden ist)

Die Beine und den Rahmen hab ich schon fertig, und die Waage meinte es sollen 3390g inkl. Servos sein. Da bleibt nur zu hoffen das die Elektroik und der Akku nicht auch noch so schwer werden. Angepeielt waren mal 5Kg.
Meine Konstruktion ist bestimmt als eigenwillig zu betrachten, dass ist mir schon bewusst. Jedes Gelenk ist Kugelgelagert, und das Servo dient als 2. Lager in der Paarung. Mit dem Spiel, der Stabilität und der Leichtgänigkeit bin ich ganz zufrieden, ist aber alles biss schwer geworden :(

An Servos sind jetzt Verbaut:
Hüfte: Savöx SC-1256 TG (mit denen habe ich schon gute Erfahrungen im Modelbau gesammelt 1:8 Verbrenner Truggy)
Rest: Savöx SC-1251MG

Ganz ohne Konzept habe ich auch nicht angefangen. Ein Proto"Bein" musste erst mal her halten um überhaupt mit der IK und der Software klar zu kommen. Meine IK ist jetzt bestimmt alles Andere als die effektivste, aber ich hab es allein hin bekommen. Zumindest sollange bis der Tierchen auch mal richtig laufen soll ;)

Den Irrweg über BASCOM hab ich am Anfang auch mal mit einem ATMega644PU versucht. Aber meine Ineffektive IK hat mich zum XMega und C getrieben. Nur sind meine Programmierfähigkeiten eher zu gering um die Hardware sinvoll zu nutzen. Daher plane ich mit zwei XMega256a3B. Einer als Master für Sensoren und Steuersignal aufnahme (optional->später) und die grobe Lauf-Abfolg. Der Zweite soll dann die eigentlichen Berechnungen pro Bein vornehemen.

Nun hab ich auch den Schaltplan fertig, somit kann ich jetzt auch den Lötkolben auspacken :)

Wo ich mir noch nicht ganz so klar bin ist die Stromversorgung. NiMh und glatte 6V dafür aber schwer. Oder lieber SBEC und LIPO/LIFE aber mit Störungen durch den Schsltregler. Wie macht ihr das?



2837928380

[Joe23]

Nice,

endlich mal wieder ein gescheiter Hexa. Alu, Kugellager...hört sich sehr gut an.

NimH-Akkus sind schon überholt. Würde auf jeden Fall Lipos empfehlen, die sind vom Verhältnis Leistung/Gewicht deutlich besser.

Für deinen Bot würden sich gerade was das Gewicht entspricht auch "gute" Servos eignen. Ich empfehle von Dynamixel die Serie MX.

Die haben verdammt gute Auflösungen, sind präzise, stark und laufen zwischen 12V und 16V. Hier brauchst du dann keinen Schaltregler für den Betrieb...die kannst du direkt an den Lipo hängen.

Denk bitte daran das du dir mit "schlechten" Servos deine gute Mechanik kaputt machst. Hier lohnt es sich etwas mehr zu investieren.

Gruß

[Tomy83]

Hallo joe23,

Erstmal danke für deine Antwort. Die Servos haben ja sehr interessante Merkmale. Die Kosten sind aber schon sehr Hoch, finde ich.
Aber ich habe ja bereits die beschriebenen Savöx verbaut.
Ich habe sie mal provisorisch am Roboter getestet, und bei einer Stromaufnahme von ca.2A (blockierstrom) schafft es ein einzelnes Servo den Roboter einseitig hoch zu drücken. Also bin ich mal optimistisch das die verbauen Teile auch funktionieren. Ansonsten werde ich mir deinen Tipp mal ganz genau ansehen.

Nun kommt mein eigentliches Problem wieder zum tragen. 18x2A müssen erstmal in 6V bereit gestellt werden. Und ich frage mich ob ich mir den Gewichtsvorteil durch die Spannungswandler/sbec wieder zunichte mache und dafür noch Störungen durch die Schaltreger in Kauf nehmen muss? Leider fehlen mir zu den Schaltreglern praktische Erfahrungen, um deren Leistung und Störimpulse abschätzen zu können.

Für die Lipos sprechen die hohen Ströme und das geringere Gewicht. Und für die Nimh die Einfachheit und Kosten Ersparnis.

[Rabenauge]

Wenn du halbwegs gute NiMh haben willst (und die wirst du brauchen) kannst du Kostenersparnis vergessen. Das wird nix mit den billigen- inzwischen sind LiPo`s weit preiswerter zu haben als _gute_ NiMh.

Was deine Wandler bzw. die Servos angeht: mehr vertragen die nich?
Wenn die Dinger wenigstens 7.5V aushalten könnten, wärst du mit zwei Zellen Lipo bestens bei-und bräuchts keine Wandler.
Zudem geht der Strom noch bissle runter bei gleicher Leistung.

Auch, dass LiPo`s aufwendiger in der Handhabung sind, kommt nicht wirklich zum tragen: es ist kein Problem, nen paar LiPozellen per Controller zu überwachen, und den haste eh an Bord. Und nen vernünftiges Ladegerät brauchst du zwingend für hochstromfähige NiMh auch- und die können meist LiPo`s ebenfalls.

[Tomy83]

Die Antworten zur Akkutechnologie sind ja recht eindeutig. Obwohl es schön were wenn noch Jemand Erfahrngswerte hätte. Ladegerät für Lipo und Co. ist sowieso vorhanden. Und da ich ohnehin vor hatte, für die ersten Versuche, ein Netzteil (leider 12V) zu Nutzen, kommt mir der SBEC eigentlich ganz recht.

Nochmal eine andere Frage, da ich gerade die Platine machen will. Ist es für die PWM (Servosignal) sinnvoll ein Pulldown Wiederstand zu nutzen? Da der µC Ausgang ja sowieso über den Optokoppler auf GND geführt wird. Auf meiner Versuchsplatine funktioniert das zwar, aber eine Fachmeinung wäre mir schon ganz lieb.

Danke schonmal

[HannoHupmann]

Sehr schön! Mal wieder ein Hexa der nicht laufen wird! Ich werde nicht müde in jedem Thread diesen Satz zu bringen, auch wenn es mich immer wieder erstaunt, dass die Grundüberlegungen beim Bau eines Hexas nicht beachtet werden.
Wieso sagt eigentlich jeder, dass er die Hexa Threats im Forum gelesen hat und am Ende komm ich doch wieder mit meiner Standardrechnung?

Besonders wenn es ein Hexa mit 3,4kg bzw. respektive 5kg werden soll. Die verwendeten Servos sind ein Witz, für den Preis so wenig Leistung. 90Ncm reicht nie, bzw. kommt auf den Hebelarm an, aber rechnen wirs mal wieder vor:

DerKlassiker, die Auslegungsberechnung:
3,3kg / 4 Beine' * 10cm Hebelarm = 82,5Ncm hart an der Grenze aber ganz ohne Elektronik und Akkus gerechnet. Die Servos müssten 792g auf die Wage bringen.
--> 4kg / 4 Beine' * 10cm Hebelarm = 100Ncm und schon gehts nicht mehr und 10cm Hebelarm ist sehr kurz gerechnet.

') Beine am Boden

Aber um gleich den Antworten zuvor zu kommen, bei 5kg (und das wird er mit Akkus locker wiegen!!!) wird auch 5 Beine auf dem Boden nicht ausreichen, denn es kommt wieder 100Ncm raus.

Realistisch wäre:
5Kg * 20cm / 4 Beine = 250Ncm * 1,5 (Sicherheitsfaktor) = 375Ncm!! pro Schulterstervo!

Dieser Rechnung berückstichtig übrigens nur die statischen Kräfte. Dynamisch ist es leider noch etwas mehr und daher sollte man die Servos nicht auf die statischen Drehmomente auslegen sondern auf dynamische.
Übrigens dürfte man bei der Drehemomentanforderung bei 15 - 20A Spitzenstrom und 4-7A Dauerstrom liegen. Ein 25A SBEC wiegt etwa 100g aber bei der Leistung braucht man ordentlich viel Pufferkondensatoren!


Eigentlich ist es ganz einfach: ein Hexabot muss so leicht wie es nur irgendwie geht werden, dann wird er kostengünstig und effizient!
Die Konstruktion ist ja ganz hübsch aber viel zu masiv ausgelegt ich würde sagen in der Bauform könnte der Hexa mit 50kg belastet werden ohne dass sich was verbiegt. Sorry aber klassischer Planungsfehler bei der Auslegung und ich weise lieber jetzt darauf hin als wenn dann alles hübsch aufgebaut ist und die Servos nacheinander ausfallen.

[Tomy83]

hallo und danke für deine Antwort,

Leider bin ich nicht der Einzigste der nicht alles durch liest. Die Schulterservos haben 200Ncm.
Das die Konstruktion etwas Überdimensioniert ist, hatte ich ja bereits erwähnt. Und falls die Servos doch zu schwach sind, da muss ich halt in den sauren Apfel beißen :-(
Ein klein wenig hab ich ja auch gerechnet, der Beinabstand ist unausgelenkt 92,05mm. Das wären ca.7kg Trag Gewicht. Die 5kg könnten statisch bei 45° auslenkung noch gehalten werden, aber bei 35° ist eh Schluss zwecks Kippgefahr.
Gut die Dynamischekraft hab ich ganz außen vor gelassen.
Mir stellt sich noch die Frage: wozu alles berechnen wenn ich am Ende sowieso 50prozent aufschlage.

[HannoHupmann]

Ohne genaue Angaben kann ich natürlich keine genaue Gegenrechnung anfangen. Mit einem kürzeren Hebelarm lässt sich das Drehmoment natürlich verkleinern. Dann könnten die 200Ncm auch ganz gut reichen, zumindest für die Schulter. Wobei ich ehrlich gesagt nicht sagen kann worauf sich die "92,05mm unausgelenkt" beziehen sollen. Die Angabe ist etwas schwammig, da ich und die anderen User nicht wissen worauf du dieses Maß beziehst. So erschließt sich mir auch nicht was die beiden Winkelangaben beteuten sollen. Hier kann ich nur raten, worauf ich keine Lust habe.

Den Sicherheitswert von 50% - 100% verwendet man um sich genau diese dynamische Berechnung zu sparen. Da diese recht kompliziert ist, nimmt man die statischen Kräfte und gibt eine großzügigen Sicherheitswert dazu und fertig.
Korrekt ist das nicht, aber unkompliziert.

Für den Fuss würde ich mit 90Ncm wirklich vorsichtig sein. Bei unkünstiger Beinstellung können hier hohe Drehmomente entstehen und der Roboter geht in die Grätsche!

[PICture]

... wozu alles berechnen wenn ich am Ende sowieso 50prozent aufschlage.

... weil es z.B. 500 % nötig werden._. Na ja, du fängst erst lernen an. ;)

[Tomy83]

mal fix ein Bild um 92,05mm zu erläutern, diese sind jetzt doch 90,05mm. Dabei ist das Bein exakt 90° zum Roboter in Laufrichtungsachse. Oder anders gesagt, in Mittelstellung. Sollte Statisch, ohne Sicherheit 6,792Kg tragen, und bei Max-Auslenkung (35° von Mittelstellung entfernt / Bein Ausgestreckt) 5,417Kg (@11,29cm Gesamtlänge Horizontal Fußspitze bis Schulter).
28498

Den Spaß für den "Fuß" hab ich auch mal durchgerechnet (wieder ohne Sicherheit). Da wäre eine wirksame Hebel Länge (Horizontale) von 52,21mm. Das würde 85,36 Ncm machen.

Das jetzt die Sicherheit fehlt, davon habt ihr mich überzeugt. Und das Haltekraft und Stellmoment nicht das selbe sind sollte klar sein. Da ich nun die Servos schon einmal habe werde ich es auch erst mal damit versuchen. Falls es nicht geht, dann ist der Apfel sauer, und ich brauch 6 neue. Dann könnten die 200Ncm-Teile ins Knie und an der Schulter gibt's dann gleich Lipo-Fähige. Das würde auch die Stromversorgung entlasten (SBEC). Zur Pufferung wollte ich es erst mal mit 8200µF pro Bein versuchen, und das Oszi wird mir schon sagen ob das reicht (hoffe ich).

Die 90Ncm Servos sind eigentlich hauptsächlich drin, weil diese Low-Profile sind. (deswegen etwas teuer)

Zum Gewicht, da war ich anfangs etwas Blauäugig... Ist doch etwas schwerer geworden wie erhofft. Wie gesagt das ist mein erster Roboter. Quasi der "Anfang vom Lernen". Deswegen sind auch so viel Löcher im ALU. Da ich Größtenteils auf Halbzeuge zurückgreifen wollte, war die Auswahl an Material etwas beschränkt (keine Fräsmaschine verfügbar).

Hat noch jemand Meinung zu den Pull-Downs am PWM??? Da ich mir nicht sicher bin, ob die Störungen durch die LED im Optokoppler bereits abgebaut werden.

[HannoHupmann]

Pull Down und Optokopler braucht es in der Signalleitung nicht zwingend und ich fahre bisher ganz gut ohne. Einzig ein paar Ferritkerne habe ich den Kabeln gespendet.

Die Querverstrebungen im Körper braucht es meiner Meinung nach nicht, das sieht mir so schon sehr massiv aus. Da lässt sich wieder ein wenig Gewicht sparen.

Meinen nächsten Hexa werde ich wohl nach konsequentem Leichtbau bauen, aber das wäre dann auch schon Nummer 5 :-P

[PICture]

Hat noch jemand Meinung zu den Pull-Downs am PWM??? Da ich mir nicht sicher bin, ob die Störungen durch die LED im Optokoppler bereits abgebaut werden.

Ich bin (fast) sicher, dass die LED im Optokopler mit seinem Strom um ein paar mA im Vergleich zu stärker beströmten Motoren unbedeutend ist.

Sonst wollte ich nur das von HannoHupmann bestätigen und dazu sagen, dass wenn ohne "pull down" der Logikpegel stabil gleich Null ist, braucht man ihn nicht (angeblich gibt es dann in ICs interne). In gestörter Umgebung helfen gegen EMV Impulsen oft sehr kleine Kapazitäten um 100 pF an Eingängen (je nach Eingangsimpedanz und am Eingang "hängende" Leitungslänge), die dort auftrettende Störrungsimpulse kurzschließen und den genutzten Flanken nicht schaden.

[Tomy83]

Danke für eure Antworten ,

Ich meinte nicht das die LED stört, da hab ich mich falsch ausgedrückt. Ich meine das der Stromflus durch die LED eventuell schon reicht um "Spannungs-Einstrahlungen" abzuleiten. Aber eure Antworten war eindeutig genug :-)
Den Tipp mit dem C an Ausgang behalte ich mir mal im Hinterkopf.

Die Querverstrebung wird noch zur Befestigung der Elektonik (Platinen, Akku...) benötigt. Ich glaube auch nicht das ich damit die Karre aus den Dreck ziehen kann. Das Grundübel sind die Kugellager, deren Sitze und der gesamte Material verbrauch, also die Summe aller Teile. Wenn es nicht funktioniert, worauf ihr mich ja hingewiesen habt, dann gibt es ja noch Servos in der Bauform Bis 35kg.

Danke nochmal für eure Tipps.

[HannoHupmann]

Solange es keine tragenden Querstreben sind, die für die mechanische Stabilität notwendig sind, können sie auch aus einem leichteren Material sein. Der Trick am Leichtbau ist überall immer wieder ein paar Gramm einzusparen. So kommt an Ende ein vernünftiges Gewicht heraus.
Mein nächster Roboter wird konsequenter Leichtbau aus CFK, aber das wäre dann Hexa Nummer 5 :-P.

Dein Problem ist, dass mit dem hohen Gewicht ein Teufelskreis entsteht: hohes Gewicht --> Stärkere Servos --> Mehr Akkus --> höheres Gewicht --> usw.
Das ganze wird dann ineffizient und vor allen teuer!!!
Es gibt einen Grund warum sich Hexas bei 2 bis 3,5kg einpendeln, denn da ist das Preis-Leistungsverhältnis der verfügbaren Modellbau-Servos am besten.

Zur Verdeutlichung: Deine ausgewählten Servos kosten zusammen über 900€!!! Dafür, dass sie außer einem schnelle Stellbereich nichts vorweisen können. Die 0,09sec für 60° sind bei einem Hexabot völlig unwichtig und daher bezahlt man für ein Feature, dass man nicht braucht. Damit ist das Preis/Leistung Verhältnis bei den Savox Servos einfach Mist.

Als Beispiel könnte man alternativ für 56€ (immer noch billiger als die 62€) folgende Servos von HiTec nehmen:
HSR-8498HB
Drehmoment: 6.0V 7.4kgcm /7.4V 9.0kgcm (auf einigen Seiten wir das fälschlich als Ncm angegeben).
Beidseitig Kugelgelager => Kein Gegenlager notwendig.
Mit Datenrückmeldung: Strom, Spannung, Position.
Zudem eine Betriebsspannung von 6,0V bis 7,4V, so dass kein SBEC notwendig wäre.
Fazit: Genauso "stark" und mehr sinnvolle Features.

Alternativ kann man auch Standard Modellbau Servos verwenden, die kosten dann nur 30€ und man könnte 300€ für ein Re-Design des Körpers ausgeben und wäre immer noch billiger.

Ein Hexa ist teuer genug. Die Auswahl der Servos ist das Kernthema und alles andere wird entsprechend dieser Entscheidung ausgelegt.
Die Auswahl deiner Servos kann mich leider nicht überzeugen, aber es ist dein Geld und wenn du 900€ für Servos ausgeben möchtest, dann kann ich nur froh sein, dass es nicht mein Geld ist. Auch wenn ich damit rechne, dass das Projekt dann sehr schnell aus Kostengründen eingestampft wird. Es gibt nicht viele Menschen auf der Welt die sich mal eben 1000€ für einen "ersten Versuch" leisten können.

[Tomy83]

Du scheinst ja nicht aufgeben zuwollen, mit den Servos. Warum dir das auch immer so wichtig ist?
Willst du wirklich ein 90Ncm Servo mit einem 200Ncm Servo vergleichen? Gut das hat keinen output, aber das Verhàltniss Kraft/Preis ist bei Savöx deutlich besser als bei Hitec!
Die Auswahl hatte ich bereits erläutert. Die 90Ncm Servos sind "low profile", also niedriger.Das spart Bauhöhe.

Falls es also, wie von dir Vorrausgesagt, nicht geht. Weil "meine Servos" zu schwach sind, dann muss ich ja nicht gleich 18 Stück neu kaufen und obendrein noch den kompletten Roboter neu bauen. Es reicht doch wenn ich 6 stärkere Schulterservos kaufe. Und die "alten" 200Ncm Teile ins Knie setzte. Mit den übrigen könnte man sich überlegen, dieser bei ebay zuverkaufen.

dein Einwand zur Drehmoment Auslegung, bei Gewicht x finde ich ja wirklich gut, aber als Moderator sollte ich doch nicht jedes Detail kaputreden. Und eventuell nicht von vornherrein als unmöglich abstempeln "und wieder ein Hexa der nicht laufen wird". Verstehe mich Bitte nicht falsch, aber ich bin der Meinung das Kritik nicht vernichtend ausfallen muss.

Und wieviel Geld ich für meine Freizeit beschäftigung ausgebe, ist absolut nur Meine Sache. Da kann man 900€ auch in weniger Stunden los werden. Irgendwo ist auch der Weg das Ziel. Hätte ich nur einen Roboter haben wollen, dann hätte ich mir einfach einen fertigen gekauft. Aber ich will in meiner Freizeit auch ab und an was sinnvolles tun. Und das Thema Geld vs. Hobby ist absolut OT

[HannoHupmann]

Es ist nun mal einfach ein Fakt, dass die Savox Servos nicht die besten für einen Hexa sind und diesen Tipp zu geben werde ich in der Tat nicht müde.
Dabei vergleiche ich keineswegs deine 200Ncm mit den 90Ncm, sondern 200Ncm mit 6kg und 90Ncm mit 2,5kg und dabei stelle ich fest: Servos für 52€ bieten mehr als Servos für 62€.

Als alter Hase im Bereich Hexabots und im Forum nehme ich es mir einfach so heraus :-P, neue Hexaprojekte zu kritisieren, ihre Schwachstellen aufzuzeigen und optimierungsmöglichkeiten anzubieten. Bisher hat sich noch keiner beschwert, wenn ich ihm Einsparungsmöglichkeiten von mehreren hundert Euro aufgezeigt habe, da bist du offensichtlich der erste. Mir geht es auch gar nicht darum ob du 900€ oder 500€ für dein Hobby ausgibst, sondern dass es unsinnig ist 900€ für eine Funktion auszugeben die man für 500€ haben kann und damit 400€ für irgendwas anderes zur Verfügung hat (z.B. einen schicken CFK Leichtbau Körper, sieht auch stylisch aus und man bekommt die ganzen Probleme von oben nicht).

Kleiner Technischer Hinweis noch zum Gewicht, da ich noch nicht all mein Pulver verschossen habe:
Schwerere Beine bedeuten auch eine größere Massenträgheit bei jeder Bewegung, diese wiederrum wirkt sich äußerst eklig aus bei Hexas. Daher gilt insbesondere für Beine: jedes Gramm einsparen!


Aber mal ganz unter uns, am Ende steht es dir natürlich komplett frei wie du deinen Hexa baust, dir steht es sogar frei meine lästigen Kommentare komplett zu ignorieren und wenn du mir verbietest dein Projekt zu kommentieren, dann akzeptiere ich das sogar, aber bis dahin werde ich hier weiter meine Meinung posten :)

[Tomy83]

Natürlich werde ich dir nicht deine Meinung verbieten, auch nicht das du diese kund tust. Ich war nur etwas erschrocken, wie direkt du dies tust. Und da sei mir die Kritik an der Kritik auch nicht untersagt.

Zu deinen Servovergleich: Da du ja selber festgestellt hast das ich mehr Drehmoment brauche, ist der Vorschlag für ein 60Ncm Servo nicht unbedingt vorteilhaft. Das würde nur bei einen kompletten Neuaufbau Sinn machen. Das habe ich aber nicht vor. Ich bin ja auch nicht angetreten um den besten Hexa zu bauen, sondern um "meinen Hexa" zu bauen. Wäre ja auch blöd wenn wir alle die gleichen, Eineiigen, Hexa`s bauen würden ;-) .
So bleibe ich erst mal dabei, ich Probiere es mit den Vorhandenen Servos. Und falls das nicht geht, dann muss ich halt eine Lösung erarbeiten (kürze Beine, stärkere Servos...)
Die Tipps mit den Einsparrungen würden auch sehr sehr gut sein, wenn ich die Servos und den Körper nicht schon fertig hätte. :oops:

Gewicht der Beine: Da kann ich leider nix dagegen sagen. Die Masseträgheit wird ein Problem. Aber da stellt sich mir auch die Frage, wie schnell sich den die Beine bewegen sollen. Die 0,09Sek. vom Servo sind ja eine Angabe ohne Last. In der Praxis wird man das nie erreichen können.

P.S. Abseits vom Thema: Kompliment an dein "Wall-E", Junior (3) hat Ihn auf Anhieb erkannt.

[HannoHupmann]

Danke, noch so ein Projekt an dem ich mal weiter machen wollen würde, nur leider fehlt fürs Hobby hinten und vorne die Zeit oder ich habe einfach zu viele Ideen :)

[Tomy83]

Ja die liebe Zeit. Wenn mir Beruf und Familie mehr Zeit liesen, dann würde ich nicht auch schon 2 Jahre daran rum werkeln. Gut war ja auch ein 3/4 Jahr Baustopp dazwischen.

Deine Hinweise zu den Servos haben mir nun auch keine Ruhe gelassen. In einen sehr primitiven Aufbau, habe ich mal die Servos probiert. Der Roboter stand einseitig auf einen Buch und auf der anderen Seite musste ein einzelnes Schulterservo das "Hochdrücken" übernehmen. Das hat bei 4,8V-6,2V super geklappt. Die Stromaufnahme war in der Spitze 2,6A (was an Spitze mit dem Multimeter Messbar ist) und durchschnittlich bei 2A. Nur Roboter hoch halten waren so ca. 1-1,5A. Wenn ich mal ordentlich mit dem Finger auf den Roboter gedrückt habe, stieg der Strom auf ca. 3A. Aber dann bin ich schon im Limit vom Netzteil. Das Servo hat übrigens seine Position nicht verlassen. Die Hebellänge war anfangs ca.90-95mm, und später hab ich es auch mit 13,5cm versucht. Was auch tadellos geklappt hat. Das Knieservo war behelfsmäßig mit Kabelbinder am Oberschenkel fixiert. Wie das dann am Ende mal funktioniert, mit richtigen Laufen, das werde ich so nicht feststellen können. Zu bemerken wäre noch, dass wenn ich nur den Roboter oben halte, dann wird das Servo recht schnell heiß. Und dann verliert es an Leistung :-(

Im Web hab ich gelesen das die Modelbaufahrer ihre Savöx Servos direkt mit LIFE-Akkus 2S betreiben, also 6,6V. Hat von euch schon Jemand Erfahrung damit, die Servos mit 0,6V Überspannung zu betreiben??? Dann könnte ich mir das SBEC sparen, den die allermeisten von den SBEC haben nur 10A Dauerstrom :-(

[HannoHupmann]

Punkt 1: Mit einem 3A Netzteil wird es später schwierig werden den Roboter zu betreiben. Früher hatte ich auch nur ein 3A Netzteil, aber mein Vinculum verschlingt mehr Strom, daher musste jetzt ein 12V-20A Netzteil her. Damit lässt er sich wunderbar betreiben.

Punkt 2: Beim Hexa davo hatte ich HiTec Servos verbaut und die auch gnadenlos mit 7,4V betrieben, was ohne Probleme funktioniert hat. Der Nachfolger hat einen 20A SBEC (ca. 19€) bekommen. Damit kann ich gleich zwei Fliegen mit einer Klappe erschlagen: a) die Servos bekommen eine schöne 6V Spannung (mit entsprechend vielen Kondensatoren dazu für die Leistungsspitzen) b) ich kann die Servos auch am 12V Netzteil betreiben.


http://www.ebay.de/itm/P20B12S-20A-5V-5-5V-6V-7V-9V-12S-51V-SBEC-UBEC-DCDC-Schaltregler-BEC-AMAX-Platin-/331090605815?pt=RC_Modellbau&hash=item4d1689b6f7

[Tomy83]

So nun ist doch wieder einige Zeit ins Land gegangen. Aber mitlerweile hat der Kleine (Übergewichtige) seine ersten Schritte gemacht.
Für einen stabilen 3-3 Lauf ist die Gewichtsverteilung alles andere als Ideal. Und am 4-2 Lauf (auch 5-1) muß ich noch gewaltig am Schrittablauf feilen. Bis jetzt kann er auch nur geradeaus und zurück laufen. Aber das ist ja schonmal was. Die Kraft der Servos scheint subjektiv zu reichen, und die Gesamtstromaufnahme ist ca. 12,5A (soweit es das Display am Netzteil anzeigt).

Wie macht ihr das mit der Beinbewegung, um ein Wackeln einzudämmen? Ich habe das Problem, dass wenn ein bein gerade fertig ist mit umsetzen und dann den Boden erreicht, das andere gerade abhebt. Was zu einem "Wackeln" des Körpers führt. Auch Bremst das gerade am Boden ankommende Bein die Bewegung des Körpers ab.
Wäre es Ratsam das Umzusetzende Bein eher auf dem Boden aufzusetzen um schonmal die Last des Körpers aufzuteilen/aufzunehmen?
Somit würde ich auch ein Abbremsen vermindern. Aber die Synchronisation der Beine macht das umso schwerer.

P.S. die Optokoppler in den Signalleitungen funktionieren super, ich habe eine vollständige Potentialtrennung und keine Störungen in den Servos.

[HannoHupmann]

Das Problem ist bekannt und kann einfach gelöst werden für 3-3 laufen. Man fügt einfach einen zwischenschritt ein, in dem alle 6 Beine auf dem Boden sind und hebt erst dann die nächsten 3 Füße.
Wenn 3-3 geht, dann sollte 4-2 und 5-1 noch leichter funktionieren. Eventuell musst Du die Reihenfolge der Füße ändern.

[Tomy83]

sehr lange nichts mehr geschrieben....
Aber dafür ist jetzt auch meine Tochter gesund auf der Welt und wird bald laufen können. :-)

die Koordinierung der Beine ist doch um einiges komplexer als ich es mir im Vorfeld gedacht habe.
Aktuell bin ich dabei die Beine immer wieder zum Laufmuster (Abstände der Fußspitzen zueinander) zu sortieren. Weil diese im Kurvenlauf und voralledingen im "Drehmodus" (Roboter dreht sich auf der Stelle um die eigene Achse) ganz schön durcheinander kommen. Erreichen will ich das mit einer variaben Geschwindigkeit der Beine beim Umsetzen, und der Zielpunkt des Beins (Schrittweite) wird auch variabel werden.

Ein Problem ergibt sich noch aus der Berechnungsgeschwindigkeit des chips. Es werden ja für Geradeauslauf weniger mathematische Formel benötigt als Kurvenlauf. Und da ich den Roboter pro Schleifendurchlauf um 0-1mm bewege, ergibt sich in der Kurve eine langsamere Geschwindigkeit.
Nun will ich aber das die Geschwindigkeit immer gleich ist. Also wäre es das einfachste die Vorgabe 0-1mm auf Kurve und geradeaus anzupassen. Aber wie am besten? mit einen Timer den Schleifendurchlauf messen und im nächsten Durchlauf berücksichtigen?
Gibt es da noch andere Ansätze?

[Tomy83]

hier mal ein wackliges Handyvideo zum akuellen Stand vom Projekt.
https://youtu.be/ZVZ_HfIVDpg

Zur Zeit ist die Kabelvernbedinung dran, die soll mittels Bluetooth Kabellos werden.

[HaWe]

hallo,
Glückwunsch, das ist wirklich ein tolles Projekt! 8)
Zur besseren Übersicht: könntest du nochmal kurz die Specs komplett zusammenfassen?

(Mit der Bezeichnung für die Gelenke tu ich mir etwas schwer, bei Beinen spricht man ja von Hüfte und Knie, bei Armen von Schulter und Ellbogen. Ich denke hier handelt es sich um "Beine"...) :

Leergewicht ohne Batterien
Rumpf: Länge, Breite
Beinlänge:
- Oberschenkel
- Unterschenkel+Fuß
- max. Auslenkung (Hüfte diagonal -> Fusspitze)
Tragkraft (Tara)
verbaute Servos (Typ, Drehmoment):
- Hüfte:
- Knie:

Das wär wirklich gut, damit man auch eigene Alternativen und Varianten mal / künftig / evtl besser beurteilen kann!
http://www.mindstormsforum.de/images/smilies/icon_thumbs.gif

[Tomy83]

Mit den Bezeichnungen tu ich mich auch Schwer. Da das Hüftgelenk 2 Freiheitsgrade besitzt habe ich es für mich folgend unterteilet. Den Roboter Vorwärtsbewegen = Hüfte. Den Roboter nach Oben hieven = Oberschenkel. Die Verbindung zwischen Oberschenkel und Unterschenkel ist logischerweise das Knie. Falls es da eine gebräuchlichere Bezeichnung gibt klärt mich bitte auf.

Leergewicht: 3800g
Länge Rumpf: 348mm
Breite Rumpf: 232mm
Länge über Alles (Fuß Vorn/Hinten Maximal): 330mm
Breite über Alles (Fußspitze zu Fußspitze @ Normalabstand): 370mm
Länge Oberschenkel (Drehpunkt <-> Drehpunkt): 80mm
Länge Unterschenkel (Knie <-> Fuß): 152mm
Maximale Auslenkung Bein (Diagonale Fußspitze <-> Hüfte) [@42mm / 25° / Hüftdrehpunkt über Boden 91,7mm]: 135,21mm
Maximale Auslenkung Bein (Diagonale Fußspitze <-> Hüfte) [@90,05mm / 45° / Hüftdrehpunkt über Boden 91,7mm]: 156,93mm
Tragkraft (Statisch, grob gerechnet, Gesamtgewicht mit Akku ca. 4,2Kg): Laufmodus 3-3 = 1,95Kg, 4-2 = 4Kg, 5-1 = 6Kg . Aber aufgrund des vielen Aluminiums und unter Beachtung seines Übergewichts würde ich 0Kg sagen.
Servo Hüfte+Knie: Savöx SC-1251MG, 90Ncm
Servo Oberschenkel: Savöx SC-1256TG, 200Ncm
µC: 2x XMega256A3B (einer davon liest aber z.Zt. nur die Fernbedienung ein) der schafft seine Berechnung (mit viel float) in 4,64 - 11,06ms je nach Rechenaufwand für Geradeaus/Kurve/Kreisdrehung. Da reicht um die Servos mit 70Hz anzusteuern.

Ich hoffe ich hab jetzt alles, falls nicht sag bescheid. :-)

mfg Thomas

[HaWe]

dankeschön, ein super Überblick!
Ich hatte auch ganz übersehen, dass das Hüftgelenk ja auch bei dir 2 Servos/Freiheitsgrade hat:
einmal beug-streck (heben-senken) und einmal seitlich für den Vortrieb vor-zurück (heranziehen-abspreizen)
"Oberschenkel" ist ja kein Gelenk, sondern ein Segment wie Unterschenkel oder Fuss.
Wie die Hüftbewegungen beim Menschen oder Säugetier heißen, ist mir klar, aber was sich in der Robotik eingebürgert hat, weiß ich auch nicht, zumal der Vielfüßler ja im Hüftgelenk quasi seitlich geht.
Ich würde sie vlt HüftHeber und HüftVortrieb nennen.

Von der Tragkraft her:
würdest du heute evtl. stärkere Hüftheber-Servos nehmen, evtl. doppelt so viel Nm ?

[Tomy83]

Ja stärkere Servos wären möglich, dann aber nur mit 7,4V. Aber die sind sehr teuer. Also würde ich statt Alu lieber Plastik oder Dibound verwenden, um das Gewicht zu reduzieren. Auch kleinere Kugellager würden sich positiv auswirken. Falls ich einen zweiten Hexa bauen würde, dann wäre die Beingeometrie anderes als jetzt. Aber so wie es ist funktioniert es, und daher hab ich keinen Grund etwas daran zu ändern.
Mir Schwebte auch mal vor, Schrittmotoren mit Schneckengetriebe zu nutzen. Die wären Selbsthemmend. Aber der Fertigungsaufwand war mir dann zu hoch. Ich hab ja keine Fräsmaschine.

Nur die "Oberschenkel" Servos, zum heben und senken, bekommen jetzt noch einen Temperatursensor spendiert. Voralledingen im Stillstand, bei gleichzeitig angehobenen Körper, werden die ganz schön warm. Zur Zeit Senke ich den Körper, bei keiner Eingabe über die Fernbedienung, nach Zeitkonstante ab.

[HaWe]

kannst du zur Lastminderung für die Heber/Senker nicht Federzüge einsetzen, die das statische Gewicht kompensieren? Dann müssten die Heber/Senker nicht das konstante Gewicht auch im Stillstand und beim Gehen stemmen, sondern nur die Vertikalbewegungen ausführen...? Denn dann wird ja das Gewicht durch die Federn weitgehend "gestemmt/balanciert"...

[Tomy83]

Würde ich das volle Gewicht durch Federn ect. Kompensieren, dann hätte ich doch beim "Rückhub" (Umsetzen der Beine) die volle Kraft gegen die Federn aufzubringen. Dazu ist das Drehmoment sehr dynamisch, je nach Laufmodus und Auslenkung. Und so weit ich weiß sind Federn auch nicht linear in ihrer Kraft. Wäre also etwas komplex da die richtigen Federn auszusuchen.

[HaWe]

mag ja sein, aber immerhin hättest du Beladungs-Kapazität und andererseits brauchst du ja pro Bein nicht das volle Leergewicht / 6 abfedern, sondern nur einen Teil, vlt auch nur hälftig, und dazu kommt, dass die Rückstellungszeit ja nur kurz ist gegen die Belastungs- und Vortriebszeit.
Sicher macht es aber mechanisch keinen Sinn, die Servos volle Kraft leisten zu lassen, nur um das Grundgewicht ständig zu stemmen, bei null Rückstellkraft.
Das wäre das gleiche, als wenn du dich statt zu gehen ständig in Kosakentanzhaltung mit 90° angewinkelten Knien vorwärtsbewegen würdest.

naja, war ja nur ein Tipp für mehr Traglast, und ein paar Zugfedern wären ja keine große Sache....

[Tomy83]

Danke für deinen Tipp. Recht hast du ja schon. :-)
Wenn man es so betrachtet, wäre ein Radantrieb doch viel effektiver. Der muß nur Energie in den Vorschub investieren. Im Tierreich liegen die Reptielen mit seitlich angeschlagenen Beinen auch die meiste Zeit auf dem Bauch. Und die Tiere die immer stehen, haben die Beine unter dem Rumpf.

P.S. Die Rückstellzeit ist in etwa gleich lang wie die Vortriebszeit(im Laufmodus 3-3).

[HaWe]

wie gesagt, war ja nur ein Tipp, aber auch Muskel-und Sehnensysteme bei Tieren haben durchaus eine federartige Vorspannung, und der evolutionäre Schritt, die seitlichen Beine unter den Körper zu bringen und durch die "passive Stützung" die Energie-Effizienz zu verbessern, war ja auch mit einer Erhöhung der Lauf-Effizienz verbunden.

Was die Bewegungssteuerung angeht, interessiert mich aber auch dein "Bewegungs-Algorithmus":

Wie genau sieht dein Progamm aus, wie steuerst du jede Bein-Einzel-Bewegung, um die gewünschte Gesamt-Bewegung des Rumpfes zu erreichen, z.B. bei Kurven oder auf der Stelle drehen und verschiedenen Bogen-Geschwindigkeiten und -Richtungen, auch rückwärts und seitwärts?

[Tomy83]

Was die Bewegungssteuerung angeht, interessiert mich aber auch dein "Bewegungs-Algorithmus":

Wie genau sieht dein Progamm aus, wie steuerst du jede Bein-Einzel-Bewegung, um die gewünschte Gesamt-Bewegung des Rumpfes zu erreichen, z.B. bei Kurven oder auf der Stelle drehen und verschiedenen Bogen-Geschwindigkeiten und -Richtungen, auch rückwärts und seitwärts?

die Frage ist gut... Da könnte man bestimmt ein paar Stunden am Stammtisch rum bekommen. Ich versuch mich mal kurz zu fassen.:confused:

Das Steuerungskonzept sieht bis jetzt so aus: Der 1x µC liest die Potis/Tasten von der Kabelvernbedinung ein. Dort werden die noch mit einer "keine Eingabe erfolgt" Zeitkonstante verwurstet und per UART an den 2. µC gesendet. Also hat der 1.µC z.Zt. nicht wirklich viel zu tun.

Geradeaus (Vorwärts/Rückwerts):
Es wird für jedes Bein die bekannte IST-Position genommen, und die Position des Beins längs zum Roboter neu berechnet. Also -+ 0 bis 1mm pro Schleifendurchlauf, je nach Potistellung. Der Seitliche Abstand bleibt dabei konstant, es sein den dieser weicht vom Soll (Standartwert) ab. Das wird dann beim Umsetzen des Beins behoben.

Kurvenlauf (Vorwärts/Rückwärts):
Aus der Bekannten IST-Position des Beins (im Bezug zur Robotermitte) wird die Globale Position zum Kurvenmittelpunkt errechnet. Für jedes Bein einzeln. Der Kurvenmittelpunkt liegt immer auf einer Achse Vetikal (Seitich) zur Robotermitte. Aus dem UART empfangenen Daten sind Vorwärts/Rückwärts (0 bis 1mm) und der Kurvenradius bekannt. Es wird um "Vorwärts/Rückwärts" die Sehnenlänge verschoben. Und aus den neuen Globalen Position auf jedes Bein einzeln zurück gerechnet. Somit ergibt sich für jedes Bein eine neue Längs- und Seiten- Position.

Um die eigene Achse drehen (Links/Rechts):
wie Kurenlauf nur das der Radiusmitelpunkt in der Mitte des Roboters sitzt.

Seitliches laufen kann er nicht.

Um die Schrittfolge der Beine einzuhalten sind gleich zwei funktionen implementiert. Erstens wird immer jedes Bein um (Vorwärts/Rückwärts) verschoben. Bei Kurvenlauf im Verhältniss des Einzelbein-Radius zur Robotermitte (Strahlensatz). Überschreitet ein Bein z.B. beim Umsetzen sein Grenzwert, wird dieser bei "Bein nicht am Boden" wieder aufgeholt.
Auserdem ist für jeden Laufmodus (3-3/4-2/5-1) die Beinspreizung in % hinterlegt (Konstante). Befindet sich das Bein im Rückhub (Umsetzen) wird die IST-Position in % zur Sollposition in % verglichen. Bei Abweichungen wird das Bein verlangsamt, beschleunigt oder der Rückhub sogar abgebrochen. Ist ziemlich kompliziert implementiert, da es so viele Optionen gibt (3 Laufmodi mit Vor/Rück, Kurve, Kreisdrehung)

Jedes Bein wird ein paar mm vor Ende seiner maximalen Auslenkung (längs zur Robo) bereits vom Boden abgehoben, um bei Richtungsumker des Beins nicht den Roboter abzubremsen. Gleiches gilt für das ende des Umsetzvorgangs. Das Bein nimmt erst wieder fahrt mit dem Roboter auf ehe es auf den Boden aufsezt. Das Spart so ca. 4A, und der Robo läuft um einiges flüssiger.
Die Grenzwert längs zum Roboter werden im Kurvenlauf auf den Beineinzelradius angepasst. Somit ist die Schrittlänge jedes Beines variabel. Geradeaus sind alle gleich. Aber bei Kurvenlauf, werden die kurveninneren Schrittlängen eingekürzt.

So ich denke das ist es jetzt mal ganz knapp, und hoffe ich hab mich verständlich ausgedrückt. Falls nicht werde ich versuchen deine Fragen zu beantworten.

mfg Thomas

[HaWe]

danke für die Info!
Ich meinte allerdings keine Kurzbeschreibung, sondern vor allem auch den genauen Programmcode. Könntest du den bitte auch mal hier einstellen?
vG
Helmut

[Tomy83]

Ob dir mein Code hilfreich ist, wage ich zu bezweifeln. So toll sind meine Fähigkeiten dann nicht. Aber wer weiß vielleicht kannst du mir auch paar Tips geben.
Was hast du mit der SW vor? Rein zum basteln, oder ein kommerzielles Projekt?

[HaWe]

ich selber interessiere mich nur hobbymäßig für Hexapods, besitze aber noch keine Hardware dafür und auch noch keinen Programmier-Plan.

[Tomy83]

Für eine PN ist die Nachricht zu lang. Dateien Hochladen (keine Bilder) schaffe ich hier nicht. Am besten wäre wohl eine "echte" email...

[HaWe]

für welche Plattform (AVR, Arduino, Raspberry) und ggf. in welcher Sprache programmierst du denn (C, C++, Bascom, Python, Java,....) ?

[Tomy83]

Bascom habe ich aufgeben. Ist in C für ATXmega.

[HaWe]

ok, dann wäre es immerhin von prinzipiellem Interesse, je nachdem, wie deine C Libs und API Funktionen aussehen, denn ich verwende nur Sketch für Arduino und C plus Geany für den Raspi. Aber vlt kann man sich ja was abgucken.
Den C-Sourcecode einfach reinkopieren geht nicht?

[Tomy83]

Es werden die maximalen Zeichen überschritten. Und dein Posteingang ist voll.

Ich nutze das Atmel Studio. Vom C her müsste das auch gehen. HW muss sowieso angepasst werden. Aber ich denke auf dem Adurino müssen die float Berechnungen gegen int ersetzt werden.

[HaWe]

float gegen int ?
wieso?
ich benutze meist den Arduino DUE (ARM Cortex M3, 84Mhz, 92kByte RAM, der arbeitet sogar mit 64bit double), ganz selten auch noch mit dem Mega2560 (8kByte RAM, schafft aber immerhin 32bit float).

Wundert mich, dass der Code zu groß ist für das Forum - mein Postfach aber ist nicht voll, kann nur sein, dass da auch der Code nicht reingepasst hat.
Ich finde beides hier im Forum bei weitem viel zu extrem eingeschränkt. Bei uns im Forum sind 64kByte Zeichen oder Code als reiner Text pro Post möglich, Uploads im Post >10MByte.

Aber vlt kannst du ja wenigstens ein paar Code Beispiele / Snippets posten, damit ich einen groben Eindruck von deiner Bewegungssteuerung kriege...?

[Tomy83]

Ups Ardurino DUE da hab ich was verschlafen. Der ist dann doch um einiges schneller. Hat der eine FPU?

ich versuchs heut Abend nochmal mit dem Code posten.

[HaWe]

Nein, der Due hat keine fpu, aber ist auch so ~10x so schnell wie ein AVR (84 vs. 16MHz) (berichtigt)
Erst der Teensy 3.1 mit ARM Cortex M4 hat eine fpu, aber dafür mit 72 MHz etwas weniger Taktfrequenz - unterm Strichg kaum ein Unterschied.

[Unregistriert]

Nein, der Due hat keine fpu, aber ist auch so 10x so schnell wie ein AVR (84 vs. 8MHz) ..Die meisten AVRs, selbst Tinys, laufen mit bis zu 20 MHz, einige sind bis 16 MHz spezifiziert. Ich habe erst letztlich nen nanoclone mit mega328 auf 20 MHz aufgebohrt.

[HaWe]

sorry, hatte mich vertippt beim Ergänzen: ich meinte 16MHz bei AVrs und bezog mich auch nur auf die Arduino AVRs. Habe es berichtigt. Trotzdem ist der Due ~10x so schnell wie z.B. ein Mega2560, aber das soll jetzt hier nicht Thema sein.

[Tomy83]

Nun ich nutze ja nur einen ATXmega der macht 32MHz, soll sich sogar bis 48MHz takten lassen.

[Ceos]

Tomy83 wenn du dich mit der Magie der DMAs beschäftigst, wirst du erst erfahren wie MÄCHTIG die XMegas wirklich sind :D

PS:
wenn du einen exakten, driftfreien Takt brauchst, stell den Controller auf 32MHz PLL Compensation Mode (die PLL taktet gegne den internen 32kHz RTC) ... wir haben das Ding qualitativ mal vermessen, der macht weniger als 0.3% Fehler über einen Temperaturverlauf von -40°C bis +85°C

[Mxt]

Erst der Teensy 3.1 mit ARM Cortex M4 hat eine fpu, aber dafür mit 72 MHz etwas weniger Taktfrequenz - unterm Strichg kaum ein Unterschied.
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).

[HaWe]

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.

[Tomy83]

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."

[HeXPloreR]

Hallo Tomy83,

das hört sich nach einem "Dreiteiler" an ;)

Viele Grüße
Jörg

[Tomy83]

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.
31901
31902

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.



//-------------------------------- 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_Of fset,L_Bein_Horrizontal_Offset,L_Bein_Horrizontal_ Offset,L_Bein_Horrizontal_Offset,L_Bein_Horrizonta l_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))*1 80/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))*1 80/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_Be in/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_Horriz ontal_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_Horriz ontal_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))*1 80/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)+sq uare(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)+sq uare(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_Horrizont al_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_Horrizont al_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.

[HaWe]

aber gern! 8)

[Tomy83]

//------------------------------- 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

- - - Aktualisiert - - -

bitte noch einmal etwas nutzlosen posten

[HeXPloreR]

aber na klar ;)

[Tomy83]

//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 --------------------------------------------



- - - Aktualisiert - - -

das waren dann alle 3 Teile :-({|=

Die Funktion "Bein Berechnungen" wird dann in der Hauptschleife 6 mal aufgerufen. Von 6 bis 1, das liegt daran das die Synchronisation der Beinspreizung (Winkel/Stellungen zu einander) vom Bein 6 aus beginnt.
Ich weiß das man es bestimmt geordneter schreiben kann, aber für mich war das der Weg meines 1. C Projekts.

[HaWe]

danke, aber - puh - das ist ja deutlich, ja sogar extrem mehr Aufwand als ich je vermutet hätte!

[Tomy83]

Die reine Beinbewegung, lokal auf ein einzelnes Bein betrachten sind nur die letzten ~20Zeilen der Funktion (Beinberechnungen). Der große Rest ist dann wann welches Bein Wo und wann sein soll.
Bestimmt gibt es noch elegantere Wege, bloß den kenn ich auch nicht. Ich halte selbst auch nicht viel vom Abschreiben, wenn dann will allein hin bekommen. Aber irgendwo braucht man immer einen Anfang.

[HaWe]

ja klar, ich finde es schon bemerkenswert wie du das gelöst hast!

[Tomy83]

Die Nabelschnur ist jetzt ab. Die Stromversorgung läuft über ein SBEC (angeblich bis 20A). Und für die Fernsteuerung habe ich einen alten Playstation Dualshock2 Controller geschlachtet. An dem ist aber auch nur noch das Gehäuße orginal. Die Stromversorgung wurde mit einen Miniakkupack (4xNimH) realsiert. Controller ist ein ATXmega128A1u, der etwas überdimensioiert ist. Datenübertragung bewälltigen 2 BTM220a.

Hat jemand erfahrung mit den Bluetoothmodulen? Ich habe eine Verzögerung von ca. 1Sekunde (geschäzt), zwischen Eingabe an der Fernbedieung und der umsetzung im Roboter.

31992 31993

[HaWe]

welche BT Module?

[Tomy83]

BTM220a steht drauf. Vermutlich von Rayson. Zumindest das Datenblatt vom BTM222 hat gepasst. Es handelt sich um Bluetooth Module.

[HaWe]

ok, die kenne ich nicht, ich hatte dir ja schon mal die HC-05 empfohlen, damit klappt es bei mir rasend schnell und ohne Verzögerung, seriell mit 115000 baud, als wäre nur ein Kabel dazwischen.

[Tomy83]

Die BTM220 hatte ich schon da. Ich dachte, da BT Prinzip bedingt Paketweise überträgt, könnte es daran liegen. Und eventuell hatte jemand schonmal diese Module verwendet. Die Baud hoch setzen könnte/werde ich nochmal probieren.

[HaWe]

meine BT Module werden einfach mit UART verbunden und können sowohl einzelne Bytes als auch Pakete als auch einen iostream übertragen, eben genau so wie es aus UART "rauskommt", sowohl synchron als auch asynchron. Ich verwende dazu ein optimiertes Serial Protokoll, das im Handshake abwechselnd Daten in 2 Richtungen überträgt (remote control und Daten auch zurücklesen, vor allem auch um eine Rückmeldung über korrekten Empfang erhalten zu können (Transmission Control, Checksum, Acknowledge mit Checksum-Rückmeldung). Außerdem wird in einen Puffer gelesen, sodass bei Verbindungsunterbrechung sowohl ein timeout als auch eine Wiederaufnahme mit einem Rest des bereits im Puffer befindlichen Teils vor der Übertragung weitrverwendet wird, was das erneute Einlesung des bereits übertragenen Teils unnötig macht. Da es ein reines UART Protokoll ist, funktioniert es ntl auch, wenn man die RX/TX Kabe durch die BT-Funkstrecke ersetzt, den Rest für die Wirelss Sache machen die Module eigenständig. Hier ist der Link zu meinem Code:
http://www.mindstormsforum.de/viewtopic.php?f=78&t=8491&start=15#p67476

[Tomy83]

Mein selbst erfundenes UART Protokoll (Ping Pong Spiel, ähnlich deinem) wollte anfangs nicht laufen. Da kamm nur Datenmüll raus. Im Nachhinein hatte ich einen Fehler in meiner Routine die den ADC-Wert verrechnet. Also wartet jetzt die Fernbedienung (BT Master) auf einem Empfang vom BT Slave. Danach startet kontinuierlich die Übertragung. Beginnend mit 2 Byte um den Anfang der Sequenz zu dedektieren. gefolgt von 4 x 2 Byte für die ADC (4 Kanäle a 11bit), da wäre noch platz um das ein oder Andere Bit mit unterzuschieben. Danach kommen noch 2 Byte von der Tastatur-Matrix. Zum Schluß kommen nochmal 2 Byte mit der Quersumme, damit prüft der BT-Slave die richtigkeit der Daten und gibt diese im Programm frei. Mit den Status-Ausgängen am BT-Modul dedektiere ich die aufgebaute BT-Verbindung.

Ich muß jetzt nur noch im Empfänger feststellen das die Daten nicht schon veraltet sind. Wenn die Verbindung gestört wird, dauert es eine Weile ehe mir das BT-Modul das per Hardwarepin meldet. Da werde ich einfach schauen ob aller paar Millisekunden die Empfangs-ISR aufgerufen wird.

Gesendet wird immer bei leeren Sende-Register mittels ISR. Da die Tastatur nur alle 90ms fertig ausgelesen ist, sollten mir ca. 10 Übertragungen pro Sekunde reichen.
(14 Byte = 112 Bit | + 14 Stopbits = 126 bit | 11 Übertragungen * 126 bit = 1386 baud) Stimmt das so?
Eingestellt habe ich 57600 baud.



Zum Thema "Der Sender prüft die zurückgesendeten Daten vom Empfänger":
Das hatte ich in meiner "Ping Pong UART Übertragung" mal getestet. Festgestellt habe ich das ich nie eine Fehlübertragung hatte, solange Sender und Empfänger eingeschwungen sind. Deswegen sendet bei mir immer der Empfänger ein O.K. und erst dann beginnt der Sender mit der Übertragung.


Ich werd wohl nochmal prüfen ob wirklich die geänderten Daten so schnell der UART-ISR bereit stehen.

vielen Dank für deinen Code.

[HaWe]

ps,
meinen Grundcode habe ich dann auch für eine Fernsteuerungs-Anwendung zwischen einem Arduino (RemoteControl) und einem Raspi (Kettenfahrzeug) verwendet:
http://www.mindstormsforum.de/viewtopic.php?f=78&p=69089#p69042

Wie gesagt, funktioniert bei mir fehlerfrei und pfeilschnell (fehler passieren aber immer, nur kriegst du sie meist nicht mit, oder nur wenns zu spät ist. UART ist extrem störungsanfällig. Daher mein Transmission Control Protokoll. Aber du kannst ntl programmieren, so wie es dir am liebsten ist ;) )

[Tomy83]

so und nun die Ursache und Lösung der Latenz in der Bluetooth-Verbindung.

Am BT-Sender hatte ich das UART-Empfangsregister nicht ausgelesen, und somit wurde das Flag "Empfang Vollständig" nicht gelöscht. Somit hatte mein Programm immer sofort mit der Übertragung des nächsten Strings gestartet. Anfänglich betrug die Latenz ca. 200ms, und steigerte sich bis auf 1Sekunde. Dann brach die BT-Verbindung ab. Vermutlich war dann das Empfangsbuffer/Sendebuffer in den BT-Modul voll. Mit einem delay in der Senderoutine klappte es dann. Endgültig reicht es aber zu warten bis der BT-Empfänger ein "OK alle Daten empfangen" sendet. Jetzt kann ich eine Latenz zwischen 24ms bis 64ms messen.

[morob]

welche servos hast du nun genommen?

[Tomy83]

Hallo, dort das ich jetzt erst schreibe. Aber ich bin voll im Hausbau beschäftigt.
Welche Sensoren meinst du?

[morob]

ich fragte nach servos, nicht sensoren :D ich weiss hausbau ist stress :D

[Tomy83]

...hab ich mittlerweile auch bemerkt.
Für Hüfte und Knie sind es die Savöx SC-1251MG. An den Oberschenkeln (Schulter) Savöx SC-1256TG.
An für sich ist die hohe Geschwindigkeit sehr gut für eine flüssige Bewegung. Siehe https://youtu.be/hAVR7bIsoD0
Aber die Oberschenkelservos leiden an Überhitzung, bedingt durch Übergewicht. Mittlerweile überwche ich diese mittels DS18B20.
Stromhunrig sind diese auch noch. Die Spannung ist dank 6x8200µF low ESR nach dem SBEC stabil, aber vor/in dem SBEC wird es wörtlich "hörbar" und auch messbar das die Spannung viel zu stark schwank. Dies ist sehr stark Lastabhänig, die letzten Werte vom Netzteil lagen so zwischen 7-12A Dauerstrom. Auf dem Oszi wird es vor dem SBEC sehr unschön. Wie lange genau der Akku (2200mAh) durchhält habe ich noch nicht exakt gemessen, aber es sind so ca. 10-15 min.

Das letzte was ich gemacht habe, waren passende Sensoren für die Geleändevermessung auszuprobieren. Bei den Ultraschall-Sensoren (HC-SR04) ist mir der Erfassungswinkel viel zu groß. Mit den Sharp-IR-Sensoren (2Y0A21) wird es nicht viel besser. Daher werden, wenn Zeit ist, die Lasersensoren (VL6180X & VL53L0X) ausprobiert, in der Hoffnung das diese sehr Punktuell und schnell messen. Schnell ist ja relativ, ich schätze mal das 10Hz reichen.

[morob]

danke für die antwort
ich überlege gerade an einem biped zu konzepieren (excel), masse und belastungen.