"Multithreading" mit C ?

Hi,
Ich hätte gerne, dass sich die Servos von meinem Roboterarm sich gleichtzeitig bewegen können, und am Besten noch so, dass sie, egal wie weit sie sich bewegen mussten, synchron zum Stillstand kommen.
Bis jetzt habe ich meine Servos immer mit einer for-Schleife bewegt. Sofortiges setzen der Endposition für dazu, dass die 2 400 Ncm Servos, die den Oberarm bewegen, den kompletten Roboterarm aus der Verankerung reißen. Sie bewegen sich zusätzlich langsamer durch ein _delay_ms() in der Schleife.

Code:

---

int i=0;
for(i=1; i<=255; i++) {
    servo_move(1, i);
    _delay_ms(20);
}

---

Hat jemand sowas schon mal gemacht oder ne Idee wie das gehen könnte? Wollte mir nur ungern 4 weitere Pozessoren kaufen.

mfg
jagdfalke

Hallo,
hier im Forum wurde mal eine Version von Multithreading für den Asuro gepostet, musst danach suchen, finde ich jetzt nicht.
Das hat recht gut geklappt, alle LEDs blinken und faden gleichzeitig in unterschiedlicher Geschwindigkeit, obwohl nur mit warteschleifen programmiert.

Also ich mache das meistens so in der Art:

Code:

---

unsigned int Timer_1ms;
unsigned char TaskFlags;

#define TASK_1                0
#define TASK_2                1

SIGNAL(SIG_OVERFLOW1)
{       
        Timer_1ms++;
       
        if(Timer_1ms % 5 == 0)
        {
                TaskFlags |= (1 << TASK_1);
        }
       
        if(Timer_1ms % 10 == 0)
        {
                TaskFlags |= (1 << TASK_2);
        }
}

int main(void)
{
        for(;;)
        {
                if(TaskFlags & (1 << TASK_1))
                {
                        TaskFlags &= ~(1 << TASK_1);
                        Funktion_1();
                }
               
                if(TaskFlags & (1 << TASK_2))
                {
                        TaskFlags &= ~(1 << TASK_2);
                        Funktion_2();
                }
        }
}

---

Also jeder Prozess hat ein eigenes Flag in irgendeiner globalen Variable.
Ist eines dieser Flags gesetzt wird in der Hauptschleife entsprechend darauf reagiert und das Flag wieder gelöscht.
In dem Beispiel wird also eine Funktion alle 5ms, und die andere - unabhängig davon - alle 10ms aufgerufen.

Natürlich müssen die Flags nicht zwangsläufig durch einen Timer-Interrupt gesetzt werden, das sollte nur als einfaches Beispiel dienen.



Das dürfte so ziemlich die einfachste Methode sein um "Multithreading" zu simulieren.
(es geht naürlich noch besser, aber das sollte in den meisten Fällen ausreichen)

Ok, danke.
Das mit den Timers hab ich mir zwar schon durchgelesen aber noch nicht so richtig kapiert. Aber ich verstehe was in deinem Code passiert.
Thx

es geht auch das man auf den Megas ein wirkliches Multitasking programiert. Also eine Echtzeitanwendung Programieren kann.
Man muß hierzu ein betriebssystem benutzen ist nen bisschen komplizierter wie normales Programieren aber wenn man es mal raus hat geht es. ICh versuche grade mit einem solchen system einen 5 Achs roboter anszusteuern.

Soweit bin ich noch lange nicht, aber trotzdem danke für die Antwort. Vielleicht postest du mal einen Namen oder eine Website. Das interessiert bestimmt andere hier, die etwas mehr skill haben als ich.

mfg
jagdfalke

Ja klar ist ne geile sache wenn man es kann
RTOS Real Time Operating System
http://www.freertos.org/

Zitat:

---

es geht auch das man auf den Megas ein wirkliches Multitasking programiert. Also eine Echtzeitanwendung Programieren kann.
Man muß hierzu ein betriebssystem benutzen ist nen bisschen komplizierter wie normales Programieren aber wenn man es mal raus hat geht es.

---

Also eigentlich muss man sich bei einem Prozessor der Befehle nur sequentiell bearbeiten kann für eines von beiden entscheiden...

entweder Echtzeitanwendung oder Multitasking, beides zusammen geht nicht.
(nicht ohne Grund sind viele Programme zur direkten Ansteuerung von CNC-Fräsen für DOS geschrieben)


