Ich habe das Antikollisionsprogramm mit dem geometrischen Sensorschwerpunkt um Gewichtungsfaktoren ergänzt, einen Speedfactor zugefügt, mit den Grenzen etwas gespielt und versucht, Schwingungen vorzubeugen. Diese Grenzen hängen auch noch von der Tageszeit (Lichteinfluss IR), evtl. auch von der Akkuspannung (Messwerte IR) ab. Nibo bewegt sich damit schon, ohne sich zu verletzen.
Er bleibt manchmal noch unter Bürostuhlbeinen hängen. Da könnte man etwas mit der Odometrie machen. Da die Räder bei glatten Böden durchdrehen, geht aber kein einfacher Vergleich, ob sich das Rad überhaupt noch dreht. Gebogene schwarze Hindernisse, die IR schlucken und nach oben reflektieren, sind logischerweise auch ein physikalisches Problem. Gute Tipps?
Code:
/********************************************
*                                           *
*  N I B O   -   A N T I K O L L I S I O N  *
*                                           *
********************************************/

// Stand: 31.07.2007, 01:00h
// Erhard Henkes
// www.henkessoft.de

// Geometrischer Sensorschwerpunkt
// Gewichtung der Sensoren
// Einfaches Ausweichen nach Grenzwerten

// TODO: unter Bürostuhlbein einklemmen
//       fährt im Kreis, weil er z.B. immer links ausweicht

#include <stdlib.h>
#include <avr/interrupt.h>

#include "nibo/niboconfig.h"
#include "nibo/iodefs.h"

#include "nibo/delay.h"
#include "nibo/adc.h"
#include "nibo/pwm.h"
#include "nibo/i2cmaster.h"
#include "nibo/display.h"

#include "nibo/bot.h"
#include "nibo/leds.h"
#include "nibo/gfx.h"
#include "nibo/irco.h"
#include "nibo/motco.h"
#include "nibo/floor.h"

#define LINKS        0
#define VORNE_LINKS  1
#define VORNE        2
#define VORNE_RECHTS 3
#define RECHTS       4

#define SPEEDFACTOR 30

// Zustände
#define BLOCKIERT        1
#define AUSWEICHEN       2
#define FREI             0
#define HINDERNISLINKS   3
#define HINDERNISRECHTS  4
#define GERADEAUS        5


// Deklarationen von Hilfsfunktionen
void Init();
void float2string(float value, int decimal, char* valuestring);
void leds_set_status_all(uint8_t col0, uint8_t col1, uint8_t col2, uint8_t col3, uint8_t col4, uint8_t col5);
float SupplyVoltage(void);
void textout(int x, int y, char* str, int ft);


