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

[ehenkes]

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/viewtopic.php?p=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:


/*
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?