@jagdfalke
so wahnsinnig kompliziert ist "echtes" Multitasking eigentlich garnicht...
Es laufen dabei mehrere Programme quasi parallel ab, wobei jedes einen kompletten Satz Register und einen eigenen Stack hat.
Um zu einem bestimmten Thread zu wechseln muss man dessen Registerinhalte (die irgendwo im RAM abgelegt wurden) in die Prozessorregister laden.

Ein großer Vorteil bei dieser Variante ist, daß die threads erstmal keine Rücksicht aufeinander nehmen müssen.
d.h. sie müssen nicht freiwillig das Kommando über den Controller abgeben, sondern es wird ihnen vom Betriebssystem entrissen.
Daher können sich die verschiedenen threads auch nicht gegenseitig blockieren.

Der wohl größte Nachteil ist der enorme Platzbedarf im RAM.



Die von mir vorgeschlagene Methode ist platzsparender,
allerdings muss man da eben bei der Programmierung darauf achten daß die aufgerufenen Funktionen nicht zu lang sind.

Da ich mich auch damit beschäftigen will, habe ich einige Links aufgehoben:

https://www.roboternetz.de/phpBB2/ze...?p=85902#85902
oder
http://www.fictoor.nl/irbot/frameentry.html dort unter Download > Software > A.T.A.K
http://www.mikrocontroller.net/forum...78.html#240790 AvrMultOS.zip

das problem ist halt was man machen will will ich irgendwas abarbeiten und auf externe ereignisse reagieren dann kann er per Interruptgesteuerten Programmen zu zu langen reaktionszeiten kommen oder das system reagiert gar nicht weil er in dem langen Programstück feststeckt.

Also geht geht bei mir darum, dass sich Servos gleichzeitig bewegen sollen. Wie würdet ihr das machen? Vielleicht einen Timer und immer wenn so ein Overflow-Interrupt kommt überprüfen ob alle an der gewünschten Position sind, und wenn nicht jeden einen Step in die richtige Richtung machen lassen.
Würde das funktionieren? Ich brauche halt ne Möglichkeit, die meinen momentanen Fähigkeiten entspricht. Felix G's Vorschlag halte ich momentan für zu kompliziert (Stack und RAM usw.. bääää!!!! )

mfg
jagdfalke

Um das Bewegungsmässig zu optimieren, müsste man vorher wissen wie weit jeder Servo von seiner Sollposition weg ist. Denn wenn alle gleich ankommen sollen muss der Servo der den kurzeren Weg hat sich langsamer bewegen, der mit dem weiten Weg entsprechned schneller !
Wenn so ein Verhalten gewünscht ist, muss man vorher die einzelnen Schrittweiten ausrechnen.

Hi jagdfalke,

also erst einmal kein "Delay()". Während der Ausführung von Delay verlierst du die Kontrolle, dann gehen nur noch Interrupts.

Eigentlich sollte es so gehen:

Aufgabe 1: Synchrones Ende der Bewegung.
----------------------------------------------------
Am einfachsten wird es sein, wenn du abschätzt, wie lange die einzelnen geforderten Bewegungen dauern werden, das Zeitmaximum ermittelts und die Geschwindigkeit der anderen Achsen nach diesem Maximum ausrichtest. Diesen Vorgang kannst du dann während der Ausführung immer wieder durchführen und die Geschwindigkeiten nachregeln. Manche Mototen haben Mindestgeschwindigkeiten. Hier klappt's dann nicht perfekt.

Verbesserung: Geschwindigkeit kurz vor der Endestellung herabsetzen um Überlaufen über die Zielstellung hinaus zu verhindern.


Aufgabe 2: Gleichzeitiges bewegen.
-----------------------------------------------------
Alle Motoren mit der o.g. Geschwindigkeit in Gang setzen.

Aufgabe 3: Mit der Bewegung in der Zielstellung stoppen
--------------------------------------------------------------------
Alle Sensoren abfragen und auf Zielstellung prüfen. Ist Sensor in Zielstellung, Geschwindigkeit auf 0.

Aufgabe 4: Fetstellen, dass die Bewegung erledigt ist
--------------------------------------------------------------
Alle Achsen ins Zielstellung

