5 Sensoren - 2 Motoren - Nibo - KI-Ansatz in C gesucht

Ich beschäftige mich mit dem neuen Roboterbausatz Nibo (nicai-systems) und bin auf der Suche nach einer Verbesserung bezüglich Bewegung im Raum.
Hier ist mein aktuelles Programm:
https://www.roboternetz.de/phpBB2/vi...=302396#302396

Bisheriger Ansatz:
5 IR-Sensoren (links, vorne links, vorne, vorne rechts, rechts) liefern 8-Bit-Werte bezüglich Abstand.
Auswertung: Einheitsvektor * Sensorwert * weightfactor
(weightfactor: links: 1, vorne links: 2, vorne: 3, vorne rechts: 2, rechts: 1)

Das ergibt durch Summierung einen X- und Y-Wert.
Der Sensorwert vorne wird noch zusätzlich alleine betrachtet.
Schwingung (links/rechts und vorwärts/rückwärts) wird versucht zu vermeiden. Hier der entscheidende Part:

Code:

---

    /*
        MOTCO
       
        Mathematische Methode "x/y-Schwerpunkt der Sensorvektoren bilden":
        (Einheitsvektoren * 10) * Sensorwert (0-255) * weightfactor, davon Summe bilden

        VektorMalSensorSumme[...] 0 ist x-Wert und 1 ist y-Wert
        Blockade: y kann maximal 14790 groß werden (vl, v, vr 255)
        Richtung: x kann maximal -6120 (Hindernis links) bzw. +6120 (H. rechts) werden (l, vl 255 bzw. r, vr 255)
      */
     
        // Ermittlung von VektorMalSensorSumme[...] (gewichteter x- und y-Wert)
      VektorMalSensorSumme[0] = 0; // x-Wert
        VektorMalSensorSumme[1] = 0; // y-Wert

      // i entspricht links, vornelinks, vorne, vornerechts, rechts
      // j entspricht x und y
     
      for (i=0; i<5; ++i)
      {
          for (j=0; j<2; ++j)
          {
              VektorMalSensor[i][j] = Vektor[i][j] * irco_distance[i] * weightfactor[i];
              VektorMalSensorSumme[j] += VektorMalSensor[i][j];
          }
      }

      // Reaktion auf VektorMalSensorSumme[...] (x- und y-Wert)     
     
      // GrenzenY
      uint16_t GrenzeY1 = 12000; // Zustandsgrenze: BLOCKIERT / AUSWEICHEN
      uint16_t GrenzeY2 =  8000; // Zustandsgrenze: AUSWEICHEN / FREI
     
      // GrenzenX
      uint16_t GrenzeX1 =  5000; // Zustandsgrenze: LINKS / GERADEAUS / RECHTS

      // Zustandsvariable
      uint8_t zustand    = 0;
      uint8_t zustand_old = 0;     

      // Zustand ermitteln
      {
        if(VektorMalSensorSumme[1] >= GrenzeY1) // y-Wert
            zustand = BLOCKIERT;
           
        else if( (VektorMalSensorSumme[1] < GrenzeY1) && (VektorMalSensorSumme[1] >= GrenzeY2) ) // y-Wert
        {
            // x-Werte
            if(VektorMalSensorSumme[0] < (- GrenzeX1))
              zustand = HINDERNISLINKS;
       
            else if ( (VektorMalSensorSumme[0] >= (- GrenzeX1)) && (VektorMalSensorSumme[0] <= GrenzeX1) )
              zustand = GERADEAUS;
           
            else
              zustand = HINDERNISRECHTS; 
        }
        else
            zustand = FREI;
      }

      // Auf Zustand reagieren
      if(zustand == zustand_old)
      {
        // kein MOTCo-Befehl notwendig
      }
      else //Veränderung eingetreten
      {
          // Sondermaßnahmen
        // gegen Schwingung links/rechts: einmal GERADEAUS erzwingen
        if((zustand_old == HINDERNISLINKS) || (zustand_old == HINDERNISRECHTS))
        {
            zustand = GERADEAUS;
        }
        // gegen Schwingung vor/zurück: zweimal zurück
        if((zustand_old == BLOCKIERT) && (zustand == GERADEAUS))
        {
            zustand = BLOCKIERT;
        }
        // direkt vorne frei?
        if(irco_distance[2]<150)
        {
            zustand = zustand_old;     
        }
       
        //Allgemeine Maßnahmen           
        switch(zustand)
        {
            case FREI:
            //entry
            //do
              motco_setSpeed( 3*SPEEDFACTOR, 3*SPEEDFACTOR );  // rasch vorwärts
            //exit
            break;
            case HINDERNISRECHTS:
            //entry
            //do
              motco_setSpeed(  -SPEEDFACTOR,  SPEEDFACTOR );  // nach links drehen
            //exit
            break;
            case GERADEAUS:
            //entry
            //do
              motco_setSpeed( 2*SPEEDFACTOR, 2*SPEEDFACTOR );  // gemäßigt vorwärts 
            //exit
            break;
            case HINDERNISLINKS:
            //entry
            //do
              motco_setSpeed(  SPEEDFACTOR,  -SPEEDFACTOR );  // nach rechts drehen
            //exit
            break;
            case BLOCKIERT:
            //entry
            //do
              motco_setSpeed(-2*SPEEDFACTOR,-2*SPEEDFACTOR );  // rückwärts fahren
            //exit
            break;
        }
        zustand_old = zustand;
        motco_update();
      }
  }//Ende while-Hauptschleife

---

Frage:
Ist der Ansatz prinzipiell richtig? Welche Optimierungsstrategien machen Sinn? Welche Ansätze könnt ihr für C und ATmega128 noch empfehlen?