Entschuldige, ich wollte nicht böse rüberkommen, aber es gibt leider ziemlich viele Leute die in ein Forum mit der Erwartung kommen: Ach hier wird mir schon jemand die Arbeit abnehmen, wenn ich einfach mal irgendwelchen Code hinklatsche ohne wirklich was dazu zu sagen. Diese Erwartung dämpfe ich lieber gleich rechtzeitig.

Aber wie gesagt. Dein Problem liegt darin. Du liest sequeziell aus der Datei, verarbeitest das, machst dann ein "sleep(2)", und setzt dann dein Ziel, dann machst du das fürs nächste bein usw.
1. Das lesen aus der Datei und suchen braucht Zeit...viel zeit. Einmal die Datei einlesen und dann die Matrix draus bilden wäre schneller, danach nur noch auf die Matrix zugreifen
2. Sleep(2). Lässt deinen derzeitigen Thread schlafen. Je nach Betriebssystem 2sec oder 2msec. Das sollte nicht nötig sein. Daher kommt vermutlich auch der Großteil deiner Verzögerung
3. Du machst das lesen/Schlafen für jedes Bein, wie schon gesagt einzeln...daher kommt eine Verzögerung für jedes Bein. Les dir die Position für jedes Bein ein, Speicher sie dir, und setze danach alle Position in einem Rutsch. Darauf zielt auch der Ratschlag von oberallgeier. Schau nach ob du eine Funktion /command findest mit der du alle Servos aufeinmal positionieren kannst.

Noch ein kleiner Tipp zu Fragen in Foren. Wenn du ein Problem mit konkretem Quellcode/Implementierung hast, beschreibe den Quellcode, die Aufgabe, die Plattform. Liefer am besten auch gleich Links zur Dokumentation mit dazu, dann ist die "Helf-Schwelle" gleich niedriger. Vorallem aber, versuche dein Problem einzugrenzen und sage dazu was du bereits versucht hast. Formulierungen in der Art, kann mir mal jemand helfen meinen Code so und so zu schreiben kommen in fast allen Foren die ich kenne sehr schlecht an, weil es so wirkt als ob du dein Problem auf andere komplett abwälzen willst. Du wirst in Foren selten jemanden finden der für dich Code schreibt, aber viele Leute die dir gute Tipps und Ratschläge geben können.