Aufgabe 5: alles Zusammenbauen
--------------------------------------------------------
Am besten schreibst du für die o.g. genannten Aufgaben kleine Funktionen und rufst diese in einer Schleife auf. Nicht immer sind alle Ziele erreichbar, weil sie sich widersprechen oder Rahmenbedingungen verletzen (s. z.B. "Aufgabe 1"). Hier muss man sich für eine Variante oder eine Kompromisss entscheiden. Die endgültige Lösung hängt sehr von diesen Entscheidungen ab.

Diese packst du dann sinnvollerweise ebenfalls in eine Funktion.

Ein Timer und eine Interruptroutine ist nur dann notwendig, wenn z.B. noch weitere Vordergrundprozesse durchgeführt werden sollen oder Abläufe in exakt einzuhaltenden zeitabständen ausgelöst werden müssen. Dann könnstest du den o.g. Ablauf z.B. jede Millisekunde einmal anstossen. Interrupt-Programmierung hat allderdings ein paar Haken und Ösen....

Gruß RedBaron

Multitask und Echtzeit auf einem Prozessor werden im Betriebssystem auch nur Simuliert.

Echtzeit ist ja nur die definition für "schnell genug"

Das mit den Tasks und Timern ist eigentlich ganz Simpel...

Aufgabe: Servo soll von A nach Z fahren.

Die synchrone Programmierung:
PROGRAMSCHLEIFE BEGIN
[SERVO1 fahre von A nach Z]
[SERVO2 fahre von A nach Z]
PROGRAMSCHLEIFE LOOP

Die asynchrone Programmierung:
PROGRAMSCHLEIFE BEGIN
[SERVO1 fahre eins weiter]
[SERVO2 fahre eins weiter]
PROGRAMSCHLEIFE LOOP

Sie asynchrone Programmierung ist meistens etwas verhackter … denn da müssen mehr Stats abgefragt und gesetzt werden. (Wie Wo steht das Servo gerade? Kann ich weiterfahren oder soll ich doch besser etwas anderes tun?)

In Größeren Programmen (many servos) verwendet man dann meist Tasks die durch Timer die in verschiedenen MSec Abständen gesetzt sind zum steuern. (also man bastelt künstliche PROGRAMSCHLEIFEN = Multithread like)

Zitat:

---

Zitat von UlrichC

Echtzeit ist ja nur die definition für "schnell genug"

---

*ROFL*

Zitat:

---

Zitat von jagdfalke

Ich brauche halt ne Möglichkeit, die meinen momentanen Fähigkeiten entspricht. Felix G's Vorschlag halte ich momentan für zu kompliziert (Stack und RAM usw.. bääää!!!! )

---

Bei meinem Vorschlag (das Codebeispiel von oben) musst du dich eben nicht mit Registern und Stack auseinandersetzen...
Das müsstest du nur wenn du preemptive Multitasking (ähnlich wie in Windows) brauchst, und das alles selber programmieren willst.
(= Steffens Vorschlag, nur eben ohne ein fertiges OS zu verwenden)


@PickNick
hast du eine bessere Definition?

Zitat:

---

Zitat von Felix G

hast du eine bessere Definition?

---

Eben nicht, das ist die beste, die ich seit langem gehört habe. Klingt so herrlich pragmatisch und überhaupt nicht Uni-style

Zitat:

---

Zitat von PicNick

Zitat:

---

Zitat von UlrichC

Echtzeit ist ja nur die definition für "schnell genug"

---

*ROFL*

---

Mhhh...
Mal anders "Verarbeitung von Daten, die ohne nennenswerte Zeitverzögerung geschieht."
also "schnell genug"...

Die definition ist eigentlich sogar noch härter ... oder denkst Du man kann mit Echtzeit die Zeit verkehren ...

Bist Du vielleicht ein Quantentheoretiker der Tasks der Zukunft in der Vergangenheit ausführen kann?

Soll ich nun noch meine undiplomatische informatische profilneurotische Informatikerantwort dazu schreiben?

Jemanden der lediglich zwei Servos gleichzeitig beswegen möchte mit mutithread, process events,interupts und ähnlichem Käse zuquatschen?

Jedenfalls sollte man den Begriff "Echtzeitanwendungen" hier nicht im UNVERSTAND oder unpassenden Kommentaren ausweiten.

Zitat:

---

Zitat von PicNick

Zitat:

---

Zitat von Felix G

hast du eine bessere Definition?

---

