Bewegungskosmetik für Hexapods

[ikarus_177]

Hi,

ich beschäftige mich in letzter Zeit mit dem "modernen Wunderland" der inversen Kinematik für Hexapods. Das Ganze wird in Bascom realisiert. Ich habe die Kinematik auf verschiedene Funktionen aufgeteilt, von denen jede eine bestimmte Bewegung ausübt.

Die Berechnung der auftretenden Winkel sowie die Umrechnung in PWM - Werte für die Servos funktioniert auch ganz brav und ordentlich.

Die ersten Tests habe ich mit einer Funktion gemacht, die die Höhe eines Beines sowie dessen Entfernung zum Befestigungspunkt verändern kann. Zuerst habe die Funktion mit einer for-Schleife ständig aufgerufen, anfangs mit 1mm Unterschied in der Höhe. Das Bein bewegte sich auf und ab, allerdings sehr "holprig" ;-)

Also habe ich den Höhenunterschied auf einen zehntel Millimeter erniedrigt, mit dem Ergebnis, dass das Bein wild um sich schlug ;-)

Nun habe ich mir gedacht, eine zusätzliche Funktion in das Programm "einzuschleusen", welche die Servos langsam und kontrolliert an die berechnete Position annähert - mit mäßigem Erfolg.

Ich hatte mir das anfangs so vorgestellt:

- Es werden die zurückzulegenden Wege der Servos berechnet
- Der Servo mit dem kleineren Weg wird im folgenden pro Schleifendurchlauf um 1 erhöht.
- Der andere Servo wird nun genau um einen Wert erhöht, der bewirkt, dass beide Servos gleichzeitig an ihren Positionen ankommen.


Hier der dafür verantwortliche Codeschnipsel:


Smooth_driving:

Drivingbuffer(2) = Servo(2) - Servo2buffer
Drivingbuffer(2) = Abs(drivingbuffer(2))
If Drivingbuffer(2) = 0 Then Drivingbuffer(2) = 1

Drivingbuffer(3) = Servo(3) - Servo3buffer
Drivingbuffer(3) = Abs(drivingbuffer(3))
If Drivingbuffer(3) = 0 Then Drivingbuffer(3) = 1

If Drivingbuffer(2) >= Drivingbuffer(3) Then
Smin = 3
Smax = 2
Buffer = Drivingbuffer(3)
Drivingbuffer(smin) = Servo3buffer
Drivingbuffer(smax) = Servo2buffer
End If

If Drivingbuffer(3) >= Drivingbuffer(2) Then
Smin = 2
Smax = 3
Buffer = Drivingbuffer(2)
Drivingbuffer(smin) = Servo2buffer
Drivingbuffer(smax) = Servo3buffer
End If

'Assertion: smin ist der Servo, der am wenigsten Weg zurückzulegen hat, smax muss hingegen am meisten zurücklegen
'Berechnen der Schritte, die Servo(smax) zurückzulegen hat, während Servo(smin) einen Schritt macht.


For K = 0 To 1

If Servo(smin) > Drivingbuffer(smin) Then Decr Servo(smin)
If Servo(smin) < Drivingbuffer(smin) Then Incr Servo(smin)
Diff(smin) = Drivingbuffer(smin) - Servo(smin)
Diff(smin) = Abs(diff(smin))

If Servo(smax) > Drivingbuffer(smax) Then Servo(smax) = Servo(smax) - Addk
If Servo(smax) < Drivingbuffer(smax) Then Servo(smax) = Servo(smax) + Addk
Diff(smax) = Drivingbuffer(smax) - Servo(smax)
Diff(smax) = Abs(diff(smax))
Grenze(1) = Drivingbuffer(smax) - 2
Grenze(2) = Drivingbuffer(smax) + 2

Singlebuffer = Diff(smax) / Diff(smin)
Addk = Round(singlebuffer)
If Addk < 1 Then Addk = 1

'Abbruchbedingung überprüfen:

If Servo(smin) = Drivingbuffer(smin) And Servo(smax) = Drivingbuffer(smax) Then Exit For

Waitus 500

Decr K

Next

Servo(2) = Servo2buffer
Servo(3) = Servo3buffer


Return

ich hoffe, das ist jetzt einigermaßen verständlich ;-)

