Wenn der Motorstrom in Grunderregung zu hoch ist,
darf die SW den Motor nicht freigeben.
Das klingt gut!

Vielleicht kannst du ja mal ein kleines Programm schreiben, mit dem der RP6 einfach nur geradeaus fährt und parallel die Speed und den Stromverbrauch z.B. jede halbe Sekunde über UART sendet.
Die Ausgabe könnte alle 0,5 Sek. sein:
writeIntegerLength(getLeftSpeed(),DEC,3);
writeIntegerLength(getRightSpeed(),DEC,3);
writeIntegerLength(adcMotorCurrentLeft,DEC,3);
writeIntegerLength(adcMotorCurrentRight,DEC,3);
writeString_P("\n\n");

Gruß Dirk