Hallo proevofreak,
irgendwie wird das alles durch die servo schleifen blockiert.
Ja, genau. Und auch noch auf 2 Arten:
1. Die eigentliche Impulserzeugung und Impulswiederholung alle 20ms ist mit den Sleep-Befehlen voll blockierend.
2. Du erzeugst die Bewegung des Greiferarms ebenfalls voll blockierend mit While-Schleifen.

Lösung zu 1:
In diesem Thread stehen Links zu vielen Möglichkeiten, die Impulserzeugung für Servos nicht vollständig blockierend zu machen:
https://www.roboternetz.de/phpBB2/viewtopic.php?t=45790

Lösung zu 2:
Da die Bewegungen des Arms auch Zeit brauchen, würde ich sie nicht in Schleifen abarbeiten, sondern auch in einer Task laufen lassen. Das bedeutet, dass du die Bewegungen mit einer Stopwatch so steuerst, dass z.B. alle 40ms den Servos eine neue Position gegeben wird. Danach wird die Hauptschleife in Main jeweils wieder ausgeführt.

Am Ende sieht deine Main dann so aus:
while(true)
{task_SERVO();
task_teil_erkennen();
task_teil_greifen();
task_fahren();
task_RP6System();
}
Wichtig ist, dass ALLE Tasks (auch die task_teil_greifen) nur so kurz wie möglich sind und die Kontrolle immer rasch wieder an die Hauptschleife abgeben. Das Verhalten der Tasks kannst du mit globalen Flags steuern: Wenn er gerade ein Teil greift, wird z.B. in der task_teil_greifen ein Flag auf true gesetzt, das in der task_fahren benutzt wird, um den RP6 zu stoppen.

Viel Erfolg!

Gruß Dirk