Meiner Meinung nach müsste das eigentlich so funktionieren - ich sitze da jetzt schon einige Wochenenden dran, ohne dass es befriedigend funktioniert.

Hat vielleicht jemand eine Idee zur Lösung des Problems?


Viele Grüße
ikarus_177


EDIT: Ich hab natürlich weiter experimentiert, und dabei die Funktion "SmoothDriving" wieder aus dem Programm entfernt, sodass jetzt die berechneten Werte (die laut Simulator stimmig sind!) direkt an die Servos weitergereicht werden. Das Intervall der Höhe wurde auf 0,1mm festgelegt, d.h. das Programm berechnet bei jedem Durchlauf eine Höhe, die um einen Zehntel Millimeter von der vorherigen Höhe abweicht. Das Servosignal, bzw. die Servowerte schauen im Simulator vollkommen korrekt aus, in der Praxis allerdings zucken die Beine wie verrückt, ohne eine erkennbare Bewegung erahnen zu lassen...
Wird das Intervall auf einen Millimeter angehoben, sind nur "hi und da" Zuckungen zu beobachten, allerdings ist der Bewegungsablauf wegen der geringeren Anzahl an Berechnungen sehr "holprig".
Könnte das Zucken gar auf Störung der Servos durch irgendwelche Störquellen zurückzuführen sein? Was würde helfen? Ferritkerne?

Ich bitte um Rat, bin momentan echt am Ende meines Lateins...

[Willa]

Das Problem ist meiner Meinung nach die "Servo" Routine von Bascom. Ich hatte genau das gleiche Problem: Wenn man einen Timer benutzt und gleichzeitig den Servo Befehl, dann zuckt alles wild rum. Also entwedre du verzichtest auf den Timer (unmöglich nehme ich an) oder du schreibst dir deine eigene Servoroutine per Timer.

[ikarus_177]

Hi Willa,

ich verwende bereits eine eigene Servoroutine mit dem 16 Bit Timer des ATMega32, die "im Alleingang" auch sehr brav und ordentlich werkelt.

Sobald aber alles in ein Programm "zusammengewürfelt" wird, funktioniert es schlecht bis gar nicht.

Ich habe aber schon einen neuen "Verdächtigen", und zwar den Stack, den ich am Standardwert von 32 Bytes gelassen habe. Wahrscheinlich ist das aber zu wenig, alleine der Interrupt für die Servoroutine verbraucht schon 32 Bytes, dazu kommen ja dann noch die 2 Bytes pro Unterfunktion. Könnte das sein? Wäre zumindest die letzte Möglichkeit, die mir einfallen würde...

Jedenfalls würde es genau in mein "Konzept" passen, wird dem Bein allerdings nur eine Position angegeben, wird diese auch brav angefahren, erst bei vielen Positionen hintereinander beginnt das Zucken. Werde das Programm mit vergrößertem Stack auf jeden Fall am Wochenende mal testen, ich bin unter der Woche nämlich nicht zu Hause.

Viele Grüße

[Willa]

erst bei vielen Positionen hintereinander beginnt das Zucken
Wie schnell werden denn die Positionen nacheinander gesendet...? Nicht zufällig schneller als 20ms? Das mit dem Stack kannst du probieren, ich glaube ich habe den bei mir auch mal erhöht und es wurde dadurch besser.

[ikarus_177]

Hi Willa,

was würde denn sein, wenn die Positionen schneller als mit 50Hz wiederholt würden? Ich hab da jetzt auch gar nicht so besonderes Augenmerk drauf gelegt, ich dachte mir einfach, für eine möglichst sanfte Bewegung sollen möglichst viele Positionen in möglichst kurzer Zeit ausgeführt werden, sodass das "Ruckeln" der Servos in Wirklichkeit nicht mehr bemerkt werden kann...

Viele Grüße

[Willa]

was würde denn sein, wenn die Positionen schneller als mit 50Hz wiederholt würden?
Die THEORIE sagt dass Servos eine Frametime von 20ms haben. D.h. sie hätten theoretisch Probleme damit wenn Signale schneller als mit 50 Hz kommen. Ich habe bisher nur ein einziges (qualitativ hochwertiges) Servo mit Frametimes von ca. 200 Hz getestet, das funktioniert in meinem Tricopter wunderbar. Ob das allerdings bei allen Servos so ist (vorallem bei billigeren Modellen) kann ich überhaupt nicht sagen. Daher würde ich es an deiner Stelle evtl. mal mit einer langsameren Signalfolge versuchen.

