.: Vinculum :. - Hexabot

[Geistesblitz]

Welche Bahnkurve meinst du jetzt? Das Vorranheben des Beines oder meinst du allgemein?

[HannoHupmann]

Das was du meinst ist die inverse Kinematik, die ist nun nahezu vollständig implementiert. Was ich jetzt suche ist die sogenannte Bahnkurve. D.h. die Trajektorie bei der sich der Roboter von A nach B bewegt. Also von einem Ausgangspunkt zu einem Zielpunkt und diesen Weg muss ich irgendwie in der Mathematik abbilden.

- - - Aktualisiert - - -

Das was du meinst ist die inverse Kinematik, die ist nun nahezu vollständig implementiert. Was ich jetzt suche ist die sogenannte Bahnkurve. D.h. die Trajektorie bei der sich der Roboter von A nach B bewegt. Also von einem Ausgangspunkt zu einem Zielpunkt und diesen Weg muss ich irgendwie in der Mathematik abbilden.

[HeXPloreR]

Ich würde es versuchen so zu lösen das man den Anfangspunkt und den Zielpunkt (A-Z) als Variablen, dann den Weg dazwischen in die kleinste Einheit zerlegen die die Beine sich bewegen können und dann solange auf den Anfangstpunkt aufaddieren bis man den Zielpunkt erreicht hat. Sollte auch für Koordinaten funktinieren. Habe ich selbst sovor, aber noch nicht umgesetzt. Daher ist es erstmal nur ne Idee.

[HannoHupmann]

Meine Gedanken gehen in eine ähnliche Richtung. D.h. ich habe eine Aktuelle Ist Position definiert durch x=0, y=0, und Drehwinkel alpha = 0° und eine Zielposition mit z.B. x = 150cm, y = 150cm, alpha 45°. Damit müsste der Roboter dann 150cm nach vorn und 150cm nach rechts laufen (translatorisch) und dabei die Orientierung des Körpers um 45° drehen (rotatorisch). Soweit so gut. Einziges Problem was ich bisher noch habe ist, wie ich die Wegstrecke sinnvoll in Schritte unterteile, denn die Zielpositionen können unterschiedlich weit entfernt sein und daher in variablen Anzahl von Schritten erreicht werden, die Anzahl gilt es irgendwie sinnvoll zu bestimmen.

[Arkon]

Mir fehlt es an Erfahrung in diesem Bereich aber manchmal können Ideen von "Unwissenden" ja ein Anstoß für die eigene Lösung sein ;)

Als Bezug würde ich das mittlere Beinpaar wählen, um die Bewegungen des vorderen, bzw. hinteren, Beinpaares davon abzuleiten.

Den Bogen, den das kurvenäußere Bein zurücklegt, würde ich in Bogensehnen unterteilen. Die Länge dieser Sehnen könnte die maximal zurücklegbare Strecke des Beins bei einer Bewegung sein, aber kleiner um auf den Untergrund angepasst eine bessere Mobilität zu erziehlen (auf ebener Fläche sind keine Steine oder Löcher zu erwarten).

Die Länge der ermittelten Sehnen würde ich im Verhältnis äußere Kurvenbahn zu innerer Kurvenbahn verkürzen um mit der gleichen Anzahl Schritten den benötigten Weg zurück zu legen. Wenn ein Mensch um eine Kurve läuft hebt er ja auch immer abwechselnd die Füße und macht unterschiedlich große Schritte.

Auf Basis dieser Bewegungen müsste man dann die Bewegungen der verbleibenden vier Beine errechnen. Ich denke, es sollen immer drei Beine Bodenkontakt haben (Mitte links + Vorne/Hinten Rechts oder umgekehrt)

[HannoHupmann]

Hallo Arkon,

was du beschreibst - und auch völlig richtig - ist die weiter Führung der Inversen Kinematik zu einer einfachen Bein-Trajektorie. Was allerdings noch fehlt ist die übergeordnete Bahnplanung oder um es weniger technisch auszudrücken: der Weg.

Hier mal meine Lösung, wie ich mir das Vorstelle:
26426

Nur leider wird er bei einer überlagerten translatorischen und rotatorischen Bewegung keine Kurve laufen, sondern einfach schräg mit einer entsprechenden Drehung des Körpers.

[Arkon]

Also vereinfacht die Bahn, die ein Punkt(z.B. Schwerpunkt oder Drehpunkt) auf der Basis zwischen Start und Ziel verfolgen soll?

Dann müsste doch die Distanz nicht als Gerade (Pythagoras -> Gleichschenklige Weiber, her damit!) sondern als Bogen berechnet werden. Für die Berechnung eines Punktes auf einer Kreisbahn wäre das entsprechend http://upload.wikimedia.org/math/f/a/d/fad7aa0285bc7a8b6905e3fb8724803f.png Die Bogenlänge geteilt durch den maximal zurücklegbaren Weg pro Schritt würde dann die Anzahl der benötigten Schritte ergeben.

Sollte ich nur Blödsinn oder unnützes Zeug schreiben wink doch bitte kurz mit dem Zaunpfahl. Will nicht mit unqualifizierten Aussagen nerven ;)

[HeXPloreR]

Ich denke auch der Lösungsanstz ist der Richtige, nur sollte man vermutlich eine Kreisbogenbahn, wenn man Kurven läuft, in viele kleiner Punkte zerteilen... und diese Punkte dann mit Geraden verbinden, damit man die Steigung einfach herausbekommt.
Man kann natürlich auch den Kreisbogen mit Sinus/Cosinus in verschiedenn (aktueller) Winkeln bearbeiten damit man einzelne Punkte herausbekommt. Dazu muss man aber einen Drehpunkt setzen.

Das der Roboter nur schräg läuft mit Drehung des Körpers halte ich für ein Gerücht: Auch hier gilt, er macht das was man ihm sagt.

Wenn er schräg läuft hat man ggf nicht die richtige Bahnberechnung ausgewählt.
Wenn es verschieden Zielpositionen die aber für den gesamten Roboter (Mittelpunkt, Centrum ...wie auch immer...) gibt, dann sind diese alle nacheinander zu berechenen und anzulaufen.
Wenn die Strecken unterschiedlich sein können, dann gibt es mehrere Wege nachdem man die Strecke A-Z des Körpers und der Beinkurve kennt:
1.die Bewegung der Beine anpassen, also kürzer oder länge Schrittweite.
2 die Bewegung mit vordefinierten fester Schrittweite und am Ende der Strecke den Rest des Weges ermitteln der nicht mehr in einen vollen Schritt passt. Das bedeutet das jedes Bein das letzte zu bewegbare Bein sein kann. Auch sollte man bedenken, das der Körpermittelpunkt nicht unbedingt auch den gleichen Abstand zu allen Fußpunkten haben muss - also nicht unbedingt der wirkliche Zielpunkt sein muss. Man könnte damit also auch noch etwas schieben und die Füße ggf extra nachberechnen wenn einen die Position eines einzelnen Fußes nicht gefällt. Da gilt es dann die erlaubte Toleranz einzustellen wo der Zielpunkt liegen darf, damit man mit den Beinen den Körper ggf letztendlich aufs Ziel schieben kann.

Ich verstehe Dein Bild mit dem unteren IF - Else nicht recht. Warum ist translatorisch zu rotatorsch getrennt? Wozu ist das gut?

Das Problem was ich hier sehe ist, das es nichts damit zu tun hat ob ein Betrag der Schritte je translatorisch zu rotatorisch größer als der andere ist?
Dazu müsstest Du nochmnal was sagen.
Der Winkel kann auch nicht einfach vorggeben werden, jedenfalls nicht wenn man auch gleichzeitig Koordinaten vorgibt - denn man weiß ja eigentlich garnicht ob der Winkel zu den Koordinaten passt, ob also die Gerade den Zielpunkt schneidet. Man kann nur eines von den beiden vorgeben, wo bei Vorgabe eines Winkel auch noch die Länge (Vektoren) der Strecke angegeben werden sollte, damit die Bewegung dort beendet bzw auch die Schrittzahl berechnet werden kann. Sonst läuft er eben weiter bis "was neues" kommt, dazu muss die maximale Schrittweite por Bein also zwingend vorhanden sein.

Also man erechnet sich Koordinaten aus Winkel + Länge oder den Winkel + Länge aus Koordinaten.

Ein Frage stellt sich mir noch: Welchen Schrittmodus benutzt Du als Grundlage 3x3, 4x2, 5x1? Mischung?
Meine Idee ist vielleicht kommt es zu einem Denkfehler wenn man sich auf eine bestimmte Schrittart einschiesst ala "ein Schritt muss immer genau so lang sein". Ich denke da liegt eher der Knackpunkt des Problems.

Um nochmal auf die sinnvolle Anzahl der Schritte zurück zu kommen: Die liegt zwischen einer Definition von maximaler Schrittlänge pro Schritt/Schub bis nicht bewegt.
Lege die maximale Schrittlänge fest, dann kannst du erst berechnen wieviele Schritte mindestens nötig sind.
Lege die minimale Schrittlänge fest, dann kannst Du berechnen wieviele Schritte maximal nötig sind.
Wichtig ist hier zu unterscheiden ob eine Kurve oder eine Strecke gelaufen werden soll.
Und auch ob die Koordinaten (kurze Strecken) mit Körperverschiebung nur mit Schub ohne Schritt zu erreichen sind und eine nachfolgende Korrektur der Fußpunkte gewünscht ist.

Das sind mal son paar von meine Gedanken dazu. Ich hab die Lösung auch noch nicht so richtig fertig - und solange kommt in meinem eigenen Beitrag auch nichts neues ;)

[HannoHupmann]

Es gibt grundsätzlich zwei Dinge zu unterscheiden:

1) Die Bewegung der Fußspitze über den Boden
2) Die Bewegung des Hexas durch den Raum.

2 folgt aus 1 bzw. 2 ist eine aneinanderreihung von Schritten (1).

Zu 1) Es gibt hier einmal eine translatorische Bewegung, dabei ändert sich die Ausrichtung des Hexas nicht. Soll heißen, er zeigt z.B. immer nach vorn und läuft dann eben Schräg (der Ego-Shooter-Spieler spricht von Strafen). Soll sich die "Blickrichtung" ändern, dann braucht es eine rotatorische Bewegung. Beide sind jedoch bei einem 3DOF Hexa unabhängig voneinander. D.h. er kann sich um jeden beliebigen Punkd drehen drehen oder eben schräg in alle Richtungen laufen.
Natürlich könnte man auch beide Bewegungen überlagern, dann kommt dabei raus, dass der Hexa z.B. 2m nach vorn läuft und sich dabei einmal um die eigene Achse dreht (Drehwinkel 360°, vereinfach um den Mittelpunkt des Hexas) d.h. zwischendurch würde er rückwärts laufen. Theoretisch möglich, aber schwer zu implementieren. Weiter müssen hier die ganzen Berechnungen zur Schrittlänge und ob sich die Fusspitze nun auf einem Bogen oder einer Geraden bewegt berechnet werden.

zu 2) Ausgehend von einem Start Punkt auf dem man ein Koordinatensystem legt (x und y, wie in meiner Zeichnung) und einem Zielpunkt der ebenfalls ein Koordinatensystem bekommt und einen Winkel. Warum? Nun bei der Berechnung von Industrierobotern hat man ein Basiskoordinatensystem (dort ist der Roboter befestigt) und ein Tool-Koordinatensystem (dort ist z.B. der Greifer). Es gibt eine Ist Position des Greifers und eine Soll Position des Greifes, die mit 6 Punkten angegeben wird, x,y,z und alpha, beta, gamma, da Greifer in allen Lagen orientiert werden können. Die Steuerung rechnet nun aus, wie aus der Ist, die Soll Position erreicht werden kann...DH-Matrix und so.
Zum Glück habe ich keinen Sechs-Achs-Roboter sondern einen deutlich einfacheren: Z-Achse gibt es nicht nur nur x und y (die translatorischen Bewegungen). Bei der Rotation sieht es noch einfacher aus, hier gibt es kein alpha und beta (solange der Hexa weiter parallel zum Boden ist) sondern nur um die Z-Achse = Gamma. Daraus ergibt sich eine Verschiebung in x und y Richtung plus eine Drehung. Vereinfacht man das oben gezeigte Beispiel indem man den Drehwinkel weg lässt, dann wird der Roboter einfach schräg durch den Raum laufen. Mit dem Drehwinkel 45° muss er sich auch noch drehen dabei. Allerdings wird er keine schöne Kurve laufen, wie man das bei einem Auto erwartet, sondern eben schräg zur Seite und dabei eine überlagerte Drehung ausführen. Diese Bahn zerlegt man dann in viele kleine Teilstücke und lässt sie den Roboter ablaufen.

Hier gleich die Antwort auf die Frage von HexPlorer: Gibt man z.B. vor: "Bleib auf der Stelle stehen aber dreh dich um 180°" würde ich keinen einzigen Schritt nach vorn oder hinten brauchen, aber eben x-Schritte für die Drehung. Gleiches gilt für: "Lauf 10 Schritte gerade nach vorn, aber ohne Drehung" wieder hätten wir einmal 10 Schritte und einmal 0 Schritte. Die Abfrage IF/ELSE soll einfach die Anzahl der Schritte auswählen, die größer ist und dann ausführen. Im ersten Fall würden sich dx und dy = 0 und damit keinen Beitrag zur Bewegung der Fusspitze haben. Nur Winkel = 180° wird sich auswirken.

Zu den Schritten an sich:
Grundsätzlich bevorzuge ich ein 5x1, da es deutlich stabiler ist als 3x3 wenn auch langsamer. Vielleicht implementiere ich auch, dass bei geraden Strecken ohne Rotation ein 3x3 erlaubt ist. Für den Anfang ist 5x1 einfacher.

Richtig, maximale Schrittlänge wird durch die Mechanik bestimmt. Muss ich mal noch ausrechnen, welche Länge hier möglich ist bzw. welche Länge sinnvoll. Sollte die Strecke zwischen Ist und Ziel kleiner als 1 Schritt sein, dann kommt meine +1 in der Berechnung zum tragen (übrigens auch, wenn der Rest zum Ziel nicht ganz passt, dann wird eben nochmal ein Schritt mehr gemacht). Damit erreicht der Hexa zwar nicht exakt die Position, aber alle Beine bewegen sich und stehen danach wieder stabil.


Bisher habe ich mich nicht daran gewagt eine richtige Bahnkurve zu berechnen, warum:
Exkurs zum Thema Bahnkurven:
Nehmen wir als Beispiel ein Auto fährt um eine 90° Kurve. Zu Beginn steht das Auto mit der Ausrichtung Norden, am Ende steht das Auto mit der Ausrichtung Osten. Durch den Winkel der Vorderräder fährt es eine Kurve, solange der Radius dieser groß genug ist. Bei zu kleinen Radien, funktioniert es gar nicht, bei sehr großen Kurvenradien wird es ein sehr kleiner Lenkwinkel. Soweit dürfte das jedem klar sein. Bezeichnen wir diese Kurve als Bahnkurve und versuchen wir sie in der Mathematik abzubilden, wird es ein wenig komplex, denn wir müssen wissen wie groß der Radius ist und wo der exakte Mittelpunkt des Kreisbogens liegt. Für Teilstücke auf einem Kreis - wie bei einer 90° Kurve - ist die Bahnkurve sehr leicht mit einem Bogensegment (siehe Formel von Arkon) zu berechnen. Bei meinem letzten Hexa konnte ich die Koordinaten Kreismittelpunkts vorgeben und wieviel Grad das Kreissegment hat, eine reine rotatorische Bewegung.

Dieses Beispiel ist trivial, denn es hat nur einen Kreis den es zu berechnen gilt. Will man aber z.B. die Strecke von mir bis zu euch als Bahnkurve darstellen (vereinfacht nehmen wir eine Strasse), dann wird es eine Unmenge an Kreisen mit unterschiedlichen Radien und Mittelpunkten geben die aneinandergereiht werden, bzw. in manchen Fällen sogar eher etwas wie ein Polynom sein. Diese Bahnkurve mathematisch zu berechnen und dann in Teilstücken als einzelne Schritte abzulaufen dürfte noch mal um einiges mehr Aufwand bedeuten.

Zurück zum Auto und der 90° Kurve und wie mein Hexa diese Strecke bewältigen würde:
- Der Hexa würde einfach Schräg zum Zielpunkt laufen und sich dort um 90° drehen bzw. bei Überlagerung der beiden Bewegungen: eben schräg laufen und sich dabei drehen. Solange kein Hauseck im Weg ist kein Problem!
Praktisch: Egal wie klein der Radius des Kreises ist, der Hexa schafft es, da er auch auf der Stelle drehen kann.
Praktisch 2: Er wählt den kürzesten Weg um das Ziel zu erreichen.
Unpraktisch: Er bleibt am Hauseck hängen.

- - - Aktualisiert - - -

Und weils so schön ist, hier meine persönliche Aufgabenstellung.

Folgende Bahn soll im Hexabot implementiert werden:
26432

