Ich glaube, ich verstehe dein Problem, aber nicht wie die Lösung aussehen soll.

Du hast 2 Motoren und ein Joystick? Der Joystick soll die beiden Motoren so drehen wie du den Joystick bewegst?

Dein Code hat leider etwa wenig Kommentare und bedarf etwas mehr Nachdenken um ihn zu verstehen Wie werden denn die Motoren angetrieben? Das ganze sieht auf dem ersten Blick etwas binär aus

Wofür sind die waitms? Und wie soll sich A und B errechnen?

Das sind erstmal meine Fragen die mir zum Verständnis fehlen.
Wenn ich nach den Antworten alles nachvollziehen kann, werd ich morgen Abend mal versuchen was zu schreiben. Sofern das Problem bis dahin noch nicht gelöst ist.