@inka:
Die hauptschleife in mehrere schleifen aufteilen?
Nein, die Hauptschleife bleibt EINE Hauptschleife.

Ich hatte dir ja hier: https://www.roboternetz.de/phpBB2/viewtopic.php?t=45223 .. mal im Prinzip hingeschrieben (17.8.), wie es gehen kann.

Du hast zwar jetzt schon einzelne Funktionen, aber nur für Servo-POSITIONEN und nicht für Servo-Bewegungen.

Bevor du das Fahren da rein mixt, mach erst einmal je eine Funktion für jede geplante Bewegung fertig.
Also z.B.: Greifarm_runter() oder Schaufel_zu().
Diese Funktionen stimmst du so ab, dass sie perfekt funktionieren (also richtige Geschwindigkeit der Servoarme, Positionen werden erreicht ...).

Dann kannst du im Hauptprogramm Bedingungen formulieren, wann die Servo-Bewegung gemacht werden soll. Wenn das nach einer Roboter-Bewegung sein soll: if (isMovementComplete() ) Schaufel_zu();

Gruß Dirk