Erlärung: Der Roboter startet bei Pos 1 mit Ausrichtung nach rechts. Läuft einer Kreisbahn bis zu Pos 4. Verändert dann die Ausrichtung zum Mittelpunkt Pos 5. Diese Bewegung von 4 zu 5 ist eine überlagerte translatorische und rotatorische Bewegung. Danach bewegt sich der Roboter auf dem Kreisbogen mit Ausrichtung auf die Mitte. Ab Pos 8 schräg Richtung Pos 10. Alternativ kann auch hier wieder eine überlagerte Bewegung implementiert werden, dann is die Ausrichtung des Roboters am Ende (Pos 10) parallel zur Geraden.

Bisher kann mein Phoenix² nur entweder translatorisch oder rotatorisch:

http://www.youtube.com/watch?v=SEEkO_U-VHM

Siehe 1:02 bis zur Flasche laufen, dann um die Flasche drehen mit Ausrichtung auf die Flasche.
Ab 1:47 translatorische Bewegungen gerade aus, schräg, rückwärts.

Vereinfacht könnten man von Pos 8 zu Pos 10 auch mit 1 Schritt trans, 1 Schritt rota, erreichen.

[HeXPloreR]

Also Hanno,

ich denke wenn man eine Gerade oder einen Bahnkurve läuft, dann zählt alleine der nächste Punkt der von den Beinen angefahren werden soll.

Eigentlich ist es sogar egal ob der Bot eine Gerade läuft oder eine Kurve, wenn man die Kurve in kleine Strecken unterteilt "merkt" der das nicht mal wirklich.
Die Ausrichtung wohin der Roboter läuft hängt davon ab, ob sich das Koordinatensystem gedreht hat oder nicht. Das dreht sich also nur, um im Beispiel von mir zu bleiben, wenn man ihm einen Winkel vorgibt der der nächsten Steigung des nächsten Geradensegments entspricht. Jetzt dreht man das Koordinatensystem des Roboters dem nächsten Winkel entsprechend. Wenn man die Winkel Änderung danach wegschmeisst hat man keinen Bezug mehr zum Urschprung, deswegen muss der mindestens mit der bisherigen Winkeländerung aufgerechnet werden. Das würde die Bahnkurve der Körpermittelpunktes ergeben.
Daneben muss aber für jedes Bein auch eine Berechnung durchgeführt werden die die Beine zur Körperbewegung mitnimmt- also in die IK gegegben wird - da wäre dann auch der Abstand des Beines von Körpermittelpunkt wichtig. Jetzt stellt man entweder jedem Bein eine eigene Kurve für die Fußpunkte bereit, oder lässt je eine Beinseite auf nur einer Kurve laufen. Die beiden Kurven teilt man auch in die Anzahl der Segmente der Körperkurve.... wobei ich grade beim schreiben merke, die braucht man dann wohl auch nicht mehr... - also es ergeben sich mindestens zwei Kurven eine mit kurzen, eine mit langen Segmenten, die Segmentschnittpunkte werden mit Geraden verbunden und diese langen Geraden dürfen nun nicht länger sein als die maximale Schrittlänge eines Beines.
Wenn man jetzt den Winkel zum Zielpunkt berechnet hat, kann kann man darüber auch die Geradenteilstücke berechen entweder mit Drehung oder ohne, wichtig ist nur das die Beinberechnung davon weiß ob ein Drehwinkel vorliegt oder nicht - also ob schräg gegangen werden soll oder pro Geradensegment um den anteiligen Winkel mitgedreht wird.

Fazit: Zum Laufen würde ich zwei Geradensegmente aus n-Segmenten einführen, auf denen sich die Fußpunkte befinden, eine Radius wird bestimmt je nachdem wie stark die Kurve sein soll <= 90°; der Abstand des Fußpunktes zum Körpermittelpunkt wird bestimmt um den Abstand der Fußkurve vom Körpermittelpunkt und damit die Länge der Geraden auf der inneren sowie Äusseren "Kurve" zu erhalte.

Zeichen mal eine Kreis auf Deinen Koordiantensystem oben, mit einem Schnittpunkt im Startpunkt und einem im Zielpunkt - dann zeteilst Du das kürzer Kreissegment gleichmässig mit Geraden. Der Winkel alpha wird automatisch pro Gerade anteilig mit geteilt.

[HannoHupmann]

Eigentlich ist es sogar egal ob der Bot eine Gerade läuft oder eine Kurve, wenn man die Kurve in kleine Strecken unterteilt "merkt" der das nicht mal wirklich.
Schon richtig, aber irgendjemand muss ihm ja sagen, wohin der nächste kleine Schritt gehen muss. Also kommt wieder die Bahnkurve ins Spiel.

Worauf ich im Prinzip hinaus will lässt sich ohne Probleme als Mensch machen. Geh einfach zur Türe des Zimmers und dreh dich dabei einmal um 360°, danach läufst du mit Blick zur Wand zurück zum PC. Jeder bekommt das hin, sich durch den Raum zu bewegen und dabei um seine Achse zu drehen oder quer zu laufen mit dem Gesicht in eine bestimmte Richtung. Genau das Gleiche geht mit einem Hexa auch, wenn die Mathematik dahinter gut genug ist.


Jetzt stellt man entweder jedem Bein eine eigene Kurve für die Fußpunkte bereit... Darauf läuft es hinaus und die Mathematik dafür gilt es zu entwickeln.

...oder lässt je eine Beinseite auf nur einer Kurve laufen
Wenn ich dich richtig verstehe, willst du in etwa das implementieren was auch ein Panzer macht. Er bewegt eine Seite schneller als die Andere (nur eben Ketten statt geraden Stückchen) bzw. bei dir länger und kürzer. Finde ich persönlich nicht besondern effizent, wenn man Beine hat. Bei meinem Phoenix² hatte ich bereit für jeden Fuss eine eigene Kurve, alles andere reduziert die Möglichkeiten doch schon sehr.

Im Prinzip ist meine Steuerungen für den Hexa in mehreren Ebenen angelegt:
1a Ebene: Inverse Kinematik: Es werden aus der Vorgabe die Winkel der Servos alpha beta gamma berechnet
2 Ebene: Inverse Kinematik: Position der Fussspitze im Raum bestimmt durch f(x,y,h)= alpha, beta gamma übergeben an die Ebene 1.
2 Ebene: Inverse Kinematik: Lage des Körpers, kippen um die Längs und die Querachse f(Q, L)=dh und Übergabe der daraus resultierenden Höhenänderung
3a Ebene: Bahnplanung Fusspitze: Schritt: Vorgabe der Kurve auf der die Fussspitze entlang läuft f(dx, dy, ddh) mit Übergabe an Ebene 2
3b Ebene: Bahnplanung Fusspitze: Rückführung der Fussspitze auf die Anfangsposition f(dx, dy, ddH)
4 Ebene: Bahnplanung Körper: Bewegung im Raum von Start zu Zielkoordinaten mit entsprechender Ausrichtung f(ddx, ddy, Teta, h)
5 Ebene: Bewegung im Raum: Erstellen von Zielsequenzen um finale Position zu erreichen.

Ebene 1 und 2 sind abgeschlossen, jetzt will ich 3 und dann 4 implementieren. Die höhere Ebenen geben die Vorgaben immer weiter an die unteren Ebenen. Später ist dann noch geplant, dass ein Gyro die Lage überwacht und die Werte an die Ebene 2 übergibt.
Ebene 5 wird später die Abstandssensoren auswerten und anhand von erkannten Hindernissen einen Weg ermitteln, der um diese Hindernisse herum führt. Diese Ziele werden an Ebene 4 übergeben, die daraus ableitet, wie sich der Körper bewegen muss, um die Ziele zu erreichen. In der Ebene 3 werden daraus die Schritte generiert für jedes Bein und an Ebene 2 übergeben. Diese muss dann aus der Lage des Körpers und den Schrittvorgaben die Position der Fußspitze berechnen, die es zu erreichen gilt. Diese Werte werden an die Ebene 1 übergeben, die daraus dann tatsächliche Servowinkel generiert.

Natürlich muss man das nicht so kompliziert und komplex aufbauen, man kann sich das Leben einfacher machen in dem man rotation und Bewegen unabhängig anlegt. Dann dreht sich der Roboter erst in die Richtung und läuft dann immer gerade aus, dreht sich wieder, läuft gerade aus und so weiter. So braucht man nur drehen und gerade aus laufen implementieren. Ebene 5 und 6 können mit einer Fernsteuerung vom Menschen übernommen werden.

In Ebene 6 stell ich mir vor, dass eine Zielpunkt auf einer Karte angegeben wird und dieser erreicht werden soll. Dazu überprüft der Hexa die Sensoren und läuft auf direktem Weg zum Zielpunkt, stößt er dabei auf ein Hindernis versucht er einen Weg außen rum zu finden. Optimaler weise erstellt er dabei noch eine Karte, auf der das Hindernis eingetragen wird. Ist nach umgehen des Hindernisses der Zielpunkt erreicht: Perfekt. Falls nicht versuch einen anderen Weg zu finden. In dieser oder noch höheren Ebenen kann der Roboter dann auch erkennen, ob z.B. ein Durchgang breit genug ist um ihn quer zu durchlaufen oder er sich erst auf längs drehen muss.

usw.


PS:
11.412.211.223.344.111 Ebene: Weltherrschaft!

[robin]

Die Lösung ist eigentlich ganz einfach, 12Divisionen und 5 Vergleiche.

Dein Roboter hat doch bestimmt eine Maximalgeschwindigkeit pro berechnungsschritt vorgegeben und bewegungsrichtung vorgegeben.
Also z.B. X,Y,Z maximal 5mm/Berechnung, alpha,beta,gamma je 5°/Berechnung.
Jetzt Teilst du deinen Zurückzulegenden Weg durch das Maximum der jeweiligen Bewegungsrichtung (temporär, der alte Wert wird weiterhin benötigt). Größter Teiler.
Danach bestimmst du das Maximum der Ergebnisse und Teilst deine Bewegungsrichtungen durch den zuvor bestimmten Teiler.

--> Dein delta, das du zurücklegen musst um eine gleichmäßige Bewegung zwischen A und B zu erreichen.

[HannoHupmann]

@robin, so einfach ist es leider nicht oder ich versteh deine knappe Erklärung einfach nicht.

Die Bewegung ist nicht von der Geschwindigkeit abhängig, die hat überhaupt nichts damit zu tun. Es geht um eine Strecke und nicht wie schnell die Strecke zurückgelegt wird.
Es gibt natürliche eine maximale Strecke die die Fussspitze zurück legen kann. Diese Strecke ist davon abhängig wie weit die Spitze vom Drehpunkt in der Hüfte entfernt ist und damit wie hoch der Körper des Hexas über den Boden gehalten werden soll. D.h. hier hat man es mit einer variablen Strecke zu tun.

Auch deine Erklärung beschreibt nicht, wie die Bewegung der Fussspitze als Ableitung der Gesamtbewegung des Roboters aussehen muss. Einfaches aufsummieren von dx, dy Schritten funktioniert leider nicht, solange nicht irgendwo vorgegeben ist wie dx und dy aussehen muss.

Zurück zum Problem:
Im Moment ist die Aufgabenstellung 4a und 4b zu implementieren. D.h. wie müssen die 6 individuellen Bahnkurven der Füße aussehen um alle Bewegungen des Hexas abzudecken. Also laufen in alle Richtungen + dabei drehen um jeden beliebigen Winkel oder um es weniger technisch auszudrücken: wie müssen die Bahnkurven aussehen um Walzer tanzen zu können!

[robin]

Warum willst du mit 6 verschiedenen Bahnkurven rechnen? Selbst für Geländegänge ist nur eine Manipulation von Z der einzelnen Beine nötig (in der Regel über Endschalter am Fuß gelöst).

In deiner Obersten Berechnungsebene (Ebene 4+) hast du ein starres Koordinaten System mit fester Ausrichtung im Raum, im prinzip eine Karte. Hier drauf bewegt sich dein Roboter als Vektor (Punkt mit Richtung/-en).
Den Sinn von ebene 3 versteh ich nicht, denn deine IK muss nur 2 Dinge kennen, die aktuelle Position und delta Werte für jede Richtung/Drehung um daraus die neuen Servowinkel zu errechnen.

So würde ich das machen:
5+ Ebene: Map erstellen, Kolisionserkennung, Autonomes Laufen im Raum über X verschiedene Punkte
4 Ebene: Bahnplanung Körper: Bewegung im Raum von Start zu Zielkoordinaten einer größeren Teilstecke P1 zu P2, danach P2 zu P3....
3 Ebene: Teilschritte aus Bahn von Ebene 4 Erzeugen in abhängigkeit von maximal geschwindigkeit.
2a Ebene: Inverse Kinematik: Position der Fussspitze im Raum bestimmt durch f(x,y,h)= alpha, beta gamma übergeben an die Ebene 1.
2b Ebene: Inverse Kinematik: Lage des Körpers, kippen um die Längs und die Querachse f(Q, L)=dh und Übergabe der daraus resultierenden Höhenänderung
1 Ebene: Servosignale Generieren

Machen wir mal ein kleines Beispiel:
Dein Roboter befindet sich im Koordinatensystem an Punkt P1=(0,0,0,0°) und soll über P2(10cm,10cm,0,60°) nach P3(20cm,10cm,0,60°) laufen, ob das ganze jetzt als Kurve oder Gerade ist, spielt erstmal keine Rolle. Das ist jetzt Ebene 5+ und gibt einen Weg vor, der abgearbeitet werden soll ähnlich einem GCode.

Ebene4 bearbeitet jetzt nur eine Zeile des "GCodes" gehe von P1 nach P2. Alle weiten Punkte sind für Ebene4 ersteinmal uninteressant.

Ebene3 arbeitet jetzt immer nur Teilstücke des Weges aus Ebene4 ab (man kann wohl Ebene 3 und 4 zu einer zusammenfassen). Die Größer der Teilstücke hängt von der Geschwindigkeit ab, mit der z.B. die Strecke zurückgelegt werden soll (schleichen, Trap, galopp) oder die als Maximum definiert ist (durch geschwindigkeit der Servos und Mechanik). Die Geschwindigkeitsangabe besteht aus einer Strecke pro berechnung und einem Winkel pro berechnung (als beispiel mal 10ms pro Berechnung). Sagen wir 5mm/Berechnung und 1°/Berechnung. Ausgehend von einer geraden zwischen P1 und P2 bedeutet das einen Abstand von ~141mm /5 = 28 Berechnungsschritte. Für den Winkel werden aber 60 benötigt. Hier kann man um einen gleichmäßigen gang zu ermöglichen und sich nicht 32 Berechnungen lang am ende zu drehen, die Geschwindigkeit anpassen. Also 141mm/60=2,35mm/Berechnung. Das wäre dann der Teilschritt der Bahnkurve und wird als deltaX,deltaY,...deltaalpha an die IK übergeben.

Ebene2: Inverse Kinematik und Kontrolle ob Bein zurückgesetzt werden muss.

Ebene1: Winkel in Zeit umrechnen und PPM-Signal erzeugen.

Das ganze lässt sich natürlich noch verfeinern und z.B. Beschleunigungsrampen einbauen. Ebene 2 könnte man mit Sicherheit auch noch mehrfach unterteilen z.B. mit Schwerpunktberechnung etc.

So würde ich das auf jedenfall machen, hoffe das ist jetzt etwas verständlicher, wie ich das gemeint habe.

[HannoHupmann]

Im Prinzip reden wir vom selben, bis auf die Tatsache mit der Geschwindigkeit. Eine Wegstrecke wird nun mal nicht in Geschwindigkeit gemessen sondern in Länge und auch ein Winkel lässt sich sehr gut in eine Länge umrechnen. Es geht auch nicht darum ob der Hexa schnell oder langsam den Weg zurück legt, zumal die Servos alle mit gleicher Geschwindigkeit laufen müssen d.h. ich kann keinen schneller oder langsamer laufen lasse. Grundsätzlich kann ich für alle Servos die Geschwindigkeit rauf setzen, dann wird er schneller in allen Servos oder eben langsamer für alle Servos.
Ein weiterer Punkt ist, dass man den Servos nur Signale geben kann, aber keine Rückmeldung bekommt wo sie sich tatsächlich befinden. Abgesehen davon ist es das Gleiche.


an Punkt P1=(0,0,0,0°) und soll über P2(10cm,10cm,0,60°) nach P3(20cm,10cm,0,60°) laufen, ob das ganze jetzt als Kurve oder Gerade ist, spielt erstmal keine Rolle.
Leider geht es genau darum!

Die Schwierigkeit liegt nicht in der übergeordneten Theorie, sondern z.B. darin wie die Mathematik aussehen muss um den Roboter z.B. zu drehen. Nun ist drehen um den Mittelpunkt noch recht einfach, aber drehen um einen beliebigen Punkt braucht eben 6 unterschiedliche Bahnkurven. Bei reinen bewegungen schräg oder gerade ist es deutlich einfacher. Hier mal die Berechung von meinem Phoenix² Projekt, die hatte noch eine feste Vorgabe für die Höhe h!
2643826439

