Wie würdet Ihr das Programmieren

Hallo,

ich habe einen Roboter-Arm gebaut, der mit mehreren Hydraulik-Zylindern betrieben wird. Das System arbeitet nicht mit Magnetventilen, sondern mit einem Rotationsverteiler. Dieser Verteiler hat einen Stellmotor und positioniert die Ventile entsprechend. Dann wird der Zylinder über ein Hydraulik-Aggregat betrieben.
Hinzu kommt, dass dieser Roboter-Arm über einen Handbedienung bewegt und programmiert werden soll.

Dazu habe ich eine Steuerung entwickelt, die mit einem Atmega128 gesteuert wird. Die Handbedienung hat einen Pic16f873a.

Die Handbedienung sendet mit 9600 Baud permanent, welche Taste gedrückt wird oder ob keine Taste gedrückt wird.

In der Hauptsteuerung wertet der Atmega128 dieses Signal aus und steuert dann die entsprechenden Funktionen an.

Mein Problem ist, dass ich erst das Rotationsventil passend positioneren muss und dann das Hydraulik-Aggregat zuschalten muss.
Das ist auch kein Problem, solange die entsprechende Taste gedrückt bleibt.
Wird die Taste aber sofort wieder losgelassen (z.B. bei drücken der falschen Taste), dann muss das Rotationsventil sofort wieder in die Neutral-Stellung fahren.

Ich habe das Programm im Moment folgendermaßen aufgebaut:

1. Empfang der Daten vom Handschalter
2. Auswertung der Handschalterdaten und sprung in entsprechendes Unterprogramm
3. Festlegen der Position des Rotationsverteilers
4. Sprung in Unterprogramm zum Steuern des Rotationsverteilers
5. Starten des Rotationsverteiler-Motors
6. Prüfen ob entsprechende Position erreicht ist.
7. Ist Position erreicht, Motor ausschalten
8. Ist Position nicht erreicht, Zeitverzögerung 10ms und dann Motor ausschalten.
9. Rücksprung aus Unterprogramm
10. Prüfen, ob Rotationsverteiler richtig positioniert ist
11. Wenn richtig positioniert, dann Hydraulik-Aggregat starten.
12. Rücksprung aus Unterprogramm

Irgendwie funktioniert das aber mit dem Timing nicht richtig. Grundsätzlich arbeitet das schon so, aber die Motoren werden nicht permanet gefahren, sonder stottern ein wenig.

Wie würdet Ihr das Programmieren, damit das o.g. Programm problemlos läuft?

Ich stelle morgen früh mal noch den Quelltext hierbei, habe ich leider im Moment nicht zur Hand.

Mfg,
Thorsten