Hast du den auch mal die Werte aus diesem Befehl entfernt:

while(1==1){
copro_setSpeed(20, 20);
}

?
Du musst die Werte denk ich mal auch aus dem Befehl löschen also z.B. wenn ein Hinderniss erkannt wird die Werte von 20 auf 0 umändern. Du hast ja in deinem Programm keine Bedingung unter welcher dieser Befehl ausgeführt werden soll. Da steht einfach nur das die Motoren sich mit einem Speed von 20, 20 drehen sollen und nicht

if(Bedingung)
{
Motor soll sich drehen
}

Wie gesagt ich kenn deinen Bot nicht und kann es leider auch nicht testen aber die Tatsache das sich die Motoren drehen deutet ja darauf hin das dieser Befehl andauernd ausgeführt wird. Was du machen könntest ist erstmal alles unnötige ausklammern und dann mit einfachen Print-Befehlen eine Art Debug-Schnittstelle zu machen, also das du nach jedem

copro_setSpeed(20, 20);

eine Ausgabe über UART machst. So siehst du wann der Befehl ausgeführt wird und ob der Befehl die ganze Zeit ausgeführt wird.