Heute Nachmittag habe ich mal versucht die beiden Linien auf denen sich die Beine bewegen werden zu überlagern. Da sich allerdings das Koordinatensystem dabei mit dreht ist es rechnerisch ziemlich aufwendig die ursprüngliche Weglinie im neuen Koordinatensystem darzustellen. Soviel Rechenpower hat mein µC vermutlich nicht.

[robin]

Wenn deine IK richtig implementiert ist, dann ist eine Kurve auch nur die länge eines Kreisbogens immer auf X addieren und der Winkel um den gedreht wird. Wichtig, damit es eine Kurve wird, ist das beide Werte gleichmäßig reduziert werden.

[robin]

Die Erklärung oben ist etwas dürftig, hatte wenig Zeit.

Nochmal zu der "Geschwindigkeit". Bei der Ansteuerung hast du eine Zeitkonstante, entweder deine Berechnung der IK oder das Generieren deiner Servo Signale (<20ms). Innerhalb dieser Zeit können sich deine Servos nur um x Grad bewegen oder anders formuliert deine Fußspitzen bzw.Torso x mm zurücklegen. Klar spielt es hier auch ein rolle wie weit die beine gespreizt sind, ich gehe bei meinen Berechnungen vorerst von einer festen maximal Geschwindigkeit aus, die in jeder Beinlage erreicht werden kann, da ich den entsprechenden Rechenschritt noch nicht implementiert habe.

Da dein Roboter aber vermutlich nicht immer mit maximaler Geschwindigkeit laufen soll, kannst du deiner IK auch einen kleineren Winkel als Maxima vorgeben -> Roboter wird langsamer, da weniger Bewegung in diesem festen Zeitintervall stattfindet. Der Servo dreht dadurch nicht langsamer, sondern es wird lediglich sein Weg begrenzt, den er pro Berechnungsschritt zurück legen darf.

Zu dem Problem des fehlenden Feedbacks: Deswegen muss das Limit so liegen, dass dieser Wert unter allen Umständen erreicht werden kann.

Wie schon gesagt, sollte das drehen um einen beliebigen Punkt keine Probleme mehr in Ebene 3 bereiten, da es nur noch die richtige Kombination von Übergabe werten in deine IK benötigt. Eine Dreh- und Bewegungsrichtung mit entsprechenden "Geschwindigkeiten"/Limits, denn sonst dreht er sich zB. zu schnell im Kreis und läuft dann nur noch in eine Richtung.

Ebene 3 produziert aus deiner vorgegeben Bahnkurve die der Roboter laufen soll dann sehr kleine (wenige mm) geraden mit einem Winkel relativ zum Roboter. Im Prinzip der Geschwindigkeits Vektor deines Roboters.

Ich versuche das morgen mal für ein besseres Verständnis in eine XY-Ebene auf zeichnen und krame dann noch meine IK-Berechnungen raus, falls gewünscht.

[HannoHupmann]

Ok dacht ich mir, wir reden vom gleichen nur unterschiedlich formuliert.

Aber kommen wir zurück zur Aufgabenstellung oder besser gesagt meinem aktuellen Problem:
Nochmal der Selbstversuch: zur Türe laufen und sich dabei in der Vorwärtsbewegung um die eigene Achse drehen. Diese Bewegung enthält, dass man ein paar Schritte rückwärts läuft, dann seitwärts dann wieder gerade aus. Soweit kann das jeder Mensch durchführen, da er seine Füße auf die entsprechenen Punkte am Boden stellt die Beine dazu passend bewegt.

Ein Hexabot kann diese Bewegung genauso ausführen, allerdings muss die Bewegung vorher berechnet werden. D.h. diese spezielle Bewegung gliedert sich in eine Bewegung Richtung Türe = translatorisch und eine Bewegung um die eigene Mittelachse = rotatorisch. Beide Bewegungen werden gleichzeitig ausgeführt und überlagern sich damit.

Beschreibt die Fußspitze von der minimal Position zur maximal Position eine Gerade auf dem Boden ist diese Bewegung nicht möglich, dann wäre es nur eine gerade Bewegung in eine Richtung. Keine Frage eine Kreis oder Kurvenbewegung lässt und muss hier in viele kleine Geradenstückchen unterteilt werden. Die Bewegung ist also eine abfolge von kleinen Geradenstückchen. Die Frage ist also, A: wie müssen die einzelnen Teilstücken der Bewegung von min zu max aussehen, damit sich der Körper nach vorn bewegt und gleichzeitig dreht und B: wie kann diese Bewegung rechnerisch dargestellt werden?

Vorüberlegung:
Zeichnet man sich beide Bewegungen auf, sieht man eine Gerade von min zu max (für die Bewegung nach vorn) und eine Tangente auf dem Kreisbogen um den Roboter zu drehen. Summiert man beide Bewegungen auf, erhält man eine Bewegung dazwischen die der gewünschten entsprechen sollte. Nur nach jeder kleinsten Teilbewegung des Roboters ändert sich seine Ausrichtung:

Ansatz 1: Roboter Koordinatensystem
Ich habe kein globales, ortsfestes Koordinatensystem sondern ein Roboterkoordinatensystem, welches fest auf dem Roboter ist (y- zeigt immer nach vorn). Bei einer translatorischen Bewegung ohne Drehung bleibt das Koordinatensystem wie es ist und wird im Raum verschoben (+5m zur Tür in Y-Richtung). Die Fussspitze beschreibt eine Gerade definiert durch einen Vektor V1( x=1, ymin = 5) zu V2(x=1 und ymax = 8). Bei einer Drehung entsteht ein Winkel zwischen dem ursprünglichen und dem Zielkoordinatensystem: Daraus folgt, dass die Bewegung V1 (1 5) und V2(1 8) im ersten Koordinatensystem in das neue Koordinatensystem übertragen werden muss und dort korrekt darzustellen ist. Meine Versuche die alte Bewegung von oben nach ins neue, gedrehte Koordinatensystem umzurechnen haben soviele Rechenschritte erzeugt, dass ich es aufgegeben habe, da es nicht praktikabel im Code umgesetzt werden kann.
Bei diesem Ansatz ist übrigens egal ob man nur ein infinitisimal kleines Wegstück betrachtet oder die ganze Bewegung von min zu max, denn die Umrechnung ins neue Koordinatensystem muss auch bei kleinen Wegstrecken korrekt sein.

Ansatz 2: Globales Koordinatensystem
Ein globales Koordinatensystem mit Ausrichtung Nord bleibt unverändert nur der Roboter in diesem Koordinatensystem verändert sich. Auch hier muss die Trajektorie laufen angepasst werden, da sich die Position des Roboters ändernt und damit auch die Abstände von Global X und Y. Es läuft immer darauf hinaus, dass die Positionsänderung des Roboters in der Bahnkurve der Füße berücksichtigt werden muss; was mit Trigonometrie nur sehr schwer zu beschreiben ist.

Ansatz 3: Bisher und bei Phoenix² implementiert
Zerlegen der Bewegung in zwei Teilbewegungen: Drehen um den Mittelpunkt und dann gerade aus laufen. Sehr trivial aber möglich, insbesondere sieht man an der Rechnung oben, dass ich damals schon "Drehen um einen beliebigen Punkt im Raum" implementiert habe. D.h. es muss gar nicht der Mittelpunkt sein um den sich der Roboter dreht, die daraus entstehenden Bahnkurven sind in meiner Berechnung dargestellt und wie man sieht auch nur Geraden, die in der Summe dann eine Kreisbewegung ergeben.

Meine Problemstellung ist also weniger, wie es übergeordnet aussieht, sondern mehr die reine, banale Mathematik um die Theorie der Bewegung in die Praxis der Bahnkurve einer Fußspitze umzusetzen. Es wird bei diesen komplexen Bewegungen immer auf eine individuelle Bahnkurve für jedes Bein hinaus laufen (was übrigens viel einfacher ist als man denkt), ist bei uns ja nicht anders wenn wir die oben beschriebene Bewegung ausführen.

[robin]

Also ist dein Problem eigentlich nur eine Simple Koordinatentransformation deiner delta werte aus dem Globalen Koordinatensystem in dein Roboter Koordinatensystem.

http://de.wikipedia.org/wiki/Koordinatentransformation

[HeXPloreR]

Hallo Hanno,

ich denke es gibt noch Denkfehler: Du benötigst auch ein globales Koordinatensystem um den Zielpunkt fest zu legen. Auch wen Du es nicht explizit vorgibst, ergibt es sich sobald Du irgendeinen Winkel für eine Drehung angibst und verarbeitest. Das bedeutet dann das Roboterkoordinatensystem sich mit dem Roboter dreht - es darf nicht fest stehen, denn sonst wäre es das Globale. Es spielt auch keine Rolle ob das Globale- und oder das Roboterkoordinatensystem nach Norden ausgerichtet sind. Wichtig ist hier nur der Bezug zueinander beim Start, an dem Punkt müssen sie sich decken - also Ursprung (0,0) haben. Dann entfernt sich der Roboter-Nullpunkt von Globalen-Nullpunkt. Das muss mitberechnet werden.
Es würde sicherlich ersteinmal die Koordinaten Vorgabe erleichtern wenn man es so macht. Stell Di hier bitte mal vor Du musst immer die neuen koordinaten selbst ausrechenen weil Dein Bot grade mal irgendwo 33° zu Norden steht. Dafür ist allein das Roboter-KooSy in Bezug auf Globale(Welt-KooSy) zuständig.

Jetzt musst du aber auch noch festlegen, welche Koordinaten Du bei einem Ziel angibst, die des Roboters oder der Welt. Und die bisherige Drehung und Entfernung in Bezug mit den Beiden setzen. Eine Karte kann möglicherweise ohne Kenntniss der Welt gebaut werden, aber sobald Du diese Karten auch als Bezugspunkte heranziehst benötigt sie den bezug zum WeltKoordinatensystem, nicht zum Roboterkoordinatensystem.

Meine Überlegung zu Deinem Problem: Ein Vektor wird von einem Fuß abgefahren (Schub), der Vektor wird mit dem aktuelle Drehwinkel gedreht. Der Vektor nähert sich jetzt immer mehr einer Bewegungsgrenze an, wo demnächst ein Schritt ansteht. Diesen neuen Schrittpunkt setzt man wieder auf den gleichen Schrittpunkt wie vorher im Roboter-Koo-Sy (das ...-Koo-Sy ist jetzt mein wort ;) ), aber auch nur wenn die (Dreh)-Bewegung insgesamt gleichmässig ist - also der Drehwinkel sich nicht geändert hat. Auch eine Neuberechnung schadet hier wohl dennoch nicht.
Eine Änderung der Position einen Fußes kann nur über den Robotermittelpunkt passieren: Der Roboter dreht sich und dadurch ändert sich ja auch die Hypotenuse ziwschen Fuss und Mittelpunkt. Das erscheint mir eine einfache Sinus/Cosinus Rechnung zu sein, wobei der Vektor mitgedreht werden und der nächste Fußpunkt darauf liegen muss. Und jenachdem ob man sich in einer Schub- oder ein Schrittphase befindet entsprechend die Richtnung der Bewegung in Koordinaten für dei Füße ausgedrück werden muss.

ich bin kein Mathematiker, ein konkrete Rechnung kann und werde ich nicht liefern, vielleicht wenn ich die Lösung auch programmiert bekommen habe.
Ich denke auch es so ziemlich alles dazu gesagt worden.

EDIT: Ich möchte auch nochmal einen Link/PDF (http://gymglinde.de/typo40/fileadmin/user_upload/Robotik/pdf/Besondere_Lernleistung.pdf) beisteuern.

[schorsch_76]

Hallo Hanno,
diesen Artikel [1] finde ich sehr nützlich und verständlich.

Gruß
Georg

[1] http://freespace.virgin.net/hugo.elias/models/m_ik.htm

[HannoHupmann]

Interessanter Ansatz, das ganze mit Kräften zu berechnen, statt mit Position.

Allerdings bin ich schon ein bischen weiter. Im Moment geht es mir darum die Target Positionen zu finden. Hier habe ich heute sehr viel gerechnet und in excel getestet jetzt habe ich die Berechnungen für die einzelnen Fusskurven-Segemente soweit fertig. Morgen muss daraus noch eine Abfolge werden. Ich bin noch nicht sicher ob es dann so funktioniert wie geplant, aber das werde ich erst sehen, wenn ich alles im Spin implementiert habe.

[HannoHupmann]

So die Berechnung ist nun erst mal fertig und komplett im Excel programmiert als erster Test, denn dort kann ich mir die Positionen in einem Diagramm anzeigen lassen und meine Formeln prüfen.

Nächster Schritt ist es die ganze Mathematik die dabei entstanden ist in Spin zu programmieren und dann hoffe ich, dass die ersten Tests erfolgreich sein werden.

Gerade habe ich noch heraus gefunden, dass ich zwischen den Geschwindigkeiten 3:3, 4:2, 5:1 relativ leicht durch das Ändern eines Parameters umschalten kann, damit kann der Roboter in allen Geschwindigkeiten laufen :cool:

[Ryoken]

Hier gleich die Antwort auf die Frage von HexPlorer: Gibt man z.B. vor: "Bleib auf der Stelle stehen aber dreh dich um 180°" würde ich keinen einzigen Schritt nach vorn oder hinten brauchen, aber eben x-Schritte für die Drehung. Gleiches gilt für: "Lauf 10 Schritte gerade nach vorn, aber ohne Drehung" wieder hätten wir einmal 10 Schritte und einmal 0 Schritte. Die Abfrage IF/ELSE soll einfach die Anzahl der Schritte auswählen, die größer ist und dann ausführen. Im ersten Fall würden sich dx und dy = 0 und damit keinen Beitrag zur Bewegung der Fusspitze haben. Nur Winkel = 180° wird sich auswirken.

Zu den Schritten an sich:
Grundsätzlich bevorzuge ich ein 5x1, da es deutlich stabiler ist als 3x3 wenn auch langsamer. Vielleicht implementiere ich auch, dass bei geraden Strecken ohne Rotation ein 3x3 erlaubt ist. Für den Anfang ist 5x1 einfacher. Also wenn schon jeweils ein Bewegungsschema für reine Geradeaus- bzw. Drehbewegung vorhanden ist, würde ich für eine geschmeidige Fortbewegung eine einfache Überlagerung versuchen:
Für die Drehung bewegen sich vermutlich die einen Beine vorwärts und die anderen Rückwärts - es gibt also quasi eine Differenz in den Bewegungsvorgaben für die Beine. Da zur Fortbewegung die Vorwärtsbewegung evtl. schon ausgeschöpft ist, kann man diese Differenz bei Überlagerung also nur durch Verlangsamung der "Kurveninneren" Beine erreichen. Der erzielbare Betrag verringert (halbiert?) sich dadurch also.
Wenn man jetzt eine Bahnplanung hat, sollten die auszuführenden translatorischen und rotatorischen Operationen bekannt sein. Dann die Bewegungsvorgaben einzeln berechnen, die für die Rotation noch durch Faktor 2 (?; s.o.) oder etwas höher, für glatteren Ablauf, Teilen, zur Translation addieren und das ganze ausführen lassen.


Aber kommen wir zurück zur Aufgabenstellung oder besser gesagt meinem aktuellen Problem:
Nochmal der Selbstversuch: zur Türe laufen und sich dabei in der Vorwärtsbewegung um die eigene Achse drehen. Diese Bewegung enthält, dass man ein paar Schritte rückwärts läuft, dann seitwärts dann wieder gerade aus. Soweit kann das jeder Mensch durchführen, da er seine Füße auf die entsprechenen Punkte am Boden stellt die Beine dazu passend bewegt.

Ein Hexabot kann diese Bewegung genauso ausführen, allerdings muss die Bewegung vorher berechnet werden. D.h. diese spezielle Bewegung gliedert sich in eine Bewegung Richtung Türe = translatorisch und eine Bewegung um die eigene Mittelachse = rotatorisch. Beide Bewegungen werden gleichzeitig ausgeführt und überlagern sich damit.

Beschreibt die Fußspitze von der minimal Position zur maximal Position eine Gerade auf dem Boden ist diese Bewegung nicht möglich, dann wäre es nur eine gerade Bewegung in eine Richtung. Keine Frage eine Kreis oder Kurvenbewegung lässt und muss hier in viele kleine Geradenstückchen unterteilt werden. Die Bewegung ist also eine abfolge von kleinen Geradenstückchen. Die Frage ist also, A: wie müssen die einzelnen Teilstücken der Bewegung von min zu max aussehen, damit sich der Körper nach vorn bewegt und gleichzeitig dreht und B: wie kann diese Bewegung rechnerisch dargestellt werden?

Vorüberlegung:
Zeichnet man sich beide Bewegungen auf, sieht man eine Gerade von min zu max (für die Bewegung nach vorn) und eine Tangente auf dem Kreisbogen um den Roboter zu drehen. Summiert man beide Bewegungen auf, erhält man eine Bewegung dazwischen die der gewünschten entsprechen sollte. Nur nach jeder kleinsten Teilbewegung des Roboters ändert sich seine AusrichtungHier scheinen sich für mich auch wieder (übergeordnete) Bahnplanung und "konkrete" Bewegungsplanung des Bots (Schrittfolge) zu vermischen. Die Berechnung über Kreissegmente dürfte außerdem recht aufwendig werden, oder?



Ansatz 3: Bisher und bei Phoenix² implementiert
Zerlegen der Bewegung in zwei Teilbewegungen: Drehen um den Mittelpunkt und dann gerade aus laufen. Sehr trivial aber möglich, insbesondere sieht man an der Rechnung oben, dass ich damals schon "Drehen um einen beliebigen Punkt im Raum" implementiert habe. D.h. es muss gar nicht der Mittelpunkt sein um den sich der Roboter dreht, die daraus entstehenden Bahnkurven sind in meiner Berechnung dargestellt und wie man sieht auch nur Geraden, die in der Summe dann eine Kreisbewegung ergeben.

Meine Problemstellung ist also weniger, wie es übergeordnet aussieht, sondern mehr die reine, banale Mathematik um die Theorie der Bewegung in die Praxis der Bahnkurve einer Fußspitze umzusetzen. Es wird bei diesen komplexen Bewegungen immer auf eine individuelle Bahnkurve für jedes Bein hinaus laufen (was übrigens viel einfacher ist als man denkt), ist bei uns ja nicht anders wenn wir die oben beschriebene Bewegung ausführen.Das dürfte die einfachste und flexibelste Umsetzung sein.
Ähnelt wohl auch meinem oben beschriebenen Ansatz. Nur würd ichs nach der getrennten Berechnung, dann aber überlagern.

Das Türproblem ist allerdings auch schon recht speziell ist finde ich. Dadurch das zu der geraden Hauptrichtung der Bewegung trotzdem eine Drehung kommen soll hast Du ja unterwegs zwischen "straightforward" und "90° seitlich" quasi beliebige "Strafing-Winkel" zu bewältigen, um in der Hauptrichtung auch noch vorwärts zu kommen. Die müssten wahrscheinlich vorher erstmal "eintrainiert" werden.

[HannoHupmann]

Für die Drehung bewegen sich vermutlich die einen Beine vorwärts und die anderen Rückwärts
Gilt nur für Drehungen bei denen der Drehpunkt innerhalb des Roboters liegt. Für Drehpunkt außerhalb bewegen sich die Beine in die gleiche Richtung. Der Radius bzw. die Länge der Bogensehne ändert sich. Das hatte ich schon bei meinem Phoenix² implementiert. Lässt sich allerdings sehr leicht errechnen, wenn man für die Drehung die Vorgaben v,w (die x und y Koordinaten des Drehpunkts) und den Winkel teta angibt um wie viel Grad der Roboter sich um den Punkt drehen soll. Bei v,w = 0, gilt was du sagst, dann bewegt sich eine Seite nach vorn und die Andere zurück. Bei v = 20 und w = 0, wäre es eben eine Bewegung auf dem Kreisradius. Für jedes Bein wird dann ein eigenes Bogensegment berechnet mit der dazugehörigen Tangente und einer spezifische Länge (berücksichtigen des Abstands vom Drehpunkt).

Mittlerweile habe ich die Aufgabenstellung sehr weit lösen können. Die Rotatorische Bewegung wird wie oben berechnet und durch die Translatorische überlagert. Daraus wird die Schrittlänge und die Bahnkurve berechnet für jedes Bein berechnet. Am Ende der Bahnkurve erfolgt die Rückführung zur Startposition, mit einer kleinen Trajektorie damit es sanfter aussieht. Wenn sich alle Beine einmal über die Schrittlänge bewegt haben ist ein Voll_Schritt fertig. Aus Teta und dem maximal möglichen Winkel der pro VollSchritt gedreht werden kann ergibt sich die Anzahl der Voll-Schritte für drehen und bei translation nehme ich einfach x²+y² = Distanz² und Teile die Distanz durch die kleinste Schrittlänge.

Weitere Features die bereits implementiert sind: Ich kann drei Geschwindigkeiten wählen ob ich nun 3:3, 4:2, oder 5:1 bewegen möchte (Anzahl der Beine am Boden : Beine in der Luft) die Berechnung ermittelt dann automatisch die korrekten Punkte auf der Bahnkurve und natürlich wie schnell sich die Motoren generell drehen sollen. Natürlich auch kippen und neigen wird berücksichtigt.
Das lässt sich alles mit genügend Mathematik erschlagen und aktuell bin ich dabei die Formeln im Code darzustellen und dann wird es vermutlich ziemlich viel debugging geben.

Ob diese Berechnung schon für mein Tür-Problem reicht kann ich schwer sagen, im Prinzip ja, aber vielleicht muss ich auch noch eine übergeordnete Ebene programmieren.

[HannoHupmann]

So der Code ist geschrieben, aber funktioniert natürlich noch nicht, gestern habe ich es nur noch geschafft die Translation zu debuggen.

Anderes Thema: Servos
Leider ist ein weiterer Servo (BMS-705MG) ausgefallen und ich vermute einer steht kurz davor, aber der Reihe nach.
1) Servo lässt sich gar nicht mehr ansteuern, wenn er manuell bewegt wird, geht er sehr schwergängig.
2) Servo wird noch angesteuert aber dreht sich auch schon deutlich schwerer als die übrigen.