[ikarus_177]

Hi Willa,

ich glaube nicht, dass es daran liegt, ich verwende Digitalservos (MG995). Außerdem hatte ich die Servoroutine schon getestet gehabt, der Servowert (in Microsekunden) wird pro Millisekunde um 1 erhöht, die Bewegung war da wunschentsprechend "geschmeidig und sanft".

Viele Grüße

[Willa]

ich verwende Digitalservos (MG995)
Ich befürchte dass ich mich ständig wiederhole wenn ich sage dass schlechte Digiservos viel schlechter sind als gute Analogservos, aber ich sage es trotzdem, vielleicht glaubt mir ja eines Tages jemand.... (probiert es doch einfach mal aus!)


pro Millisekunde um 1 erhöht
Das kann ich mir nur sehr schwer vorstellen. Ich nehme an dass du weißt wie das mit dem Servosignal funktioniert. Das Signal selber ist doch nur 1-2ms lang (1.5ms = mitte) und wird alle 20 ms wiederholt. Wenn du jede ms ein neues High/Low Signal anlegst kann doch (wieder theoretisch) das Servo gar nix mehr schnallen. Poste doch mal den Code deiner Servoroutine bitte. Ich kann mir bei irgendwie nicht vorstellen dass eine Servoansteuerung mit 1000Hz (20 mal so schnell wie vorgesehen) funktioniert.Aber vielleicht ist es ja doch so - das wär prima - dann würde ich mir gerne mal den Code angucken.

...der Servowert (in Microsekunden)...
Das klingt fast so als würdest du "waitus" benutzen für die Servoansteuerung?

[ikarus_177]

Hi,

ich meinte, der Wert für den Servo, der der Funktion übergeben wird, ändert sich mit 1kHz, die Funktion selber ist so ausgelegt, 3 Servos anzusteuern. Zuerst wird der erste Servo mit dem Puls versorgt, dann der zweite und der dritte. Durch die Pulse der anderen Servos entsteht schon sowas wie eine Pause im Signal, die aber je nach Dauer der Pulse der anderen beiden Servos variabel ist. Deshalb habe ich noch 3 zusätzliche "Dummyservos" definiert, die ebenfalls angesteuert werden, und so die Pause verlängern.

Aber hier mal der Code:


$regfile = "m32def.dat" 'verwendeter Chip
$crystal = 16000000 'Taktfrequenz
$hwstack = 32 'HardwareStack
$swstack = 32 'SoftwareStack
$framesize = 32

'-----------------------------------------------------------------------'
'Timer initialisieren: '
'-----------------------------------------------------------------------'

Config Timer1 = Timer , Prescale = 8
On Timer1 Servo
Enable Timer1
Enable Interrupts


'-----------------------------------------------------------------------'
'Portdefinition: '
'-----------------------------------------------------------------------'

Config Porta.0 = Output
Servo1 Alias Porta.0

Config Porta.1 = Output
Servo2 Alias Porta.1

Config Porta.2 = Output
Servo3 Alias Porta.2


'-----------------------------------------------------------------------'
'Variblendefinition: '
'-----------------------------------------------------------------------'

Dim Servo(7) As Word 'Array für die Servowerte
Dim I As Byte 'Zählvariable
I = 1
Dim J As Word
Dim K As Byte
Dim Dummyservo As Byte
Const S = 5000


'-----------------------------------------------------------------------'
'Hauptprogramm: '
'-----------------------------------------------------------------------'

For J = 4 To 7
Servo(j) = S
Next


Gosub Enable_servos

Do

For J = 2500 To 3500

Servo(1) = J
'Servo(2) = 1500
'Servo(3) = 1500

Waitms 1

Next

Loop

Enable_servos:
Start Timer1
Return


Disable_servos:
Stop Timer1
Return


Servo:

Select Case I
Case 1 : Servo1 = 0
Case 2 : Servo2 = 0
Case 3 : Servo3 = 0
Case 4 : Dummyservo = 0
Case 5 : Dummyservo = 0
Case 6 : Dummyservo = 0
Case 7 : Dummyservo = 0