Eben nicht, das ist die beste, die ich seit langem gehört habe. Klingt so herrlich pragmatisch und überhaupt nicht Uni-style

---

Tja Munition zurück #-o
Sorry kam nicht so an ...

Zitat:

---

Zitat von UlrichC

Sorry kam nicht so an ...

---

Meine Schuld. War mißverständlich. O:)

Wie UlrichC bin ich der Meinung, dass für das synchrone Ansteuern von Servos kein Multithreading, Realtime OS oder sonstiger Software-Overhead notwendig ist.

Zuerst solltest Du die Servo-Ansteuerung mal mit Hilfe von Timer-Interrupts realisieren. Damit hat das Hauptprogramm auf einmal ne Menge Zeit, sich um andere Dinge zu kümmern. Als nächstes muss die Servoansteuerung schrittweise geschehen, also nicht sofort die Zielposition ansteuern, sondern eine Menge von Zwischenpositionen. Das Inkrementieren der Zwischenposition kann man ebenfalls in der Interrupt-Routine ausführen, wenn man vorher die Steps ausgerechnet hat.

Welcher Servo welche Steps macht, mit welcher Geschwindigkeit dies getan wird und wer das Tempo angibt, kommt auf die Servos an und ist durch entspr. Testläufe zu ermitteln.

Gruß, Achim.

Hi, ich habe jetzt mal folgendes geschrieben:

Code:

---

#define F_CPU        8000000

#include <avr/delay.h>
#include <avr/interrupt.h>
#include <avr/signal.h>

#include "uart.h"
#include "servo.h"

volatile uint8_t servo_flag = 0;

SIGNAL (SIG_OVERFLOW0)
{
        servo_flag = 1;
}

int main (void) {
        uart_init(1, 0);

        sei();
        TCCR0 |= (1<<CS02) | (1<<CS00);
        TIMSK |= (1<<TOIE0);



        while(1)
        {
                if(servo_flag == 1) {
                        cli();
                        uint8_t move_made = 0;  // 1 wenn eine Änderung gemacht wurde
                        int8_t step = +1;
                        if(shoulder_pos != shoulder_dest) {
                                if(shoulder_pos > shoulder_dest) { step = -1; }
                                shoulder_pos += step;
                                move(1, shoulder_pos); // hier werden sie 2
                                move(2, shoulder_pos); // 400 Ncm Servos gleichtzeitig bewegt
                                move_made = 1;
                        }
                        if(elbow_pos != elbow_dest) {
                                step = +1;
                                if(elbow_pos > elbow_dest) { step = -1; }
                                elbow_pos += step;
                                move(3, elbow_pos);
                                move_made = 1;
                        }
                        if(wrist_pos != wrist_dest) {
                                step = +1;
                                if(wrist_pos > wrist_dest) { step = -1; }
                                wrist_pos += step;
                                move(4, wrist_pos);
                                move_made = 1;
                        }
                        if(gripper_pos != gripper_dest) {
                                step = +1;
                                if(gripper_pos > gripper_dest) { step = -1; }
                                gripper_pos += step;
                                move(5, gripper_pos);
                                move_made = 1;
                        }
                        sei();
                }
        }
        return 0;
}

---

servo.h

Code:

---

volatile uint8_t shoulder_pos = 127;
volatile uint8_t elbow_pos = 127;
volatile uint8_t wrist_pos = 127;
volatile uint8_t gripper_pos = 127;

volatile uint8_t shoulder_dest = 255;
volatile uint8_t elbow_dest = 1;
volatile uint8_t wrist_dest = 1;
volatile uint8_t gripper_dest = 80;

void move(int servo, int pos)
{
      loop_until_bit_is_set(UCSRA, UDRE);
      UDR = '#';
      loop_until_bit_is_set(UCSRA, UDRE);
      UDR = 's';
      loop_until_bit_is_set(UCSRA, UDRE);
      UDR = servo;
      loop_until_bit_is_set(UCSRA, UDRE);
      UDR = pos;
}

---

Ist der Code so in Ordnung? Bewegen tun sich die 4 "Gelenke" schon mal gleichzeitig. Das mit der Steuerung der Geschwindigkeit verstehe ich nicht. Ich glaube aber so wie ich den Code jetzt geschrieben habe kann man die Geschwindigkeit nicht mit berücksichtigen. Was hier jetzt lustig ist: Wenn ein Servo noch bewegen muss wenn die anderen schon fertig sind ist er schneller als wenn noch einer mitlaufen muss. (Das stört irgendwie mein ästhetisches Empfinden :D )