Der letzte Servo (BMS660 DMG + HS 52) der ausgefallen ist hatte deutliche Brandspuren auf der Steuerplatine. Motor und Getriebe haben noch wunderbar funktioniert.
Leider habe ich noch keine Ahnung was diesen "Tod" verursacht, denn noch bewegen sich die Beine nur in der Luft, d.h. ohne Belastung.
Kann es einfach an der Chinaproduktion liegen?
EDIT: Gerade bei Hobbykin.com gelesen: "Der gute erste Eindruck (Getriebespiel, Rueckstellgenauigkeit usw.) steht einer sehr geringen Lebensdauer gegenber. Die Motorbuersten sind stark unterdimensioniert und brennen schnell ab, was zum Ausfall des Servos führt. ... Betroffen sind auch BMS 706, 760, 761 mit identischem Motor !!"Frage ist nun, kann man das irgendwie verbessern/optimieren?


Dummerweise ist meine ganze Konstruktion auf diese beiden Abmessungen abgestimmt, so dass ich die Servos nicht einfach mit einer anderen Marke austauschen kann.

[robin]

Ds klingt nicht gut, da bleibt dir ja fast nur die möglichkeit einen ersatzmotor zu suchen. Die frage ist dann aber ob der dann auch die gleiche kraft/Geschwindigkeit liefern wird.

Hast du mal die Maße der Motoren? Vllt. findet man ja was.

[HannoHupmann]

BMS-705MG: Dimensions: 42 x 21.5 x 22 mm
http://www.hobbyking.com/hobbyking/store/catalog/BMS-705MG.jpg

BMS660 DMG: Dimensions: 40.5 x 20 x 42mm
26458

[robin]

Hab eigentlich die internen motoren gemeint, nicht die Servos an sich ;)

[HannoHupmann]

Pu da muss ich erst kucken, hab den letzten Ausfall zerlegt da kann ich ein Bild von machen.

[HannoHupmann]

So langsam frage ich mich eine LED zum Debugen reicht. Die geraden Bewegungen funktionieren schon ganz wunderbar in alle Richtung, nur bei den Drehungen gibt es im Moment noch Probleme. Vor allen welche die gar nicht auf das Programm zurück zu führen sein dürften. So kann ich z.B. die y-Bewegung der Fussspitze konstant auf 0 setzen und trotzdem dreht sich das Bein ein wenig in diese Richtung.

EDIT: Problem gelöst, wozu hat man denn 8 Kerne in seinem µC sollen die was tun für ihr Geld und die Berechnung parallel durchführen!

[schorsch_76]

Hallo Hanno,

das mit der LED zum debuggen ist oft sehr wenig. Meine Idee momentan ist, praktisch alles via I2C anzusteuern und dann nur den Kontroller auszutauschen. Zum Debuggen ist der Kontroller dann der PC via USB I2C Adapter ;)

Habe aber dazu noch nicht so viele Erfahrungen...

Gruß
Georg

[HannoHupmann]

Ich habe noch ein i2c Display rumliegen aber bisher die Mühe gescheut das anzuschließen bzw. zu programmieren. Liegt vielleicht daran, weil es mit 5V betrieben wird, meine µC aber nur mit 3,3V

[HannoHupmann]

Irgendwo in den unendlichen Weiten meines Programmcodes habe ich mich verloren bzw. hatte es irgendwann soweit verbugged äh debugged, dass es gar nimmer recht synchron laufen wollte mit den Beinen. Jetzt allerdings bin ich endlich soweit, dass zumindest schon mal translatorisch vollständig funktioniert. Morgen werde ich mich dann noch um den rotatorischen Part kümmern und meine neuen Servos vom Zoll abholen. Dann könnte man endlich auch richtig Tests machen.

[HannoHupmann]

Aus alt macht neu. Einen der defekten Servos konnte ich mit einen anderen defekten Servo zu einem funktionierenden Servo umbauen. Jetzt hat das Vinculum wieder 6 funktionsfähige Beine und ich kann mich an die restliche Programmierung machen. Danach gibt es dann auch wieder ein Video vom aktuellen Stand der Technik.

[HannoHupmann]

Diesmal sind es nur 1050 Zeilen Code geworden. Mein Phoenix² ist noch mit etwa 800 Zeilen ausgekommen.



CON
_CLKMODE = XTAL1 + PLL16X 'Set to ext crystal, 16x PLL, 80MHz Clock
_XINFREQ = 5_000_000 'Frequency on XIN pin is 5 MHz

l_fuss = 138.0 'Länge vom Fuss (b)
b_2 = 19044.0 ' (l_fuss)²

l_schenk = 110.0 'Länge vom Oberschenkel
a_2 = 12100.0 ' (l_schenk)²

aa_p_bb = 31144.0 'a²+ b² !!! mit xxx.0 angeben
aa_m_bb = -6944.0 'a²- b² !!! mit xxx.0 angeben
ax2 = 220.0 '2a !!! mit xxx.0 angeben
abx2 = 30360.0 '2ab !!! mit xxx.0 angeben

'Globale Werte für HEXA - können noch dynamisch gemacht werden
x_off = 101.0
'Ri_max = 100.0
'Steps = 11 -7>steps_all>7
ON = 1
OFF = 0

OBJ
math : "Float32Full"

Var
'-----------Servo-Puls-Erzeugung----------------------
LONG new_peri_r,PinStart_r,pin_r,high_pulse_r[9] 'Reihenfolge nicht ändern da Pointer in Move-Servo darauf zu greifen
LONG new_peri_l,PinStart_l,pin_l,high_pulse_l[9] 'DO NOT TOUCH
LONG periode
LONG stack_setpulse[25]

LONG speed
LONG Apos[3], Aini[3], Alast[3], Aziel[3], Asign[3]
LONG Bpos[3], Bini[3], Blast[3], Bziel[3], Bsign[3]
LONG Cpos[3], Cini[3], Clast[3], Cziel[3], Csign[3]
LONG Dpos[3], Dini[3], Dlast[3], Dziel[3], Dsign[3]
LONG Epos[3], Eini[3], Elast[3], Eziel[3], Esign[3]
LONG Fpos[3], Fini[3], Flast[3], Fziel[3], Fsign[3]
LONG xA_trans, xB_trans, xC_trans, xD_trans, xE_trans, xF_trans
LONG yA_trans, yB_trans, yC_trans, yD_trans, yE_trans, yF_trans
LONG xA_rota, xB_rota, xC_rota, xD_rota, xE_rota, xF_rota
LONG yA_rota, yB_rota, yC_rota, yD_rota, yE_rota, yF_rota
LONG a_pos, b_pos, c_pos, d_pos, e_pos, f_pos
LONG p_a, p_b, p_c, p_d, p_e, p_f

' Geometrische Variablen
LONG hA, hB, hC, hD, hE, hF
LONG xA, xB, xC, xD, xE, xF
LONG yA, yB, yC, yD, yE, yF
LONG lA, lB, lC, lD, lE, lF, l_max
LONG ph_A, ph_B, ph_C, ph_D, ph_E, ph_F
LONG RiA, RiB, RiC, RiD, RiE, RiF, Ri_min
LONG i_A, i_B, i_C, i_D, i_E, i_F
LONG Ri_max
LONG wQ, wL, hBasis, wx, wy

BYTE A_back, B_back, C_back, D_back, E_back, F_back
BYTE c_a, c_b, c_c, c_d, c_e, c_f

pub main
init
speed := 3000
'-------------------------------------------------------------------------------------------------------------------------
'### INITIALPOSITION ANFAHREN
coginit(6,@moveServo,@new_peri_r)
coginit(7,@moveServo,@new_peri_l) ' Anfahren der Initalposition
coginit(5,setpulse,@stack_setpulse)

repeat
busy_led ' Bestätigen, dass bereit
if ina[6] == 1
waitcnt(2_000_000 + cnt)
outa[26]~
outa[27]~~
quit
'-------------------------------------------------------------------------------------------------------------------------

hBasis := 193.0
wQ := 0.0
wL := 0.0
wx := 0.0
wy := 0.0

xA := x_off
yA := 0.0
hA := 0.0

xB := x_off
yB := 0.0
hB := 0.0

xC := x_off
yC := 0.0
hC := 0.0

xD := x_off
yD := 0.0
hD := 0.0

xE := x_off
yE := 0.0
hE := 0.0
xF := x_off
yF := 0.0
hF := 0.0
IK
'-------------------------------------------------------------------------------------------------------------------------
repeat
busy_led ' Bestätigen, dass bereit
if ina[6] == 1
waitcnt(2_000_000 + cnt)
outa[26] := OFF
quit

hBasis := 193.0
wQ := 0
wL := 0
wx := 0
wy := 0

Richtung(0.0, 0.0, -350.0, 0.0, 45.0, 1.0) 'Richtung(y, x,w_y, v_x, teta, speed)
'Vorgabe

repeat
outa[27] := OFF
waitcnt(10_000_000 + cnt)
outa[27] := ON
waitcnt(10_000_000 + cnt)
repeat
outa[27] := ON



' ################################################## ################################################## ##
' ##################### EBENE 2 ################################################## #####################
' ################################################## ################################################## ##
PUB Richtung(y,x,w,v,teta,speed_2)|alpha_trans,distanc e,steps_trans,steps_rota,steps_all, step_counter,Xi,sign ' v,w,

alpha_trans := math.Atan2(y,x) 'y,x in Rad
if (y== 0.0 AND x < 0.0)
alpha_trans :=math.Radians(180.0)

if(x==0.0)AND(y==0.0)
distance := 0.0
else
distance := math.FSqr(math.FAdd(math.pow(x, 2.0), math.Pow(y,2.0)))

if ((alpha_trans => -1.0471 AND alpha_trans =< -2.0943) OR (alpha_trans => 1.0471 AND alpha_trans =< 2.093)) AND(teta==0.0)
Ri_Max := 150.0
elseif (teta == 0.0)
Ri_Max := 90.0
else
Ri_Max := 90.0

lA := math.FSqr(math.FAdd(math.Pow(math.FSub(math.FSub(m ath.Fneg(x_off),110.0), v),2.0), math.Pow(math.FSub(-115.0,w),2.0)))
lB := math.FSqr(math.FAdd(math.Pow(math.FSub(math.FSub(m ath.Fneg(x_off),110.0), v),2.0), math.Pow(w, 2.0)))
lC := math.FSqr(math.FAdd(math.Pow(math.FSub(math.FSub(m ath.Fneg(x_off),110.0), v),2.0), math.Pow(math.FSub(115.0,w), 2.0)))
lD := math.FSqr(math.FAdd(math.Pow(math.FSub(math.FAdd(x _off, 110.0), v),2.0), math.Pow(math.FSub(115.0,w), 2.0)))
lE := math.FSqr(math.FAdd(math.Pow(math.FSub(math.FAdd(x _off, 110.0), v),2.0), math.Pow(w, 2.0)))
lF := math.FSqr(math.FAdd(math.Pow(math.FSub(math.FAdd(x _off, 110.0), v),2.0), math.Pow(math.FSub(-115.0,w),2.0)))

l_max := math.FMax(lA, lB)
l_max := math.FMax(l_max, lC)
l_max := math.FMax(l_max, lD)
l_max := math.FMax(l_max, lE)
l_max := math.FMax(l_max, lF)

Xi := math.Degrees(math.ATan(math.FDiv(Ri_max,math.FMul( 2.0,l_max))))

if (teta == 0.0)
steps_rota := math.FRound(0.0)
Ri_min := Ri_max

i_A := math.FDiv(Ri_Max,14.0)
i_B := i_A
i_C := i_A
i_D := i_A
i_E := i_A
i_F := i_A

else
steps_rota := math.FRound(math.FAbs(math.FDiv(teta, Xi)))
steps_rota++