End Select

Incr I

If I = 7 Then I = 0

'-----------------------------------------------------------------------'
'Portpin des gewählten Servos wieder auf 1 setzen: '
'-----------------------------------------------------------------------'

Select Case I
Case 1 : Servo1 = 1
Case 2 : Servo2 = 1
Case 3 : Servo3 = 1
Case 4 : Dummyservo = 1
Case 5 : Dummyservo = 1
Case 6 : Dummyservo = 1
Case 7 : Dummyservo = 1
End Select

Timer1 = 65535 - Servo(i)

Return

Wie gesagt, dieses Programm funktioniert wunschgemäß, im IK Programm "verpackt" allerdings, produziert die Funktion nur noch Müll...


Viele Grüße

[Bluesmash]

Hallo Ikarus

sorry dass ich nicht auf deine PM geantwortet habe, ich hatte sie gelesen und dachte ich antworte dir später, aber ich habe es dann voll vergessen :( und jetzt habe ich deinen tread entdeckt und da ist es mir wieder in den sinn gekommen ;)

Ich habe kurz deinen Code überflogen und habe etwas entdeckt was auch bei mir zu problemen geführt hat... Ich versuche es mal zu erklären...

für die Servoposition verwendest du ja word variablen (servo(x)...) dein prozessor kann aber pro takt nur 8bit verarbeiten. dass heisst wenn der timer interrupt während den folgenden zeilen auftritt:

Servo(2) = Servo2buffer
Servo(3) = Servo3buffer

kann es sein das erst die ersten 8bit der variable beschrieben sind und so der wert der word variable nicht stimmt und so das servo eine völlig falsche position erhält... ich würde mal folgendes probieren:

disable interrupts
Servo(2) = Servo2buffer
Servo(3) = Servo3buffer
enable interrupts

damit die variablen sauber beschrieben werden können bevor sie in der timer routine verwendet werden.

zusatz:
ich sehe gerade das du die servo() varibeln an verschiedenen orten verwendest, ich würde nochmals zusätzliche variabeln hinzufügen welche du nur einmal beschreiben musst und in der timer isr verwenden kannst z.B. so am ende deiner berechnung:

disable interrupts
servo_pos(1)=servo(1)
servo_pos(2)=serv0(2)
enable interrupts

und dann die servo_pos() variabeln in deiner timer isr vewenden!


Den Stack vergrössern schadet auf jedenfall auch nicht! die standarteinstellung ist auf jedenfall zu wenig, ich würde ihn mindestens vervierfachen solange genug SRam zur verfügung steht!

gruss Bluesmash

[ikarus_177]

Hi Bluesmash,

danke sehr für deine Antwort! Werde deine Tipps am Wochenende mal ausprobieren, da ich, wie gesagt, während der Woche im Internat weg von zu Hause bin...

Wäre doch gelacht, wenn das nicht zum funktionieren zu bewegen wäre ;-)!


Viele Grüße

[ikarus_177]

Nachtrag:

Hab das Programm jetzt mit vervierfachtem Stack sowie der von Bluesmash vorgeschlagenen "Interruptabschaltung" während dem Beschreiben der Variablen getestet - es funktioniert einwandfrei!!! \:D/ =P~

Das mit dem Stack wird mir wahrscheinlich kein zweites passieren, da muss man auch erst einmal draufkommen ;-)


Herzlichen Dank an dieser Stelle an Bluesmash und Willa für die großartige Hilfe!!

Viele Grüße

[Bluesmash]

dann warten wir gespannt auf ein video :)

gruss bluesmash

[ikarus_177]

Hi Bluesmash,

Die IK ist zur Zeit in der Lage, einen bestimmten Punkt im Raum anzufahren - es gibt also (noch) nicht sooo viel zu sehen...

Ich werde in den kommenden Wochenenden sowie in den Semesterferien noch die Servoaufhängung überarbeiten, da mir die MG995 momentan reihenweise ausfallen :-k

Ein Video werde ich dann auch noch nachreichen - ich bitte an dieser Stelle aber noch um etwas Geduld, an einem Wochenende freier Zeit pro Woche lässt sich nicht so viel erreichen...


Viele Grüße