Im Augenblick ist kaum was dazwischen außer meine Motor-Routine, das soll auch so bleiben.
Das kann nicht der Grund sein, dass er überschießt und nicht mehr zurückrudert.
Es kommt später aber tatsächlich mit absoluter Sicherheit noch jede Menge mehr dazwischen,
zum Einen noch mehr Motoren (insg. 6 mit Mega), deren Werte an die PID-Controller übergeben und gestellt werden müssen,
und dann noch eine i2c-Read/Write-Funktion, die Werte und Befehle vom Master (NXT) in Empfang nimmt und Sensorwerte regelmäßig wieder zurückschickt. Diese haben ein delay jew. von 10-20ms, plus Codierung/Decodierung der i2c-msg-arrays.
Anders geht es nicht.
Ein guter PID-Controller vergleicht aber die echten, gestoppten loop-Zeiten und bringt sie in den I- und D- Summanden mit ein, damit die echte delta-Zeit zur Steuerung zu grunde gelegt wird. Da macht dann eine Verzögerung nichts aus.
reg = P + I*dt + D/dt
Ansonsten: Zeitscheiben-Multitasking, jeder Motor-Thread und jeder i2c-Thread in eine extra Zeitscheibe. So mache ich es beim NXT, das geht sogar mit einer VM, die alles um den Faktor 10 verlangsamt, in Echtzeit.
Da wäre also noch was deutlich ausbaufähig, beim Mega.