RiA := math.FMul(lA,math.FMul(math.tan(math.Radians(Xi)), 2.0))
RiB := math.FMul(lB,math.FMul(math.tan(math.Radians(Xi)), 2.0))
RiC := math.FMul(lC,math.FMul(math.tan(math.Radians(Xi)), 2.0))
RiD := math.FMul(lD,math.FMul(math.tan(math.Radians(Xi)), 2.0))
RiE := math.FMul(lE,math.FMul(math.tan(math.Radians(Xi)), 2.0))
RiF := math.FMul(lF,math.FMul(math.tan(math.Radians(Xi)), 2.0))

Ri_min :=math.FMin(RiA,RiB)
Ri_min :=math.FMin(Ri_min,RiC)
Ri_min :=math.FMin(Ri_min,RiD)
Ri_min :=math.FMin(Ri_min,RiE)
Ri_min :=math.FMin(Ri_min,RiF)

i_A := math.FDiv(RiA,14.0)
i_B := math.FDiv(RiB,14.0)
i_C := math.FDiv(RiC,14.0)
i_D := math.FDiv(RiD,14.0)
i_E := math.FDiv(RiE,14.0)
i_F := math.FDiv(RiF,14.0)


if(teta > 0.0)
sign := 1.0
else
sign := -1.0

if(distance == 0.0)
steps_trans := math.FRound(0.0)
else
steps_trans := math.FRound(math.FDiv(distance, Ri_min))
steps_trans++

ph_A := math.FNeg(math.FSub(90.0,math.Degrees(math.ASin(ma th.FDiv(math.FAdd(w,115.0),lA)))))
ph_B := math.FNeg(math.FSub(90.0,math.Degrees(math.ASin(ma th.FDiv(w,lB)))))
ph_C := math.FNeg(math.FSub(90.0,math.Degrees(math.ASin(ma th.FDiv(math.FSub(w,115.0),lC)))))
ph_D := math.FSub(90.0,math.Degrees(math.ASin(math.FDiv(ma th.FSub(w,115.0),lD))))
ph_E := math.FSub(90.0,math.Degrees(math.ASin(math.FDiv(w, lE))))
ph_F := math.FSub(90.0,math.Degrees(math.ASin(math.FDiv(ma th.FAdd(w,115.0),lF))))

case speed_2
1.0:
a_pos := -2.0
b_pos := -4.0
c_pos := 8.0
d_pos := -1.0
e_pos := 5.0
f_pos := -7.0
1.75:
a_pos := 8.25
b_pos := 1.75
c_pos := -5.25
d_pos := 1.75
e_pos := 8.75
f_pos := -5.25
2.5:
a_pos := 10.5
b_pos := -3.5
c_pos := 10.5
d_pos := -3.5
e_pos := 10.5
f_pos := -3.5

if (steps_trans => steps_rota)
steps_all := steps_trans
else
steps_all := steps_rota

c_a := 0
c_b := 0
c_c := 0
c_d := 0
c_e := 0
c_f := 0

p_a := 0
p_b := 0
p_c := 0
p_d := 0
p_e := 0
p_f := 0

Step_counter := 0

repeat until(step_counter == steps_all)
if (step_counter == steps_rota)
teta := 0.0
if (step_counter == steps_trans)
distance := 0.0

if (a_pos > 7.0)
A_back := 1
case c_a
0:
p_a := 7.0
hA := -50.0
c_a++
1:
p_a := 0.0
hA := -50.0
c_a++
2:
p_a := -7.0
hA := -50.0
c_a++
3:
p_a := -7.0
hA := 0.0
a_pos := -7.0
c_a := 0
else
A_back := 0
p_a := a_pos
hA := 0.0

if (b_pos > 7.0)
B_back := 1
case c_b
0:
p_b := 7.0
hb := -50.0
c_b++
1:
p_b := 0.0
hb := -50.0
c_b++
2:
p_B := -7.0
hB := -50.0
c_b++
3:
p_b := -7.0
hB := 0.0
b_pos := -7.0
c_b := 0
else
B_back := 0
p_b := b_pos
hB := 0.0

if (c_pos > 7.0)
C_back := 1
case c_c
0:
p_c := 7.0
hC := -50.0
c_c++
1:
p_c := 0.0
hC := -50.0
c_c++
2:
p_C := -7.0
hC := -50.0
c_c++
3:
p_c := -7.0
hC := 0.0
C_pos := -7.0
c_C := 0
else
C_back := 0
p_c := c_pos
hC := 0.0

if (d_pos > 7.0)
D_back := 1
case c_d
0:
p_d := 7.0
hD := -50.0
c_d++
1:
p_d := 0.0
hD := -50.0 '150
c_d++
2:
p_D := -7.0
hD := -50.0
c_D++
3:
p_d := -7.0
hD := 0.0
D_pos := -7.0
c_D := 0
else
D_back := 0
p_d := d_pos
hD := 0.0

if (e_pos > 7.0)
E_back := 1
case c_e
0:
p_e := 7.0
hE := -50.0
c_e++
1:
p_e := 0.0
hE := -50.0
c_e++
2:
p_E := -7.0
hE := -50.0
c_e++
3:
p_e := -7.0
hE := 0.0
e_pos := -7.0
c_e := 0
else
E_back := 0
p_e := e_pos
hE := 0.0

if (f_pos > 7.0)
F_back := 1
case c_f
0:
p_f := 7.0
hF := -50.0
c_f++
1:
p_f := 0.0
hF := -50.0
c_f++
2:
p_F := -7.0
hF := -50.0
c_F++
3:
p_F := -7.0
hF := 0.0
F_pos := -7.0
c_f := 0

else
F_back := 0
p_f := f_pos
hF := 0.0


if (distance == 0.0)
xA := math.FSub(x_off,math.FMul(math.FMul(math.FNeg(p_A) ,math.cos(math.Radians(ph_A))),math.FMul(i_A,sign) ))
xB := math.FSub(x_off,math.FMul(math.FMul(math.FNeg(p_B) ,math.cos(math.Radians(ph_B))),math.FMul(i_B,sign) ))
xC := math.FSub(x_off,math.FMul(math.FMul(math.FNeg(p_C) ,math.cos(math.Radians(ph_C))),math.FMul(i_C,sign) ))
xD := math.FSub(x_off,math.FMul(math.FMul(p_D ,math.cos(math.Radians(ph_D))),math.FMul(i_D,sign) ))
xE := math.FSub(x_off,math.FMul(math.FMul(p_E ,math.cos(math.Radians(ph_E))),math.FMul(i_E,sign) ))
xF := math.FSub(x_off,math.FMul(math.FMul(p_F ,math.cos(math.Radians(ph_F))),math.FMul(i_F,sign) ))

yA := math.FMul(math.FMul(math.FNeg(p_A),math.sin(math.R adians(ph_A)) ),math.FMul(i_A,sign))
yB := math.FMul(math.FMul(math.FNeg(p_B),math.sin(math.R adians(ph_B)) ),math.FMul(i_B,sign))
yC := math.FMul(math.FMul(math.FNeg(p_C),math.sin(math.R adians(ph_C)) ),math.FMul(i_C,sign))
yD := math.FMul(math.FMul(math.FNeg(p_D),math.sin(math.R adians(ph_D)) ),math.FMul(i_D,sign))
yE := math.FMul(math.FMul(math.FNeg(p_E),math.sin(math.R adians(ph_E)) ),math.FMul(i_E,sign))
yF := math.FMul(math.FMul(math.FNeg(p_F),math.sin(math.R adians(ph_F)) ),math.FMul(i_F,sign))


else
if (teta == 0.0)

xA := math.FSub(x_off,math.FMul(math.FNeg(p_A),math.FMul (i_A,math.cos(alpha_trans))))
xB := math.FSub(x_off,math.FMul(math.FNeg(p_B),math.FMul (i_B,math.cos(alpha_trans))))
xC := math.FSub(x_off,math.FMul(math.FNeg(p_C),math.FMul (i_C,math.cos(alpha_trans))))
xD := math.FSub(x_off,math.FMul(p_D,math.FMul(i_D,math.c os(alpha_trans))))
xE := math.FSub(x_off,math.FMul(p_E,math.FMul(i_E,math.c os(alpha_trans))))
xF := math.FSub(x_off,math.FMul(p_F,math.FMul(i_F,math.c os(alpha_trans))))

yA := math.FMul(math.FNeg(p_A),math.FMul(i_A,math.Sin(al pha_trans)))
yB := math.FMul(math.FNeg(p_B),math.FMul(i_B,math.Sin(al pha_trans)))
yC := math.FMul(math.FNeg(p_C),math.FMul(i_C,math.Sin(al pha_trans)))
yD := math.FMul(math.FNeg(p_D),math.FMul(i_D,math.Sin(al pha_trans)))
yE := math.FMul(math.FNeg(p_E),math.FMul(i_E,math.Sin(al pha_trans)))
yF := math.FMul(math.FNeg(p_F),math.FMul(i_F,math.Sin(al pha_trans)))

else
xA_rota := math.FSub(x_off,math.FMul(math.FMul(math.FNeg(p_A) ,math.cos(math.Radians(ph_A))),math.FMul(i_A,sign) ))
xB_rota := math.FSub(x_off,math.FMul(math.FMul(math.FNeg(p_B) ,math.cos(math.Radians(ph_B))),math.FMul(i_B,sign) ))
xC_rota := math.FSub(x_off,math.FMul(math.FMul(math.FNeg(p_C) ,math.cos(math.Radians(ph_C))),math.FMul(i_C,sign) ))
xD_rota := math.FSub(x_off,math.FMul(math.FMul(p_D ,math.cos(math.Radians(ph_D))),math.FMul(i_D,sign) ))
xE_rota := math.FSub(x_off,math.FMul(math.FMul(p_E ,math.cos(math.Radians(ph_E))),math.FMul(i_E,sign) ))
xF_rota := math.FSub(x_off,math.FMul(math.FMul(p_F ,math.cos(math.Radians(ph_F))),math.FMul(i_F,sign) ))
yA_rota := math.FMul(math.FMul(math.FNeg(p_A),math.sin(math.R adians(ph_A)) ),math.FMul(i_A,sign))
yB_rota := math.FMul(math.FMul(math.FNeg(p_B),math.sin(math.R adians(ph_B)) ),math.FMul(i_B,sign))
yC_rota := math.FMul(math.FMul(math.FNeg(p_C),math.sin(math.R adians(ph_C)) ),math.FMul(i_C,sign))
yD_rota := math.FMul(math.FMul(math.FNeg(p_D),math.sin(math.R adians(ph_D)) ),math.FMul(i_D,sign))
yE_rota := math.FMul(math.FMul(math.FNeg(p_E),math.sin(math.R adians(ph_E)) ),math.FMul(i_E,sign))
yF_rota := math.FMul(math.FMul(math.FNeg(p_F),math.sin(math.R adians(ph_F)) ),math.FMul(i_F,sign))

xA_trans := math.FSub(x_off,math.FMul(math.FNeg(p_A),math.FMul (i_A,math.cos(alpha_trans))))
xB_trans := math.FSub(x_off,math.FMul(math.FNeg(p_B),math.FMul (i_B,math.cos(alpha_trans))))
xC_trans := math.FSub(x_off,math.FMul(math.FNeg(p_C),math.FMul (i_C,math.cos(alpha_trans))))
xD_trans := math.FSub(x_off,math.FMul(p_D,math.FMul(i_D,math.c os(alpha_trans))))
xE_trans := math.FSub(x_off,math.FMul(p_E,math.FMul(i_E,math.c os(alpha_trans))))
xF_trans := math.FSub(x_off,math.FMul(p_F,math.FMul(i_F,math.c os(alpha_trans))))
yA_trans := math.FMul(math.FNeg(p_A),math.FMul(i_A,math.Sin(al pha_trans)))
yB_trans := math.FMul(math.FNeg(p_B),math.FMul(i_B,math.Sin(al pha_trans)))
yC_trans := math.FMul(math.FNeg(p_C),math.FMul(i_C,math.Sin(al pha_trans)))
yD_trans := math.FMul(math.FNeg(p_D),math.FMul(i_D,math.Sin(al pha_trans)))
yE_trans := math.FMul(math.FNeg(p_E),math.FMul(i_E,math.Sin(al pha_trans)))
yF_trans := math.FMul(math.FNeg(p_F),math.FMul(i_F,math.Sin(al pha_trans)))

xC := math.FAdd(math.FDiv(math.FSub(xC_trans, xC_rota),2.0),xC_rota)
yC := math.FAdd(math.FDiv(math.FSub(yC_trans, yC_rota),2.0),yC_rota)
xD := math.FAdd(math.FDiv(math.FSub(xD_trans, xD_rota),2.0),xD_rota)
yD := math.FAdd(math.FDiv(math.FSub(yD_trans, yD_rota),2.0),yD_rota) }}


IK
a_pos := math.FAdd(a_pos, speed_2)
b_pos := math.FAdd(b_pos, speed_2)
c_pos := math.FAdd(c_pos, speed_2)
d_pos := math.FAdd(d_pos, speed_2)
e_pos := math.FAdd(e_pos, speed_2)
f_pos := math.FAdd(f_pos, speed_2)


' ################################################## ################################################## ##
' ##################### EBENE 1c ################################################## #####################
' ################################################## ################################################## ##

pub move|i
i:=0
repeat 3
if Aziel[i] > Alast[i] ' Servo-bewegung vorwärts
Asign[i] := +1
if Aziel[i] < Alast[i] ' Servo-bewegung rückwärts
Asign[i] := -1

if Bziel[i] > Blast[i] ' Servo-bewegung vorwärts
Bsign[i] := +1
if Bziel[i] < Blast[i] ' Servo-bewegung rückwärts
Bsign[i] := -1

if Cziel[i] > Clast[i] ' Servo-bewegung vorwärts
Csign[i] := +1
if Cziel[i] < Clast[i] ' Servo-bewegung rückwärts
Csign[i] := -1

if Dziel[i] > Dlast[i] ' Servo-bewegung vorwärts
Dsign[i] := +1
if Dziel[i] < Dlast[i] ' Servo-bewegung rückwärts
Dsign[i] := -1

if Eziel[i] > Elast[i] ' Servo-bewegung vorwärts
Esign[i] := +1
if Eziel[i] < Elast[i] ' Servo-bewegung rückwärts
Esign[i] := -1

if Fziel[i] > Flast[i] ' Servo-bewegung vorwärts
Fsign[i] := +1
if Fziel[i] < Flast[i] ' Servo-bewegung rückwärts
Fsign[i] := -1

if A_back == 1
ASign[i] := ASign[i]*5
if B_back == 1
BSign[i] := BSign[i]*5
if C_back == 1
CSign[i] := CSign[i]*5
if D_back == 1
DSign[i] := DSign[i]*5
if E_back == 1
ESign[i] := ESign[i]*5
if F_back == 1
FSign[i] := FSign[i]*5
i++

repeat ' Bewegung
i:= 0
repeat 3
Apos[i] += Asign[i] ' Positionsänderung
if ((Asign[i]>0.0) AND (Apos[i] > Aziel[i])) OR ((Asign[i]<0.0) AND (Apos[i] < Aziel[i])) ' Ziel erreicht
Asign[i]:=0
Bpos[i] += Bsign[i] ' Positionsänderung
if ((Bsign[i]>0.0) AND (Bpos[i] > Bziel[i])) OR ((Bsign[i]<0.0) AND (Bpos[i] < Bziel[i])) ' Ziel erreicht
Bsign[i]:=0
Cpos[i] += Csign[i] ' Positionsänderung
if ((Csign[i]>0.0) AND (Cpos[i] > Cziel[i])) OR ((Csign[i]<0.0) AND (Cpos[i] < Cziel[i])) ' Ziel erreicht
Csign[i]:=0
Dpos[i] += Dsign[i] '
if ((Dsign[i]>0.0) AND (Dpos[i] > Dziel[i])) OR ((Dsign[i]<0.0) AND (Dpos[i] < Dziel[i])) ' Ziel erreicht
Dsign[i]:=0
Epos[i] += Esign[i] ' Positionsänderung
if ((Esign[i]>0.0) AND (Epos[i] > Eziel[i])) OR ((Esign[i]<0.0) AND (Epos[i] < Eziel[i])) ' Ziel erreicht
Esign[i]:=0
Fpos[i] += Fsign[i] ' Positionsänderung
if ((Fsign[i]>0.0) AND (Fpos[i] > Fziel[i])) OR ((Fsign[i]<0.0) AND (Fpos[i] < Fziel[i])) ' Ziel erreicht
Fsign[i]:=0
i++

setpulse ' Servo einen Schritt bewegen
repeat speed ' Warten

until ((Asign[0] == 0) AND (Asign[1] == 0) AND (Asign[2] == 0)AND(Bsign[0] == 0) AND (Bsign[1] == 0) AND (Bsign[2] == 0)AND (Csign[0] == 0) AND (Csign[1] == 0) AND (Csign[2] == 0)AND(Dsign[0] == 0) AND (Dsign[1] == 0) AND (Dsign[2] == 0)AND(Esign[0] == 0) AND (Esign[1] == 0) AND (Esign[2] == 0)AND(Fsign[0] == 0) AND (Fsign[1] == 0) AND (Fsign[2] == 0)) ' Bis zum Ziel und Schluss

