Un problème à la fois. C'est le plus simple pour apprendre.
Donc, il faut commencer par savoir piloter les moteurs. SANS POTAR.
Vérifier déjà que tu maîtrises la commande dans un sens, puis dans l'autre et enfin le freinage dynamique. Ensuite les rampes d'accélération.
Après tu pourras commencer à implémenter ton système de commande.
Et si j'ai un conseil c'est d'éviter au maximum ces if ... else ... imbriqués et les uns derrière les autres parce que pour arriver à un fonctionnement inattendu (ou même à pas de fonctionnement) y'a pas mieux.
Il est plus sûr:
d'examiner tout les paramètres d'entrée au début de loop.
de construire un mot d'état à partir de l'analyse des paramètres
et d'utiliser un switch .. case ... pour traiter les cas.
l'avantage du switch case c'est que tous les cas sont au même niveau dans le même bloc et bien identifiables ce qui évite des oublis ou des cas qui peuvent s'annuler les uns les autres (ce qui arrive des fois avec les if qui se suivent).
Si tu tiens à utiliser des if else alors il vaut mieux faire des appels de fonctions dans les blocs if else au lieu de mettre des blocs de code. Les tests sont plus compactes et plus lisibles et les fonctions appelées peuvent être testées séparément ce qui est plus simple pour la mise au point et c'est un gage de sécurité.