int main()
{
   Init();

   // Kollisionsvermeidung vorbereiten
    uint16_t Vektor[5][2]; // Einheitsvektoren (*10) [0] ist x- und [1] ist y-Wert
    Vektor[0][0] = -10; // LINKS x
    Vektor[0][1] =   0; // LINKS y
    Vektor[1][0] =  -7; // VORNE_LINKS x
    Vektor[1][1] =   7; // VORNE_LINKS y
    Vektor[2][0] =   0; // VORNE x
    Vektor[2][1] =  10; // VORNE y
    Vektor[3][0] =   7; // VORNE_RECHTS x
    Vektor[3][1] =   7; // VORNE_RECHTS y
    Vektor[4][0] =  10; // RECHTS x
    Vektor[4][1] =   0; // RECHTS y

   uint8_t weightfactor[5]; // Gewichtungsfaktor
   weightfactor[LINKS]        = 1;
   weightfactor[VORNE_LINKS]  = 2;
   weightfactor[VORNE]        = 3;
   weightfactor[VORNE_RECHTS] = 2;
   weightfactor[RECHTS]       = 1;

    uint16_t VektorMalSensor[5][2];   // Sensorwert * Einheitsvektor (*10)
    uint16_t VektorMalSensorSumme[2]; // Sensorschwerpunkt (x,y) für Auswertung

   // Vorbereitungen
   leds_set_displaylight(1000);
   leds_set_headlights(256);
   floor_enable_ir();
   motco_setPWM(512,512);
   motco_setSpeed(3,3);

   // fixe Display-Anzeigen
      textout(35,0,"Volt",      0);
    textout(0, 8,"distance:", 0);
   textout(0,24,"floor:",    0);
   textout(0,40,"line:",     0);   

   // Hauptschleife
   while(1)
   {
       // Akkuspannung anzeigen
       float Ubatt = SupplyVoltage();
        char text[6];
        float2string(Ubatt,2,text);     
        textout(0,0,"     ",0); // 5 Zeichen löschen
      textout(0,0,text,   0);

      // Abstandsmessung Raumgefühl
      irco_startMeasure();
      irco_update();
      
      // Floor
      uint16_t floor_distance[2];
      uint16_t line_distance[2];

      // Abstandsmessung Floor      
      floor_update();
      floor_distance[0] = floor_l;
      floor_distance[1] = floor_r;
      line_distance[0]  = line_l;
      line_distance[1]  = line_r;

      //Strings für Display
      char irco_string[5][5];
      char floor_string[2][5];
      char line_string[2][5];
       
      // Laufvariablen
      int i,j;
   
      /*
         IR-Abstandssensoren
      */
      
      for(i=0; i<5; ++i)
           textout(i*21,16,"      ",0); //löschen

      for(i=0; i<5; ++i) // z.Z. noch rechts 0 und links 4 !!!!!!!!!!!!!
       {
           itoa(irco_distance[i],irco_string[i],10);         
           textout(i*21,16,irco_string[i],0);
       }

      /*
         IR-Floorsensoren (Abgrunderkennung)
      */
      
      for(i=0; i<2; ++i)
           textout(i*28,32,"       ",0); //löschen

      for(i=0; i<2; ++i)
       {
           itoa(floor_distance[i],floor_string[i],10);         
           textout(i*28,32,floor_string[i],0);
       }

      /*
         IR-Liniensensoren
      */

      for(i=0; i<2; ++i)
           textout(i*28,48,"       ",0); //löschen

      for(i=0; i<2; ++i)
       {
           itoa(line_distance[i],line_string[i],10);         
           textout(i*28,48,line_string[i],0);
       }
      
      /*
         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]; // 4-i wegen IRCo?????
               VektorMalSensorSumme[j] += VektorMalSensor[i][j];
           }
      }

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

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

      // Zustand ermitteln
      {  // y-Wert
		 if( VektorMalSensorSumme[1] >=GrenzeY1)            zustand = BLOCKIERT;
         if((VektorMalSensorSumme[1] < GrenzeY1) && 
		    (VektorMalSensorSumme[1] >=GrenzeY2)) 
         {
            // x-Werte
            if( VektorMalSensorSumme[0] < GrenzeXlinks  )   zustand = HINDERNISLINKS;  
			if( VektorMalSensorSumme[0] > GrenzeXrechts )   zustand = HINDERNISRECHTS; 
            if((VektorMalSensorSumme[0] >=GrenzeXlinks) && 
			   (VektorMalSensorSumme[0] <=GrenzeXrechts))   zustand = GERADEAUS;       
         }
         if (VektorMalSensorSumme[1] < GrenzeY2)            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
            	leds_set_status_all(LEDS_OFF, LEDS_OFF, LEDS_GREEN, LEDS_GREEN, LEDS_OFF, LEDS_OFF); 
			//do
                motco_setSpeed( 3*SPEEDFACTOR, 3*SPEEDFACTOR );  // rasch vorwärts
				delay(10);
            //exit
            break;
            case HINDERNISRECHTS:
            //entry
            	leds_set_status_all(LEDS_OFF, LEDS_OFF, LEDS_OFF, LEDS_OFF, LEDS_ORANGE, LEDS_ORANGE); 
			//do
                motco_setSpeed(  -SPEEDFACTOR,   SPEEDFACTOR );  // nach links drehen
			    delay(10);
            //exit
            break;
            case GERADEAUS:
            //entry
				leds_set_status_all(LEDS_OFF, LEDS_OFF, LEDS_ORANGE, LEDS_ORANGE, LEDS_OFF, LEDS_OFF); 
            //do
                motco_setSpeed( 2*SPEEDFACTOR, 2*SPEEDFACTOR );  // gemäßigt vorwärts   
			    delay(10);
            //exit
            break;
            case HINDERNISLINKS:
            //entry
				leds_set_status_all(LEDS_ORANGE, LEDS_ORANGE, LEDS_OFF, LEDS_OFF, LEDS_OFF, LEDS_OFF); 
            //do
                motco_setSpeed(   SPEEDFACTOR,  -SPEEDFACTOR );  // nach rechts drehen
			    delay(10);
            //exit
            break;
            case BLOCKIERT:
            //entry
				leds_set_status_all(LEDS_OFF, LEDS_OFF, LEDS_RED, LEDS_RED, LEDS_OFF, LEDS_OFF);	 
            //do
                motco_setSpeed(-2*SPEEDFACTOR,-2*SPEEDFACTOR );  // rückwärts fahren
			    delay(10);
            //exit
            break;
         }
         zustand_old = zustand;
         motco_update();
      }
   }//Ende while-Hauptschleife

   while(1);
    return 0;
}


// Hilfsfunktionen

void Init()
{
    sei(); // enable interrupts

    i2c_init();
   pwm_init();
   display_init();
   
   bot_init();
   leds_init();
   floor_init();
    gfx_init();
}

void leds_set_status_all(uint8_t col0, uint8_t col1, uint8_t col2, uint8_t col3, uint8_t col4, uint8_t col5)
{
	leds_set_status(col0,0);	
	leds_set_status(col1,1);
	leds_set_status(col2,2);
	leds_set_status(col3,3);
	leds_set_status(col4,4);
	leds_set_status(col5,5);
}

float SupplyVoltage(void)
{
   bot_update();      
   return(0.0166 * bot_supply - 1.19);
}

void textout(int x, int y, char* str, int ft)
{
   gfx_move(x,y);
   gfx_print_text(str,ft);
}

void float2string(float value, int decimal, char* valuestring)
{
    int neg = 0;    char tempstr[20];
   int i = 0;   int j = 0;   int c;    long int val1, val2;
   char* tempstring;
   tempstring = valuestring;
   if (value < 0){   neg = 1; value = -value; }
     for (j=0; j < decimal; j++)   {value = value * 10;}
    val1 = (value * 2);
    val2 = (val1 / 2) + (val1 % 2);
    while (val2 !=0){
       if ((decimal > 0) && (i == decimal)){
          tempstr[i] = (char)(0x2E);
          i++;
       }
       else{
          c = (val2 % 10);
          tempstr[i] = (char) (c + 0x30);
          val2 = val2 / 10;
          i++;
       }
    }
    if (neg){
       *tempstring = '-';
       tempstring++;
    }
    i--;
    for (;i > -1;i--){
       *tempstring = tempstr[i];
       tempstring++;
    }
    *tempstring = '\0';
}