' ################################################## ################################################## ##
' ##################### EBENE 1a/1b ################################################## #################
' ################################################## ################################################## ##

PUB IK|i, ca, ca_2, alpha, alpha_PP, alpha_P, gamma, phi, dhAF, dhBE, dhCD, dhABC, dhDEF, dQ, dL, rA, rB, rC, rD, rE, rF, hhA, hhB, hhC, hhD, hhE, hhF

'~~~~~ BERECHNUNG KIPPEN UND NEIGEN ~~~~~
dQ := math.Sin(math.Radians(wQ))
dL := math.Sin(math.Radians(wL))
'Prüfen ob -wy bzw. -wx korrekt ist)
dhCD := math.FMul(dQ, math.FSub(-230.0, -wy))
dhBE := math.FMul(dQ, math.FSub(0.0, -wy))
dhAF := math.FMul(dQ, math.FSub(+230.0, -wy))
dhABC := math.FMul(dL, math.FSub(-110.0, -wx))
dhDEF := math.FMul(dL, math.FSub(+110.0, -wx))

hhA := math.FAdd(hBasis, dhAF)
hhA := math.FAdd(hhA, dhABC)
hhA := math.FAdd(hhA, hA)
hhB := math.FAdd(hBasis, dhBE)
hhB := math.FAdd(hhB, dhABC)
hhB := math.FAdd(hhB, hB)
hhC := math.FAdd(hBasis, dhCD)
hhC := math.FAdd(hhC, dhABC)
hhC := math.FAdd(hhC, hC)
hhD := math.FAdd(hBasis, dhCD)
hhD := math.FAdd(hhD, dhDEF)
hhD := math.FAdd(hhD, hD)
hhE := math.FAdd(hBasis, dhBE)
hhE := math.FAdd(hhE, dhDEF)
hhE := math.FAdd(hhE, hE)
hhF := math.FAdd(hBasis, dhAF)
hhF := math.FAdd(hhF, dhDEF)
hhF := math.FAdd(hhF, hF)

rA := math.FSqr(math.FAdd(math.FMul(xA, xA), math.FMul(yA,yA)))
rB := math.FSqr(math.FAdd(math.FMul(xB, xB), math.FMul(yB,yB)))
rC := math.FSqr(math.FAdd(math.FMul(xC, xC), math.FMul(yC,yC)))
rD := math.FSqr(math.FAdd(math.FMul(xD, xD), math.FMul(yD,yD)))
rE := math.FSqr(math.FAdd(math.FMul(xE, xE), math.FMul(yE,yE)))
rF := math.FSqr(math.FAdd(math.FMul(xF, xF), math.FMul(yF,yF)))

'~~~~~ BERECHNUNG SERVOWINKEL ALPHA GAMMA PHI
'----- Länge von Bein A ----
ca_2 := math.FAdd(math.FMul(hhA, hhA), math.FMul(rA,rA))
ca := math.FSqr(ca_2)
'----- Winkel von Hüfte A0 ----
phi := math.Degrees(math.Atan2(xA, yA))
phi := math.FAdd(math.FMul(math.FSub(phi, 5.0), 8.0), 809.0)
Aziel[0] := math.FRound(phi)
Aziel[0] := Aziel[0] +0
'----- Winkel Schulter A1 ----
alpha_P := math.ATan(math.FDiv(rA, hhA))
alpha_PP := math.ACos(math.FDiv(math.FAdd(aa_m_bb, ca_2), math.FMul(ax2, ca)))
alpha := math.Degrees(math.FAdd(alpha_P, alpha_PP) )
Aziel[1] := math.FRound(math.FAdd(math.FMul(math.FSub(alpha, 5.0), 8.0),809.0))
Aziel[1] := Aziel[1] - 60
'----- Winkel Fuss A2 ----
gamma := math.ACos(math.FDiv(math.FSub(aa_p_bb, ca_2), abx2))
gamma := math.Degrees(gamma)
gamma := math.FAdd(math.FMul(math.FSub(gamma, 5.0), 8.0), 809.0)
Aziel[2] := math.FSub(2910, math.FRound(gamma))
Aziel[2] := Aziel[2] +80

'---_- Länge von Bein B ----
ca_2 := math.FAdd(math.FMul(hhB, hhB), math.FMul(rB,rB))
ca := math.FSqr(ca_2)
'---_- Winkel von Hüfte B0 ----
phi := math.Degrees(math.Atan2(xB, yB))
phi := math.FAdd(math.FMul(math.FSub(phi, 5.0), 8.0), 809.0)
Bziel[0] := math.FRound(phi)
Bziel[0] := Bziel[0] +40
'----- Winkel Schulter B1 ----
alpha_P := math.ATan(math.FDiv(rB, hhB) )
alpha_PP := math.ACos(math.FDiv(math.FAdd(aa_m_bb, ca_2), math.FMul(ax2, ca)))
alpha := math.Degrees(math.FAdd(alpha_P, alpha_PP) )
Bziel[1] := math.FRound(math.FAdd(math.FMul(math.FSub(alpha, 5.0), 8.0),809.0))
Bziel[1] := Bziel[1] -40
'----- Winkel Fuss B2 ----
gamma := math.ACos(math.FDiv(math.FSub(aa_p_bb, ca_2), abx2))
gamma := math.Degrees(gamma)
gamma := math.FAdd(math.FMul(math.FSub(gamma, 5.0), 8.0), 809.0)
Bziel[2] := math.FSub(2910, math.FRound(gamma))
Bziel[2] := Bziel[2] +100

'----- Länge von Bein C ----
ca_2 := math.FAdd(math.FMul(hhC, hhC), math.FMul(rC,rC))
ca := math.FSqr(ca_2)
'----- Winkel von Hüfte C0 ----
phi := math.Degrees(math.Atan2(xC, yC))
phi := math.FAdd(math.FMul(math.FSub(phi, 5.0), 8.0), 809.0)
Cziel[0] := math.FRound(phi)
Cziel[0] := Cziel[0] +60
'----- Winkel Schulter C1 ----
alpha_P := math.ATan(math.FDiv(rC, hhC) )
alpha_PP := math.ACos(math.FDiv(math.FAdd(aa_m_bb, ca_2), math.FMul(ax2, ca)))
alpha := math.Degrees(math.FAdd(alpha_P, alpha_PP) )
Cziel[1] := math.FRound(math.FAdd(math.FMul(math.FSub(alpha, 5.0), 8.0),809.0))
Cziel[1] := Cziel[1] +80
'----- Winkel Fuss C2 ----
gamma := math.ACos(math.FDiv(math.FSub(aa_p_bb, ca_2), abx2))
gamma := math.Degrees(gamma)
gamma := math.FAdd(math.FMul(math.FSub(gamma, 5.0), 8.0), 809.0)
Cziel[2] := math.FSub(2910, math.FRound(gamma))
Cziel[2] := Cziel[2] +100

'----- Länge von Bein D ----
ca_2 := math.FAdd(math.FMul(hhD, hhD), math.FMul(rD,rD))
ca := math.FSqr(ca_2)
'----- Winkel von Hüfte D0 ----
phi := math.Degrees(math.Atan2(xD, yD))
phi := math.FAdd(math.FMul(math.FSub(phi, 5.0), 8.0), 809.0)
Dziel[0] := math.FSub(2910, math.FRound(phi))
Dziel[0] := Dziel[0] +80
'----- Winkel Schulter D1 ----
alpha_P := math.ATan(math.FDiv(rD, hhD))
alpha_PP := math.ACos(math.FDiv(math.FAdd(aa_m_bb, ca_2), math.FMul(ax2, ca)))
alpha := math.Degrees(math.FAdd(alpha_P, alpha_PP) )
Dziel[1] := math.FRound(math.FAdd(math.FMul(math.FSub(alpha, 5.0), 8.0),809.0))
Dziel[1] := math.FSub(2910, Dziel[1])
Dziel[1] := Dziel[1] +5
'----- Winkel Fuss D2 ----
gamma := math.ACos(math.FDiv(math.FSub(aa_p_bb, ca_2), abx2))
gamma := math.Degrees(gamma)
gamma := math.FAdd(math.FMul(math.FSub(gamma, 5.0), 8.0), 809.0)
Dziel[2] := math.FRound(gamma)
Dziel[2] := Dziel[2] -80

'----- Länge von Bein E ----
ca_2 := math.FAdd(math.FMul(hhE, hhE), math.FMul(rE,rE))
ca := math.FSqr(ca_2)
'----- Winkel von Hüfte E0 ----
phi := math.Degrees(math.Atan2(xE, yE))
phi := math.FAdd(math.FMul(math.FSub(phi, 5.0), 8.0), 809.0)
Eziel[0] := math.FSub(2910, math.FRound(phi))
Eziel[0] := Eziel[0] +70
'----- Winkel Schulter E1 ----
alpha_P := math.ATan(math.FDiv(rE, hhE))
alpha_PP := math.ACos(math.FDiv(math.FAdd(aa_m_bb, ca_2), math.FMul(ax2, ca)))
alpha := math.Degrees(math.FAdd(alpha_P, alpha_PP) )
Eziel[1] := math.FRound(math.FAdd(math.FMul(math.FSub(alpha, 5.0), 8.0),809.0))
Eziel[1] := math.FSub(2910, Eziel[1])
Eziel[1] := Eziel[1] -20
'----- Winkel Fuss E2 ----
gamma := math.ACos(math.FDiv(math.FSub(aa_p_bb, ca_2), abx2))
gamma := math.Degrees(gamma)
gamma := math.FAdd(math.FMul(math.FSub(gamma, 5.0), 8.0), 809.0)
Eziel[2] := math.FRound(gamma)
Eziel[2] := Eziel[2] +0

'----- Länge von Bein F ----
ca_2 := math.FAdd(math.FMul(hhF, hhF), math.FMul(rF,rF))
ca := math.FSqr(ca_2)
'----- Winkel von Hüfte F0 ----
phi := math.Degrees(math.Atan2(xF, yF))
phi := math.FAdd(math.FMul(math.FSub(phi, 5.0), 8.0), 809.0)
Fziel[0] := math.FSub(2910, math.FRound(phi))
Fziel[0] := Fziel[0] +50
'----- Winkel Schulter F1 ----
alpha_P := math.ATan(math.FDiv(rF, hhF))
alpha_PP := math.ACos(math.FDiv(math.FAdd(aa_m_bb, ca_2), math.FMul(ax2, ca)))
alpha := math.Degrees(math.FAdd(alpha_P, alpha_PP) )
Fziel[1] := math.FRound(math.FAdd(math.FMul(math.FSub(alpha, 5.0), 8.0),809.0))
Fziel[1] := math.FSub(2910, Fziel[1])
Fziel[1] := Fziel[1] -100
'----- Winkel Fuss F2 ----
gamma := math.ACos(math.FDiv(math.FSub(aa_p_bb, ca_2), abx2))
gamma := math.Degrees(gamma)
gamma := math.FAdd(math.FMul(math.FSub(gamma, 5.0), 8.0), 809.0)
Fziel[2] := math.FRound(gamma)
Fziel[2] := Fziel[2] -0

move

i:=0
repeat 3
Alast[i] := Aziel[i]
Blast[i] := Bziel[i]
Clast[i] := Cziel[i]
Dlast[i] := Dziel[i]
Elast[i] := Eziel[i]
Flast[i] := Fziel[i]
i++

pub busy_led
!outa[26] ' Ausgang invertieren
waitcnt(3_000_000 + cnt) ' 300ms warten

pub ready_led
outa[26]~~ ' LED ON
waitcnt(100_000_000 + cnt)
outa[26]~


pub init|i
math.start

'------------------Feste-Parameter----------------------------------------------------------------------------------------
periode := ((clkfreq/1000000)*20000) 'Länge der gesamten Periode
PinStart_r:=|<16
PinStart_l:=|<7
dira[7..15]~~
pin_l:= dira
dira[16..24]~~
pin_r:= dira

dira[27]~~ 'Ausgang für LED - Gelb
dira[26]~~ 'Ausgang für LED - Grün
dira[25]~ 'Eingang Button
dira[6]~

'Initialposition
Aini[0] := 1490
Aini[1] := 1490
Aini[2] := 1490
Bini[0] := 1490
Bini[1] := 1490
Bini[2] := 1490
Cini[0] := 1490
Cini[1] := 1490
Cini[2] := 1490
Dini[0] := 1490
Dini[1] := 1490
Dini[2] := 1490
Eini[0] := 1490
Eini[1] := 1490
Eini[2] := 1490
Fini[0] := 1490
Fini[1] := 1490
Fini[2] := 1490


'Min, Max - Werte für Servobewegung und Positionen anfahren.
i:=0
repeat 3
Apos[i] #>= 809 ' Untere Servogrenze
Apos[i] <#= 2169
Alast[i] := Aini[i]
Apos[i] := Aini[i]

Bpos[i] #>= 809 ' Untere Servogrenze
Bpos[i] <#= 2169
Blast[i] := Bini[i]
Bpos[i] := Bini[i]

Cpos[i] #>= 809 ' Untere Servogrenze
Cpos[i] <#= 2169
Clast[i] := Cini[i]
Cpos[i] := Cini[i]

Dpos[i] #>= 809 ' Untere Servogrenze
Dpos[i] <#= 2169
Dlast[i] := Dini[i]
Dpos[i] := Dini[i]

Epos[i] #>= 809 ' Untere Servogrenze
Epos[i] <#= 2169
Elast[i] := Eini[i]
Epos[i] := Eini[i]

Fpos[i] #>= 809 ' Untere Servogrenze
Fpos[i] <#= 2169
Flast[i] := Fini[i]
Fpos[i] := Fini[i]
i++
setpulse


' ################################################## ################################################## ##
' ##################### EBENE 0 ################################################## #####################
' ################################################## ################################################## ##

pub setpulse|i,j 'Bewegen der Servos, berechnen Impulslänge
new_peri_r:= periode
new_peri_l:= periode

'-----------------LINKE SEITE-----------------------------------------------------
i:=0
j:=0
repeat 3
high_pulse_l[i] := ((clkfreq/ 1000000) * Fpos[j]) 'aktuelle pulselänge
new_peri_l-=high_pulse_l[i]
i+=1
j+=1
j:=0
repeat 3
high_pulse_l[i] := ((clkfreq/ 1000000) * Epos[j]) 'aktuelle pulselänge
new_peri_l-=high_pulse_l[i]
i+=1
j+=1
j:=0
repeat 3
high_pulse_l[i] := ((clkfreq/ 1000000) * Dpos[j]) 'aktuelle pulselänge
new_peri_l-=high_pulse_l[i]
i+=1
j+=1

'-----------------RECHTE SEITE-----------------------------------------------------
i:=0
j:=0
repeat 3
high_pulse_r[i] := ((clkfreq/ 1000000) * Cpos[j]) 'aktuelle pulselänge
new_peri_r-=high_pulse_r[i]
i+=1
j+=1
j:=0
repeat 3
high_pulse_r[i] := ((clkfreq/ 1000000) * Bpos[j]) 'aktuelle pulselänge
new_peri_r-=high_pulse_r[i]
i+=1
j+=1
j:=0
repeat 3
high_pulse_r[i] := ((clkfreq/ 1000000) * Apos[j]) 'aktuelle pulselänge
new_peri_r-=high_pulse_r[i]
i+=1
j+=1

DAT 'Ausgeben der Pulse

ORG 0
moveServo mov Index, par 'die adresse des cograms wird nach index geschreiben

[HannoHupmann]

Gestern habe ich festgestellt, dass man die Programmierschnittstelle (RS232) des Propeller Chips, nach dem flashen auch weiter verwenden kann und darüber Daten zurück an den PC schicken (die entsprechenden Bibliotheken sind schon fertig). Die Entwicklungsumgebung hat eine entsprechende Empfangskonsole bereits implementiert und damit ist der Austausch zwischen Controller und PC wirklich leicht zu realisieren.
Im Moment lass ich mir die Berechneten Werte für die einzelnen Beine zurück geben und kann damit quasi die Telemetrie des Vinculums überwachen und auch ob alles korrekt berechnet wurde. Feine Sache aber: Ich brauch dazu im Moment ein Kabel :(

Frage an die Runde: Welche einfachen und billigen Möglichkeiten gibt es um das RS232 Kabel durch Funk zu ersetzen? Die notwendige Reichweite beträgt vielleicht 10m, nur indoor und im gleichen Raum. Bidirektional wäre fein, dann könne ich auch über Funk flashen.

Ich habe jetzt das ZigBee Zeugs mal angeschaut, bin mir aber nicht sicher ob ich damit meine Datenübertragung realisieren kann. Hat da jemand mehr Erfahrung damit? Tutorials?

[robin]

Schau dir mal die NRF24L01 an, haben zwar eine SPI Schnittstelle, aber die hat der Propeller ja auch.

Vorteil,
- sie sind günstiger als Zigbee
- Automatische checksumme erzeugen und kümmern sich auch bei fehlern um das erneute senden.
- Reichweiten von wenigen Metern bis 1km möglich.
- Baudrate auf kurze distanz mit bis zu 2Mbit

[Slowly]

Wenn Du lust auf basteln hast hätte ich hier eine Unidirektionale Sache hier liegen. Von HM Funktechnik die 70 RX-M und die 70 TX-M.

Nachtrag: Hier gerade gefunden ! http://plischka.at/Funkmodul_fuer_hohe_Reichweiten.html

[HannoHupmann]

@Slowly meinst du zum ausprobieren? Ich habe bald wieder arbeit für dich, denn ich will dem Hexa noch einen Sensorkopf spendieren, dazu braucht es aber ein Aluminium Gestell drum herum (1,5mm Stärke).

Ich habe schon gesehen, dass mein Propeller Chip auch ZigBee unterstützt und diese Funkübertragung von Parallax für den Chip angeboten wird. Die Lösung wäre etwas billiger, da ich eigentlich keine 200€ für eine Spielerei ausgeben will. Es gibt keine Notwendigkeit das Signal über Funk zu schicken, es wäre nur bequem und high-end :)

