Hallo TcH,

aus diesem State hier:

Code:
// Initialisieren
      case STATE_INITIALIZE:
      {
         // Microcontroller initialisieren
         initRobotBase();
         
         // Motorencoder initialisieren
         powerON();
         
         // Entfernungssensoren aktivierencv
         setACSPwrMed();
         
         state = STATE_CHECK_FOR_OBSTACLE;
         
         break;
      }
sollte besser alles raus und wie in den Beispielprogrammen direkt am Anfang des Programms stehen - also ganz am Anfang der Main Funktion.
Mindestens initRobotBase(); MUSS ganz am Anfang stehen - sonst funktionieren einige Dinge nicht richtig.

Um den Fehler selbst besser eingrenzen zu können, kannst Du einfach in jeden Zustand der State Machine mal eine Textausgabe reinmachen. Dann siehst Du sofort wie sich die State Machine verhält wenn sich etwas ändert.
Also einfach ein paar writeString_P("State 12345 "); reinmachen und evtl. zusätzlich noch ein paar Sensorwerte per writeInteger mit ausgeben.

Also den Roboter dabei dann erstmal am Rechner angeschlossen lassen und die Ausgaben im Terminal ansehen.
Dabei den Roboter natürlich entweder mit der Hand hochheben oder irgendwas drunterstellen so dass er sich nicht bewegen kann.

MfG,
SlyD