mfg
jagdfalke

[quote="jagdfalke"]Hi, ich habe jetzt mal folgendes geschrieben:
servo.h[/qoute]

Code:

---

#ifndef _SERVO_H_
#define _SERVO_H_

extern volatile uint8_t shoulder_pos;
extern volatile uint8_t elbow_pos;
...
extern void move(int servo, int pos);

#endif /* _SERVO_H_ */

---

Der ganze Klumbatsch ausm *.h gehört in ein *.c, im Header verbleiben nur die Deklarationen/Prototypen.

Hä? Ich versteh kein Wort von dem Code den du da gepostet hast.
Warum in ein .c-File? So klappts doch!! Und es ist übersichtlicher. Wenn C schon keine Klassen hat, muss man doch irgendwie den Code gruppieren.

mfg
jagdfalke

Ist es richtig, dass die move-Methode die Servoposition über die serielle Schnittstelle ausgibt? Heisst das, dass Du einen externen Controller zur Ansteuerung der Servos benutzt? Wenn ja, wozu?

Egal, eigentlich sehe ich keine Probleme im Code (aber normalerweise programmiere ich die AVRs in Assembler). Die Effekte, die Du beobachtest, können natürlich durch die serielle Übertragung der Kommandos (Überholeffekte?) oder durch den Servocontroller selbst verursacht sein. Besser wäre es, wenn man im Servocontroller selbst einen Synchron-Modus programmieren könnte, aber das ist wohl eine Black Box.

Mit welcher Bitrate arbeitet die serielle Verbindung? Hab mal ausgerechnet: wenn es nur 2400 bit/s sind, dauert das Senden der 5 Servopositionen über UART 0.083 Sekunden, wogegen ein Timerinterrupt alle 0.032 Sekunden auftritt. Bei 9600 bit/s reicht das Timing.

Zur Geschwindigkeit: Du solltest zu Beginn einer synchronen Bewegung die Wege aller Servos ausrechnen. Der Servo mit dem längsten Weg gibt die Geschwindigkeit vor. Er macht pro Zyklus n Schritte, wobei n = Zykluszeit (z.B. 0.032s) * max Servogeschwindigkeit (z.B. 200/s) = z.B. 6.4. Die Steps der anderen Servos werden über das Verhältnis der Wege ausgerechnet. Hat z.B. Servo 3 einen Weg von 50, so macht er einen Step von 3.2 (6.4*50/100). Haben die verschiedenen Servos unterschiedliche Geschwindigkeiten, wird es allerdings komplizierter...

So, muss jetzt arbeiten.

Der "externe Controller" ist auf dem selben Board (rnbfra 1.1). Er übernimmt das Senden der Pulse an die Servos. Auf ihm ist der RNS1-Servotreiber drauf.
Du sagst 3.2 Schritte? Hab bisher immer nur Integer-Werte rübergegeben. Mal schau ob das auch so klappt.
Die Baud-Rate ist 9600. Soll ich sie höher einstellen? Wenn sie höher ist, kann der Servotreiber dann noch was empfangen?

mfg
jagdfalke

Zitat:

---

Du sagst 3.2 Schritte? Hab bisher immer nur Integer-Werte rübergegeben. Mal schau ob das auch so klappt.

---

Integer ist schon korrekt. Du solltest die Schritte runden. Sonst gibt es eine Typverletzung. Und der Controller mag wohl auch nur Integer. Die fehlenden Nachkommastellen könnten allerdings geringe Ungenauigkeiten im Timing verursachen.

Zitat:

---

Die Baud-Rate ist 9600. Soll ich sie höher einstellen? Wenn sie höher ist, kann der Servotreiber dann noch was empfangen?

---

Nein. Wenn Du die Baudrate nur einseitig änderst, verstehen sich Sender und Empfänger nicht mehr. 9600 ist ja auch OK. Es muss noch etwas anderes geben, was zu dem komischen Verhalten führt.

Ok, das hab ich soweit verstanden. Hab auch gleichmal ein bisschen Code geschrieben:

Code:

---

#define F_CPU        8000000

#include <avr/delay.h>
#include <avr/interrupt.h>
#include <avr/signal.h>
#include <math.h>
#include <stdlib.h>