[Slowly]

Wenn Du es ausprobieren möchtest gerne. Es muß nur einiges an Hardware drumm herum gebastelt werden.
Sonst google mal das hier: Bluetooth Transceiver RF Module (https://www.google.de/search?client=firefox-a&hs=u7T&rls=org.mozilla:de:official&q=Bluetooth+Transceiver+RF+Module&spell=1&sa=X&ei=A-BfUvz3OIzWsgafmIDgDg&ved=0CCsQvwUoAA) Billiger geht kaum.

[HannoHupmann]

Stimm die Dinger sind wirklich recht günstig. Leider hat mein Rechner kein Bluetooth, d.h. ich müsste dazu noch einen kleinen USB-Bluethooth Stick kaufen. Wird damit auch nicht recht viel teurer. Frage ist nur ob es auf diese Art Funktioniert und mein µC und mein PC sich darüber einig werden, dass das RS232 Kabel jetzt eine Funkstrecke ist.

Und welchen der vielen dieser Bluetooth soll ich nehmen:
http://www.ebay.de/itm/like/400565336951?lpid=91&_configDebug=ViewItemDictionary.ENABLE_PAYMENTS_IN _HLP:true

[Slowly]

Ich würde in einem extra Thread die RN User fragen, ob jemand schon eine Lowcost Übertragung mit solchen Module realisert hat.

[patti16]

Hallo,

ich wollte mir nächste Woche diese Modul (http://www.ebay.com/itm/1pcs-RS232-TTL-HC-05-Wireless-Bluetooth-Transceiver-Module-/110929280210?pt=LH_DefaultDomain_0&hash=item19d3e67cd2) bei Ebay bestellen. Kostenlose Samples bietet dieser Ebay Shop auch an.
Lieferung ist kostenlos und sehr schnell.

Gruß

[HannoHupmann]

Patti16, dann bin ich mal gespannt auf deine Ergebnisse :)

[malthy]

ich wollte mir nächste Woche diese Modul (http://www.ebay.com/itm/1pcs-RS232-TTL-HC-05-Wireless-Bluetooth-Transceiver-Module-/110929280210?pt=LH_DefaultDomain_0&hash=item19d3e67cd2) bei Ebay bestellen

Hab mir zufällig auch gerade einen Satz (2 Stück) geordert. Mal sehen wann ich dazu komme damit mal was zu machen :-).
Aber wie kommst du darauf dass die Lieferung sehr schnell ist? Ich erwarte dass es die China-üblichen 2-4 Wochen werden.

[patti16]

Die letzte Lieferung hat genau 7 Werktage gedauert. Finde das schnell, das schaffen manche deutsche Firmen nicht.

[malthy]

echt? bei e_goto (http://myworld.ebay.com/e_goto?_trksid=p2047675.l2559)? Das wäre ja cool, hab genau da bestellt. Naja, werden wir sehen :-).

[HannoHupmann]

So heute habe ich einen ersten Test gemacht, noch ohne Akkus:


http://www.youtube.com/watch?v=MmrXHpt4-2o&amp;feature=youtu.be

Nur gerade aus mit drei verschiedenen Laufversionen, wobei ein bis drei Beine gleichzeitig gehoben werden. Wie man sieht noch etwas wakelig, was wohl an der Positionierung der Beine liegt und wie diese sich bewegen. Ich muss mal kucken wie sich das noch optimieren lässt.
In der Software muss ich nur die Geschwindigkeit angeben, die notwendigen Änderungen nimmt das Programm automatisch vor.

[HannoHupmann]

Über Weihnachten und Co ist es leider ein wenig ruhiger geworden und mein Projekt, aber jetzt finde ich langsam wieder Zeit mich damit zu beschäftigen. Nicht so viel wie gewünscht, da ich nebenbei noch ein anderes kleines Modellbau Projekt verwirklichen will, aber genug um weiter zu kommen.

Leider hat mein gutes, altes Labornetzteil ein paar Wackelkontakte bekommen und ist damit nicht mehr so stabil und zuverlässig in der Spannungsversorgung wie früher. Was mich auch dazu gebracht hat (bei der Fehlersuche, wer vermutet auch, dass das Netzteil die Ursache ist) die Netzteil-Platine neu zu überdenken. Die alten Festspannungsregler 78xx für 3,3V und 5V fliegen raus und werden durch zwei Schaltregler von RECOM ersetzt. Die können auch mit 12V Eingangsspannung arbeiten, was es etwas einfacher macht den Hexa am Netz zu betreiben. Außerdem werde ich auf das Kaskadieren verzichten und beide Regler direkt an der Eingangsspannung versorgen. Natürlich bleibt auch weiterhin der Step-Up Wandler auf 12V bestehen.
Auf der Platine wird auch gleich der GND Anschluss für die Servos vorgesehen.

Die Platine ist jetzt fertig layoutet (einseitig) und gezeichnet, aber jetzt muss ich wieder jemanden finden, der sie mir kostengünstig ätzt und bohrt. :-(

[Staind]

Wenn die Platine nicht 3000 Bohrungen hat mache ich dir die wohl fertig.

[HannoHupmann]

Hallo Staind, nein es ist eine ganz einfach Platine, deswegen will ich ja auch nicht viel Geld ausgeben :-D.

Hier der entspreche Thread dazu mit einem Bild:
https://www.roboternetz.de/community/threads/64040-Platine-einseitig-(einfach)-%C3%A4tzen?p=593196#post593196

Layout kann ich als Eagle Dateien zur Verfügung stellen.

[HannoHupmann]

Eine Frage in die Runde, im Moment bin ich mit dem Material aus dem meine Fussspitzen sind nicht glücklich. Zwar handelt es sich um eine Anti-Rutschmatte fürs Auto die auch einen ganz tollen Gripp aufweist, aber sie ist flach :-(. Wir der Fuss nun leicht gekippt, dann verringert sich die Aufflagefläche extrem. Daher bin ich jetzt auf der Suche nach einer rutschfesten Halbkugel-Lösung nur will mir nichts rechtes einfallen. Die Grundfläche der Fussspitze ist 20x20mm und viereckig. Ich dachte für den Moment einen halben Flummi aber das Zeug scheint es gar nicht mehr zu geben, denn die Kinder von heute haben wohl andere Interessen.

[Slowly]

Auf die Gefahr hin das ich hier wieder gesperrte werde... ;-)
Schau bei Ebay unter
Gummikugeln - Cal 68LG
Slow

[HeXPloreR]

Ein einfaches Radiergummie Orange - oder halb Transparen (http://www.google.de/imgres?sa=X&biw=1920&bih=914&tbm=isch&tbnid=Lti4ciqKsdcM9M%3A&imgrefurl=http%3A%2F%2Fwww.officio.de%2Fbueroartik el%2Flaeufer-radiergummi.742327.html&docid=nlwXCUzPR26D9M&imgurl=http%3A%2F%2Fmedia.officio.de%2Fvollbild%2F 7%2F742327A1.jpg&w=300&h=300&ei=P6HiUr2mNMGatQaCsICYAQ&zoom=1&iact=rc&dur=656&page=1&start=0&ndsp=49&ved=0CGgQrQMwBQ)t. Gibt es beim Bürobedarf, kannst Du Dir hinschnitzen wie Du sie brauchst. ...und doch doch, Flummies gibt es noch normal zu kaufen, sogar im Six-Pack...z.B. beim real oder Euroladen - weiß ich jetzt nicht mehr ;)

[Geistesblitz]

Würde vielleicht sowas wie Gehäusefüße gehen? Sind ziemlich rutschfest und lassen sich leicht montieren, haben allerdings keine halbrunde Form.

Hätte fast schon gesagt, ich könnte dir etwas in Wunschform aus Silikon gießen, allerdings hab ich das gerade mal getestet, das Zeug ist nicht annähernd so rutschfest wie Gummi.

[rolber]

Hallo Hanno!

Das könnte Dich interessieren:
http://www.google.de/imgres?imgurl=http://www.zinorm.de/bilder/gummipuffer/gummipuffer.gif&imgrefurl=http://www.zinorm.de/gummipuffer-und-bumper.php&h=424&w=625&sz=70&tbnid=peVCxTwAittQ-M:&tbnh=90&tbnw=133&zoom=1&usg=__ir9eDA4G3TjQ--oqEtwtXXMWf5s=&docid=1AVlDLeD_0b4zM&sa=X&ei=qr7iUqX8MsHDhAet1IFY&ved=0CFUQ9QEwAg&dur=132
Würde den Kegelförmigen nehmen.
Hoffe Dir geholfen zu haben

MfG

Roland

[HannoHupmann]

@rolber, die Idee ist wirklich gut, nur bei den Gummidingern habe ich gerade in bisschen die Sorge, dass die sehr glatt sind und damit fast keine Haftreibung erzeugen, sprich nur dämpfen.

[rolber]

Habe für meinen neuen Hexa die Puffer Typ "D" mit Gewindebolzen gewaehlt.
Habe mal die Haftung auf Laminat getestet. Rutscht gar nicht.
Ist viel besser als meine alte Konstruktion aus Ventilkappen!
Reicht für meine Anforderungen allemal aus.
Das Material ist in etwa vergleichbar mit einem weichen Radiergummi. ohne das der hohe Abrieb erzeugt wird.
Auf feuchtem Untergrund könnte es schon rutschen, aber dafür ist mein Bot eh nicht vorgesehen.

MfG

[HannoHupmann]

Gut ich habe jetzt bei Ebay welche gekauft, aber keine Ahnung ob das Material etwas taugt oder nicht. Ich lass es jetzt drauf ankommen, teuer wars nicht, alternativ kann ich ja immer noch die Typ D bestellen, wobei mir eine runde oder ovale Form lieber wäre.

Weiter bin ich gerade am aufbauen der neuen Netz-Platine, was soweit auch schon fast fertig ist, es fehlen aber noch ein paar Kondensatoren, die ich überraschenderweise nicht im "Lager" hatte.

[HannoHupmann]

Da sich vielleicht einige fragen, warum es nicht weiter geht, hier die Erklärung:
Ich wollte ein "nicht Roboter-Projekt" zwischen drin fertig bauen:

Revell Voyager Modell mit Beleuchtung:

276702767127672

Das gute Stück ist etwa 50cm lang, also ordentlich groß. Individuell bemahlt und die Beleuchtung ist quasi ein Eigenbau.


Natürlich gibt es aber auch ein paar News zum Vinculum, denn auch hier hat sich ein bisschen was getan:
- Die Primäre Energieversorgung (um in der Terminologie von StarTrek zu bleiben) wurde modifiziert durch neue EPS Verteiler (DC/DC Wandler statt Festspannungsregler).
Jetzt kann ich die Logik auch mit 12V betreiben und damit direkt am Netzteil. Bisher musste ich für den Testbetrieb immer zwei Spannungen bereit stellen: Servos + Logik, jetzt geht es mit einer. Nur im Akkubetrieb wird weiterhin 9V und 7,2V verwendet. Die Modifikation muss noch getestet werden.

- Die Sensorphalanx wurde soweit fertig geplant und befindet sich in der Umsetzung. Hier fehlen noch diverse Teile und jede Menge Programmzeilen!

- Vermutlich hat sich mal wieder ein Fußservo verabschiedet (schwergängig bei manueller Bewegung, war bei den letzten immer ein Zeichen für einen Motorschaden) , das wäre nicht weiter verwunderlich, denn die ausgewählten Servos sind leider alles andere als ausfallsicher. Ggf. muss hier noch eine größere Modifikation der Füße gemacht werden, um andere Servos einbauen zu können :-(

Nächste Aufgaben:
- IK Mathematik im Programm überprüfen, hier stimmt wohl die berechnete Position mit der Tatsächlichen nicht ganz über ein.
- Umfangreiche Lauf-Tests
- Gyroskop auslesen über i2c
- Sensoren auslesen über i2c --> AD Wandler

[rolber]

Hallo Hanno!

Was meinst Du denn mit Sensorphalanx? Oder habe ich da was überlesen!
Bei den Servos muss ich dir Recht geben: Wenn man merkt, daß sie zu einem grundsätzlichen Problem werden, weg damit und Andere einsetzen.
Habe aber bei meinem Sensobot 3 auch das Problem gehabt, als ich von MG 995 auf MG 946R umgestiegen bin, daß sich auch kurz darauf 3 der neuen Servos innerhalb eines Monats verabschiedet haben.
Danach ist nun fast 2 Jahre gar Nichts passiert!
Was mir auch aufgefalllen ist: Es ist besser die Servos in einem 3kg HEXA, so wie unsere nun mal sind, nicht länger als 1Std. kontinuierlich laufen zu lassen!
Die werden doch schon echt warm.
Mal abwarten was bei meinem Neuen so passiert. Bin im Moment mit den Metallarbeiten fast zum Abschluss gekommen.
Danach gehts an die Verkabelung. Bis sich da was bewegt, gehen noch einige Wochen ins Land.
Denke auch ab und an darüber nach, auf " Hochvolt - Servos " umzusteigen, die direkt an einem 2S Lipo betrieben werden können.
Nur um die Problematik der Spannungsversorgung zu vereinfachen.

MfG

Roland

[HannoHupmann]

Mit der Spannungsversorgung habe ich soweit keine Probleme mehr, da läuft meine Konzept aus SBEC und Kondensatoren sehr stabil. Auch die 3,3V+5V+12V Versorgung ist jetzt sauber gelöst. Die Bluebird Servos sind nur elektronisch und vom Motor her nicht besonders gut geeignet für das was ich damit mache. Da sind jetzt schon sehr viele ausgefallen, nur leider verwende ich eine "Sonderabmessung" womit es schwierig wird einen anderen Typ einzubauen.

Mit Sensorphalanx meine ich ein kleines Drehgestell auf und an dem ich alle Sensoren befestigen werde. Im Moment ist das ein 3m Polaroid Sensor, 3x Ultraschall und 2x IR. Hinzu kommt noch ein Lagesensor und Stromüberwachung für den Akku, alles wird über i²c angebunden, wobei ich bisher noch keinen dieser Sensoren angesteuert habe. Das ist immer der schwierigste Teil die Sensoren dazu zu bringen ihre Informationen zu übertragen.

[HannoHupmann]

Tja wie immer klappt es mit dem i2c nicht wie gewünscht, leider finde ich auch nirgends den Hexcode für den Baustein. Ich verzweifel mal wieder an meinem i2c Adapter mit dem ich alle Kommandos via Hyperterminal übertrage.

EDIT: Gelöst !! :)

[HannoHupmann]

So hier mal ein paar Bilder von dem Sensorkopf den ich mir ausgedacht habe:

2780627807
3x SRF02
2x IR
1x Polaroid

Muss nochmal nachschauen wie die einzelnen Messweiten sind, aber das ganze ist auf einem Servo montiert.
Hinzu kommt noch ein Lagesensor und ein Strommesssensor für den Batterieverbrauch.

[Slowly]

Sehr schick HannoHupmann (https://www.roboternetz.de/community/members/16349-HannoHupmann)!
Bitte weitermachen !

[rolber]

Hallo Hanno!

Was stellst Du mit dem Polaroid Sensor an, bzw. was willst und kannst Du damit auswerten?
Soll der gesamte Kopf sich dauernd drehen oder wie ist da deine Planung?

MfG

[HannoHupmann]

Mit dem Polaroid wollte ich eigentlich die weit entfernten Hindernisse erkennen, um daraus die Route die der Roboter nehmen soll zu ermitteln. Die anderen Sensoren sind mehr für den Nahbereich, also das Umfeld um den Roboter. Der Kopf wird sich nur drehen um die Umgebung zu vermessen, wärend der Schritte bleibt er in Position.

[HannoHupmann]

Heute habe ich versucht den Stromsensor LTC2945 auszulesen und lange zeit habe ich keine Werte bekommen. Leider ist mein I2C adapter nicht in der Lage spezifische Register auszulesen, so dass ich immer nur von 0 bis 32 auslesen kann. Hat ein bisschen gedauert die Werte den Richtigen Messungen zuzuordnen. Jetzt muss ich nur noch die Umrechnung der realen werte zu den Hexwerten machen.

[HannoHupmann]

Eine Frage an alle die sich mit Matlab auskennen.

Ich möchte gerne die Sensorwerte grafisch darstellen. Am liebsten in einem Plot in dem die neuen Werte eingetragen werden und der dabei von links nach rechts wandert. So dass immer die letzten 10 Messwerte zu sehen sind. So sollte es später aussehen:
27837
Im Moment kann ich über Matlab meinen USB Adapter ansteuern, die Register der Sensoren auslesen, zerlegen und die Messwerte in einer Variablen speichern.
Auch die UI aufzubauen ist kein Problem, nur bisher klappt es mit der Darstellung im Plot nicht. Vermutlich muss ich die Werte irgendwo in einem Register zwischenspeichern. Hat jemand sowas schon mal programmiert oder eine gute Referenz dafür?

[malthy]

Hallo Hanno!

Ich mach sowas auf die schnelle Tour in MATLAB meistens folgendermaßen:


function irgendeinName
ser = serial('COM14', 'BaudRate', 9600, 'DataBits', 8, 'Parity', 'none', 'StopBits', 1, ...
'ReadAsyncMode', 'continuous', 'InputBufferSize', 64, 'OutputBufferSize', 64, ...
'Terminator' , 'CR/LF', 'BytesAvailableFcnMode', 'Terminator', 'BytesAvailableFcn', @bav);

vals = zeros(500, 1);
h1 = plot(vals);
set(h1, 'YDataSource', 'vals')

fopen(ser);

function bav(obj, ~)
dat = str2num(fgetl(obj));
vals(2:500) = vals(1:499);
vals(1) = dat;
refreshdata(h1, 'caller')
end

end

Vielleicht "inspiriert" es dich etwas ... :-)

Gruß
Malte

[HannoHupmann]

Inspirierend schon, aber ich muss gestehen ich versteh es nicht was du machst. :-(

[malthy]

Moin!

Wo genau klemmt's denn? Also das Grundprinzip ist folgendes: Mit

ser = serial('COM14', 'BaudRate', 9600, 'DataBits', 8, 'Parity', 'none', 'StopBits', 1, ...
'ReadAsyncMode', 'continuous', 'InputBufferSize', 64, 'OutputBufferSize', 64, ...
'Terminator' , 'CR/LF', 'BytesAvailableFcnMode', 'Terminator', 'BytesAvailableFcn', @bav);

wird ein serial-Objekt erzeugt, also eine Möglichkeit zum Senden oder Empfangen über die serielle Schnittstelle geschaffen. Im Beispiel ist es eben die COM14, die verwendet wird. Wenn du konkret eine andere "Datenquelle" hast - vermutlich nichtmal einen COM Port - musst du das natürlich entsprechend anpassen/ändern. So wie das Objekt erzeugt wird, triggert es einen sogenannten Callback, wenn immer im Datenstrom ein 'CR/LF' auftaucht. Der Callback heißt in diesem Falle bav (bytes available), ich übergebe dem serial-Objekt das Funktionshandle @bav der Funktion bav. Klingt vielleicht erstmal kompliziert, ist es überhaupt nicht. Was passiert ist einfach, dass die folgende Funktion bav immer aufgerufen wird, sobald Daten seriell angekommen sind (terminiert durch 'CR/LF')


function bav(obj, ~)
dat = str2num(fgetl(obj));
vals(2:500) = vals(1:499);
vals(1) = dat;
refreshdata(h1, 'caller')
end

die Zeile "dat = str2num(fgetl(obj));" holt nun die ASCII Daten und wandelt sie in einen numerischen Typ um (double), die Daten stehen dann in dat. Die von dir angefragte Animation ist jetzt ganz simpel gemacht. 500 Datenpunkte stehen im Vektor "vals" (der Vektor wird mit 500 Nullen initialisiert), die ersten 499 Werte werden im Vektor um eine Stelle nach hinten geschoben "vals(2:500) = vals(1:499);" und an die erste Stelle der neue Wert geschrieben "vals(1) = dat;". Der Clou beim Plotten ist jetzt, dass für den Plot "h1 = plot(vals);" mittels "set(h1, 'YDataSource', 'vals')" als Datenquelle für die y-Werte der Vektor "vals" angegeben wurde. Das hat zur Folge, dass wann immer "refreshdata(h1, 'caller')" anufgerufen wird, die Daten im Diagramm upgedatet werden. Das hat den Effekt, dass mit jedem Eintreffen eines neuen Datums über die Schnittstelle der Graph um eine Stelle verschoben wird. Bei einem kontinuierlichen Datenstrom führt das natürlich dazu, dass der Graph quasi durchs Diagramm scrollt, links immer der aktuellste Wert. Die Daten als ASCII zu empfangen ist natürlich nicht sehr elegant, bei niedrigen Datenraten funktioniert das aber eben sehr einfach. Vermutlich ist genau der Teil aber auch eh anders bei dir, ich wollte nur eine einfache Varinate zeigen, um das Scrollen zu bewerkstelligen.

Gruß
Malte

[HannoHupmann]

Im Prinzip habe ich die Funktion ja schon, das einzige was ich nicht schaffe ist in der UI die x Achse zu verlänger. Soll heißen im Moment wird 5; 5.5; 6; 6,5; 7 angezeigt. Da ich nur zu allen vollen Zahlen einen neuen Wert abspeichere wird nur das x bei 6 angezeigt, denn 5 liegt genau auf der Achse und 7 ist noch nicht gemessen

[malthy]

Ich weiß nicht genau ob ich dich verstehe. Grundsätzlich ist es eine gute Lösung alle Datenpunkte die du darstellen willst in einen Vektor zu nehmen und den dann zu plotten - und wenn sich das Dargestellte änderen soll eben den Vektor als Datenquelle zu ändern und ihn dann nach jeder Änderung neu zu plotten. Für das "neu plotten" bietet Matlab mit "DataSource" eine ganz elegante Lösung, das ist die Idee bei meinem Beispiel oben.

Du kannst auch mit "hold on (http://www.mathworks.de/de/help/matlab/ref/hold.html)" dafür sorgen, dass die Datenpunkte des vormaligen Plottens erhalten bleiben. Dann müsstest du aber mit plot(x,y) plotten und x jeweils inkrementieren. Dann wird deine x-Achse aber einfach immer länger und es scrollt nicht.

[HannoHupmann]

Auch wenn es nicht so scheint, es geht weiter. Gerade in der letzten Zeit habe ich sehr viele Stunden damit verbracht drei weitere Platinen zu Layouten. Die benötige ich für die Verbindung zwischen Körper, Deckplatte und Sensorkopf. Da die Deckplatte mit dem Sensorkopf leicht zu entfernen sein soll, braucht es eine Adapterplatine um alle Anschlüsse auf ein Flachbandkabel (40pins) zu legen. Später kann man einfach dieses Kabel ausstecken und den "Deckel" abnehmen. Von dort wird ein weiteres Kabel zum Sensorkopf hinauf geführt.

Dieser hat eine Platine mit i²c - AD Wandler (4Stück für die 3 IR Sensoren), die Steuerplatine für den Polaroid Sensor und eine Verteiler-Leiste für die 3 SRF03 US-Sensoren. Nichts kompliziertes, sieht man davon ab, dass ich alle Platinen einseitig layouten wollte.

Den Aufbau zeige ich dann noch in Bildern, wenn ich alle Teile bekommen habe.

[HannoHupmann]

So die Platinen sind gekommen und zum Teil auch schon gelötet (zumindest alle das SMD Zeugs). Leider muss ich bei Reichelt noch ein paar Stecker bestellen bevor ich auch den Rest zusammenlöten kann.

Nächste große Aufgabe wird es sein das gute Stück sicher für den Transport zu verpacken, da ich mit alle meinen Roboter und Co ende Mai umziehen werde. Mal sehen wie viel ich bis dahin noch basteln kann.

[HannoHupmann]

Nach dem Umzug bin ich jetzt endlich wieder dazu gekommen mich mit meinen Robotern zu beschäftigen. Da ich kurz vorher noch einen Robonova erstanden habe, wächst die Zahl der offenen Projekte auf drei! Im Moment überlege ich noch, was man lustiges aus dem Robonova machen könnte, natürlich bietet es sich an mit den 18 Servos einen weiteren Hexa aufzubauen, aber das wäre dann Nr. 5 und mit Vinculum bin ich noch gut beschäftigt.

Bei diesen habe ich nun endlich die Zeit gefunden die Inverse Kinematik zu überprüfen. Alle Werte einzeln durchgegangen und mit meinen Berechnungen verglichen. Bis auf ein kleines, mir unerklärliches Problem stimmen alle Werte.
Gebe ich nur eine translatorische Bewegung vor, dann werden die Y-Werte nicht korrekt berechnet, hier schleicht sich eine Abweichung vom Sollwert ein, die ich mir nicht erklären kann. Je weiter die Schritte von der Mitte entfernt sind um so größer wird sie.
Ich vermute es liegt an der Sinus-Berechnung. Das allein wäre noch nicht erwähnenswert, aber sobald sich rotatorische und translatorische Bewegung überlagern, werden alle Werte korrekt berechnet.

[Geistesblitz]

Hört sich so an, als ob sich da noch ein Fehler herumschleicht. Hast du die Formeln für translatorische und kombinierte Bewegung mal verglichen?

[HannoHupmann]

Eigentlich sollte da kein Fehler sein, denn beide verwenden die selben Code-Zeilen, später wird nur translatorisch und rotatorisch aufsummiert. Die Summe stimmt dann wieder. Was irgendwie schon merkwürdig ist. Macht aber zum Glück nicht viel aus, so dass ich damit kaum ein Problem sehe und erst mal weiter machen kann. Die maximale Abweichung beträgt im Moment 24mm von Soll zu Ist Position und auch nur für die Extremstellungen, ganz außen bzw. ganz innen, also für sehr wenige Zustände.

[Geistesblitz]

24mm ist ja schon ein ganz schönes Stück. Und in der Mittelstellung ist alles in Ordnung? Das ist wirklich ein kurioses Problem...

[HannoHupmann]

Kurios ist es, dass bei der Berechnung der X-Koordinaten nur ein Fehler von +/-3mm heraus kommt und um die Mittelstellung ist es 0.
Also entweder ist meine Excel sheet Falsch oder meine Berechnung im Roboter.

Wie auch immer, ich bin froh endlich weiter machen zu können und jetzt muss ich erst mal bei Reichelt bestellen. Die Steuerrückzahlung muss sinnvoll verwendet werden!

- - - Aktualisiert - - -

Thema RS232 über Funk, gab es hier nicht mal ein paar Ideen dazu und was ist daraus geworden? Ich bin wieder an dem Thema interessiert.

[HannoHupmann]

Man sollte nicht mehr Abends basteln. Hab ich doch gestern glatt Stecker an die Stellen gelötet wo eigentlich Kabel hin gehören. Zum Glück habe ich die Platine nochmal übrig, so dass ich jetzt nicht umständlich auslöten muss.

Ich bin mal gespannt ob das danach noch so funktioniert wie davor. Konkret arbeite ich gerade an Verbinungsplatinen, soll heißen: Ich habe eine Platine im Körper auf der alle Anschlüsse für den Deckel zusammen laufen, so kann ich später mit einem einzigen 40 poligen Flachbandkabel die Signale zum Deckel übertragen. Auf dem Deckel gibt es natürlich eine Gegenstelle die die Signale dann wieder an Servos, Sensoren und Co verteilt. Unteranderem einen weiteren Anschluss für ein Flachbandkabel hinauf zum Sensorkopf.
Im Prinzip nichts anderes als Steckverbindungen um später einfacher nur noch jeweils einen Stecker lösen zu müssen.

Nichts kompliziertes, aber die Verbindungen müssen natürlich alle geprüft werden.

[HeXPloreR]

Ich arbeite quasi nur Abends und Nachts an meinem Hexapod ...das geht bisher ganz gut. Tagsüber bekomme ich den Kopf dazu nicht frei ;)

Hoffe Du denkst dran die Stecker nicht alle gleich (Form) aufzubauen...nicht das man nachher die Platinen umdreht und doch lustig einstecken kann obwohl der Anschluss nicht an diese Platine gehört ;)

[HannoHupmann]

Mein größtes Problem diesbezüglich wird sein, dass ich einmal 10Polig für I2C-3,3CV habe und einmal I2C-5V und die natürlich sauber trennen sollte. Leider sind beide Stecker und beide Buchsen schwarz. Also Brauch ich irgend eine andere Farbcodierung für die Stecker und Buchsen. Mal sehen ob ich die irgendwie einfärben kann.

[PICture]

Mein Tipp: eine "Einstecksicherung". Zuerst bestehende Verbindung gewünscht durchbohren und danach entsprechend montierte(n) (z.B. eingeklebte(n)) Stift(e) anwenden. Falls nicht alle Stifte im Stecker benutzt sind, kann man einige davon abschneiden (optimal in der Mitte des Steckers) und dazugehörige Löcher in der Buchse z.B. mit Industriekleber stopfen.

Mann könnte auch gleiche Löcheranzahl, wie Farbe, zum Gekennzeichnen verwenden. ;)

[HannoHupmann]

@PICture, das klassische Poka-Yoke :-D. Vielleicht kann ich tatsächlich einen GND Pin opfern um deiner Idee zu folgen, sollte gehen. Farbig würde ich es aber auch noch gerne irgendwie markieren, nur fürchte ich dass auf dem Plastik kein Lack halten wird und mit Iso-Band habe ich bisher auch keine guten Erfahrungen gemacht.

Na mal sehen, im Moment komm ich eh nicht richtig weiter beim basteln :/

[movido]

Schrumpfschlauch ist sehr gut zum Markieren, funktioniert aber nur bei kleinen Steckern, über die man den umgeschrumpften Schlauch schiebt, oder vor dem einbringen der Kontakte in das Steckergehäuse.

[HannoHupmann]

@movido die Idee gefällt mir ich habe hier eh noch relativ viele bunte Schrumpfschläuche herumliegen. Damit sollte die Markierung dann ausreichend auffällig sein: Einstecksicherung und Farbig, so kann auch ein Poka - Yoke Experte nichts mehr sagen :)

Leider komm ich z.Z. nur immer für ein paar Minuten zu basteln, so sind die Fortschritte eher gering.

[HannoHupmann]

So nach langer Zeit mal wieder ein paar Bilder:
28664
Sensorkopf mit Sonar, 3x Ultraschall und 2x IR Sensoren auf einem Standard-Servo montiert. Oben ist noch ein Display des Akkuwächters und unten die Knöpfe für diverse Funktionen.

Von unten sieht der Deckel so aus:
28663
Wie man schwer erkennen kann, werden alle Verbindungen über eine 38 Polige Buchsenleiste zum Körper geführt. So muss man nur ein Kabel ausstecken und kann den Deckel abnehmen.

Auf dem Vinculum sieht es dann so aus:
28665

Hinten links sieht man zur Abwechslung mal einen Roboter den ich nicht selber gebaut habe.

Den ganzen "Arbeitsplatz" kann man hier begutachten:

https://www.roboternetz.de/community/threads/41747-Basteltische/page8

[HannoHupmann]

So endlich mal wieder was neues: Ich habs geschafft mein XBee auf dem Roboter und am PC zum laufen zu bringen. Nichts groß besonders aber damit ist er der nächste Schritt getan, um irgendwann die Telemetrie des Roboters live am PC zu sehen. Noch immer halte ich am Grundsatz fest, den Roboter nicht fernzusteuern (oder höchstens als optionale Funktion), doch ist es schön die Sensorwerte, Winkelstellungen und Rechnungsschritte der IK überwachen zu können, ohne eine Kabelverbindung.

[HannoHupmann]

Nächste Schritte:
Gestern konnte ich den Hexa beim Laufen stabilisieren indem ich die Reihenfolge der Fussbewegungen verändert habe. Dadurch kippt er jetzt nicht mehr nach vorn oder hinten weg, der Schwerpunkt liegt besser auf den stützenden Beinen. Als nächstes möchte ich noch die Bewegungen so optimieren, dass der Roboter möglichst stabil und ruckelfrei läuft. Außerdem wäre es schön eine sauberen wechsel zwischen den Geschwindigkeiten zu haben ohne die Füße neu kalibrieren zu müssen. Im Moment kann ich zwischen den drei Geschwindigkeiten wählen wobei entweder 1Bein, 2 Beine oder 3 Beine gleichzeitig in der Luft sind. Natürlich ändert sich dadurch Aufsetzpunkt und Schrittlänge. Zukünftig soll ein nahtloses Wechseln zwischen den drei Geschwindigkeiten möglich sein.

[HannoHupmann]

Hier ein kleines Video von meinem letzten Lauf-Test. Ich muss dazu sagen, dass ich die Geschwindigkeit um den Faktor 4 erhöht habe. D.h. der Roboter läuft leider nicht so schnell im Moment.
Die Ansteuerung der Servos erlaubt aktuell keine flüssigen, wirklich gleichzeitigen Bewegungen.



https://www.youtube.com/watch?v=FmpgHRKLEiI&feature=youtu.be

Die Stromversorgung läuft aktuell über Kabel da ich den Akku schonen will, dieser ist aber bereits "on Board" damit das Gewicht stimmt.