#include "uart.h"


// METHODEN DEKLARATIONEN
void keep_moving(void);
void move(int servo, int pos);
void calc_steps(void);
int signum(int val);


//VARIABLEN DEKLARATIONEN
volatile uint8_t servo_flag = 0;
volatile uint8_t recalc_flag = 1;

volatile uint8_t shoulder_pos = 127;
volatile uint8_t shoulder_dest = 255;
volatile float shoulder_step = 0;

volatile uint8_t elbow_pos = 127;
volatile uint8_t elbow_dest = 255;
volatile float elbow_step = 0;





int main (void) {

        //UART AN
        uart_init(1, 0);

        //INTERUPT UND TIMER AN
        sei();
        TCCR0 |= (1<<CS02) | (1<<CS00);
        TIMSK |= (1<<TOIE0);

        while(1)
        {
                //WENN DIE EINMALE ANGEGEBENE POSITION ERREICHT WURDE
                //DÜRFEN DIE STEPS NEU BERECHNET WERDEN
                if(recalc_flag == 1) {
                        calc_steps();
                }
                //NUR AUFRUFEN, WENN EIN TIMER INTERUPT DA WAR
                if(servo_flag == 1) {
                        keep_moving();
                }
        }
        return 0;
}

void keep_moving() {
        cli();

        //NEUE POSITIONEN AUSRECHNEN UND RUNDEN
        shoulder_pos = abs(shoulder_pos + shoulder_step);
        elbow_pos = abs(elbow_pos + elbow_step);

        //POSITIONEN SCHICKEN
        move(1, shoulder_pos);
        move(2, shoulder_pos);
        move(3, elbow_pos);

        //WENN DIE POSITIONEN GERUNDET ÜBEREINSTIMMEN
        //NEUBERECHNUNG DER STEPS ERLAUBEN UND NEUES ZIEL FÜR ELBOW SETZEN
        if(abs(shoulder_pos) == shoulder_dest && abs(elbow_pos) == elbow_dest) {
                elbow_dest = 1;
                recalc_flag = 1;
        } else {
                recalc_flag = 0;
        }

        servo_flag = 0;
        sei();
}

void calc_steps(void) {
        //BERECHNEN WIE VIEL SICH DIE SERVOS BEWEGEN MÜSSEN
        uint16_t shoulder_to_go = shoulder_dest - shoulder_pos;
        uint16_t elbow_to_go = elbow_dest - elbow_pos;

        //HIER ALS ABSOLUTE WERTE
        int16_t shoulder_abs = abs(shoulder_to_go);
        int16_t elbow_abs = abs(elbow_to_go);

        //STEP ERSTMAL AUF +/- 1 SETZEN, JE NACH RICHTUNG
        elbow_step = signum(elbow_to_go);
        shoulder_step = signum(shoulder_to_go);

       
        if(elbow_abs < shoulder_abs) {
                elbow_step = 1 * signum(elbow_to_go);
                shoulder_step = (shoulder_abs / elbow_abs) * signum(shoulder_to_go);
        }
        if(elbow_abs > shoulder_abs) {
                shoulder_step = 1 * signum(shoulder_to_go);
                elbow_step = elbow_abs / shoulder_abs * signum(shoulder_to_go);
        }

}

void move(int servo, int pos){
      loop_until_bit_is_set(UCSRA, UDRE);
      UDR = '#';
      loop_until_bit_is_set(UCSRA, UDRE);
      UDR = 's';
      loop_until_bit_is_set(UCSRA, UDRE);
      UDR = servo;
      loop_until_bit_is_set(UCSRA, UDRE);
      UDR = pos;
}

int signum(int val) {
        if(val != 0) {
                return val/abs(val);
        } else {
                return 0;
        }
}

SIGNAL (SIG_OVERFLOW0){
        servo_flag = 1;
}

---

Der Code bewegt zur Vereinfachung erstmal nur die Schulter und den Ellbogen. Die Bewegung klappt schon ganz gut. Allerdings passt das Neusetzen des Ziels für Ellbogens nicht ("elbow_dest = 1;" in keep_moving() )
Wer Interesse und Zeit hat kann sich das ja mal anschauen warum das nicht klappt. Ich weiß es ist viel Code zum "kurz" anschauen aber ich habs ein wenig auskommentiert, jetzt isses recht gut zu verstehen.

mfg
jagdfalke