Stand 05.08.2010 - Dr.-Ing. Erhard Henkes

Nibo - Display-Roboter mit Raumgefühl


Foto: Dr. Erhard Henkes    "Nibo bei Nacht" (Digitalkameras "sehen" die IR-LEDs (violett, liegend vorne) im Gegensatz zum menschlichen Auge)

  1. Einführung
    1. Programmierung des Nibo
      1. ISP-Programmierung
      2. Display und LEDs
      3. Motorsteuerung mit Hilfe des Motorcontrollers (ATtiny44)
      4. Abstandsmessung mit Hilfe des Infrarotcontrollers (ATtiny44)
      5. Fünf IR-Abstandsmessungen für ein erstes Antikollisions-Programm verwenden
      6. RC5-Code
      7. C++-Programmierung des Nibo
    2. Nibo2



Links: "Hallo Welt" - Programm                                      Rechts: Nibo (mitte oben) im Vergleich mit drei ASUROs
Fotos: Dr. Erhard Henkes

Einführung

Der auf Erfahrungen mit ASURO, c't-Bot und Fußballrobotern basierende Roboterbausatz Nibo ist seit Juli 2007 bei nicai-systems erhältlich. Seit dem 20.07.2007 steht die erste C/C++-Bibliothek für Nibo zum Download bereit, deren Dokumentation man hier findet. Ich erachte den Roboterbausatz Nibo als empfehlenswertes Upgrade, wenn man bereits Erfahrungen mit ASURO gemacht hat. Er hat ein vergleichbares Antriebssystem aber deutlich mehr Möglichkeiten.

Anfängern im Löten wird empfohlen, nicht direkt mit der Bestückung der Platine des Nibo zu beginnen, da diese sehr filigran aufgebaut ist.
Im Nibo wiki findet man folgenden Beitrag zum Thema Löten:
http://nibo.editthis.info/wiki/L%C3%B6ten

Folgendes Video ist diesbezüglich auch instruktiv:
http://www.curiousinventor.com/guides/How_To_Solder

Nun lassen wir aber den Nibo selbst zu Wort kommen:
Video des Nibo (nicai-systems)


Der Nibo besitzt ein eigenwilliges Design, das bezüglich des Antriebes vom ASURO beeinflusst ist, und durch die Räder an einen "truck" oder durch die Form an einen "Traktor" erinnert. Den ASURO schlägt er in Punkto Stabilität und Aufbau um Längen. Der berühmte halbe Tischtennisball wurde z.B. durch einen Teflongleiter ersetzt, der auf eine Holzscheibe (großer Dame-/Mühlestein?) geklebt wird. Der Holzstein wird in der Mitte der Platine von oben in ein vorgebohrtes Loch sauber verschraubt. Das ist wirklich professionell. Die Gleiteinrichtung ist damit leicht abnehm- bzw. austauschbar.


Foto: Dr. Erhard Henkes (Nibo's Unterseite, Teflongleiter, fünf IR-Sensoren, vier CNY70, 25:1-Getriebe)

Der ATmega128 - via I²C flankiert durch seine beiden Helfer ATtiny44 - bietet viel Raum, auch für nicht optimierte Programme. Rechts oben auf der Platine erkennt man den 6-Pin-ISP-Anschluss für die Programmierung z.B. mit einem STK500 und AVR Studio. Der 20-Pin-Anschluss ist für ein optionales Grafik-Display (siehe Bild oben), das sich unbedingt empfiehlt, vorgesehen. Ein 10-Pin-Anschluss (oben neben dem leeren Sockel) kombiniert die beiden fünfadrigen Kabel, die zu den beiden Odometriesensorplatinen führen. Ein weiterer 10-Pin-Anschluss (unten neben dem ATtiny44) dient als Erweiterungsport. Der freie Sockel nimmt den Motortreiber L293D auf. Ein 16 MHz Quarz (unten rechts) gibt den maximalen Takt für den ATmega128 vor, der damit auch zuverlässige 400 kHz am I²C-Bus ermöglicht.


Foto: E. Henkes (Antrieb ohne Odometriesensorenplatinen)

Der Antrieb des Nibo erfolgt mit zwei Motoren vom Typ 2025-02 (IGARASHI) mit 3-12 Volt Gleichspannung, 0,001 Nm max. Drehmoment und einer Getriebeuntersetzung von 25:1 (analog ASURO). Der geschraubte Aluminiumrahmen beeindruckt durch solide Professionalität. Die Getriebezahnräder besitzen vier Innenlöcher für die Odometrie.

Man sieht hier auch sehr gut die empfehlenswerte Abdeckung für die Odometrie des Nibo, die 20 Zähler pro Radumdrehung liefert.

Ich rate dringend zum Einsatz des Grafikdisplays, denn dieses bietet mit 64*128 Pixel Platz für 8 Zeilen Text mit jeweils 21 Zeichen. Nibo's C-Bibliothek enthält bereits fertige - allerdings noch nicht optimierte - Routinen, die zumindest die Ausgabe von Text auf dem Display zum Kinderspiel machen.



links: Nibo "aufgeschraubt" (Foto: E. Henkes)           

vgl. auch: Blockschaltbild des Nibo (nicai-systems)

Reizvoll an Nibo ist das Zusammenspiel der drei Microcontroller von Anfang an. Dies praktisch zu erleben, ist wirklich lehrreich, denn dabei kommt man am Verständnis des I²C-Bus keinesfalls vorbei.

Programmierung des Nibo

Für den Nibo gibt es eine Bibliothek für C und C++. Die Java-Bibliothek ist noch in Entwicklung. Aufsteiger vom ASURO zum Nibo werden wohl eher im gewohnten C programmieren wollen. Dafür steht bereits in der ersten Version eine wirklich ordentlich strukturierte C-Bibliothek (aktuelle Version vom 25.07.2007) als Grundlage zur Verfügung, die es relativ rasch erlaubt, eigene Programme auf hohem Niveau zu entwickelen. Dafür sorgt der vorbildlich aufgebaute Header iodefs.h und die weitere Unterteilung in Low Level und High Level Routien und insbesondere die Routinen für den Motorcontroller und IR-Controller (fünf Abstandssensoren).

ISP-Programmierung

Programmiert wird der Nibo noch professionell über eine Standard-6-Pin-ISP-Schnittstelle, z.B. mit dem Atmel STK500 Starter Kit und der kostenlosen Software AVR Studio (ebenfalls von Atmel, aktuelle Version 4.13 (kostenlos im Internet) oder Version 4.12 (auf Datenträger beim STK500 dabei) - unbedingt mit Service-Paket! - verwenden). Ich kann dieses integrierte System nur empfehlen. Das AVR Studio bietet vor allem einen brauchbaren Debugger, mit dem man Programme schrittweise durchgehen und dabei detailliert die einzelnen Zustände des Microcontrollers analysieren kann.

Die Steckerbelegung findet man hier.


Foto: E. Henkes (Sechsadrige ISP-Verbindung zwischen Atmel STK500 Starter Kit und Nibo)


Fangen wir mit einigen einfachen Sourcecodes zur Demonstration der Möglichkeiten des Nibo an.


Foto: E. Henkes (Nibo besitzt sechs Duo-LEDs. Rot plus Grün ergibt Orange)


Display und LEDs

Hier folgt als Beispiel der Sourcecode in der Programmiersprache C für das bei einem Display obligatorische "Hallo Welt!"-Programm (Bild siehe oben):

#include <avr/interrupt.h>

#include "nibo/niboconfig.h"

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

#include "nibo/bot.h"
#include "nibo/gfx.h"
#include "nibo/leds.h"

int main()
{
    sei(); // enable interrupts
    bot_init();
    leds_init();
    pwm_init();
    display_init();
    gfx_init();
 
    leds_set_displaylight(800);

    gfx_move(10,10);
    gfx_print_text("Nibo sagt:", 0);

    gfx_move(10,30);
    gfx_print_text("Hallo Welt!", 0);
 
    int i,j;
    for(i=0;i<10;++i)
    {   
        for(j=0;j<6;++j)
        {
            leds_set_status(LEDS_RED,j);
            delay(500);
        }

        leds_set_headlights(256);

        for(j=0;j<6;++j)
        {
            leds_set_status(LEDS_ORANGE,j);
            delay(500);
        }

        leds_set_headlights(512);

        for(j=0;j<6;++j)
        {
            leds_set_status(LEDS_GREEN,j);
            delay(500);
        }

        leds_set_headlights(1024);
    }

    while(1);
    return 0;
}

Man erkennt, auf welch einfache Weise man Text auf dem für diese Roboterklasse herrlich großen LC-Grafik-Display (64*128 Pixel) ausgeben kann. Dies ist ein wichtiger Vorteil, und zwar nicht nur für Einsteiger in die Programmierung von Microcontrollern.

Als nächstes kümmern wir uns um die Anzeige der Versorgungsspannung:

#include <avr/interrupt.h>

#include "nibo/niboconfig.h"

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

#include "nibo/leds.h"
#include "nibo/bot.h"
#include "nibo/gfx.h"
#include "nibo/bot.h"


void float2string(float value, int decimal, char* valuestring)
{
      /* do it yourself */
}

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 Init(void)
{
    sei(); // enable interrupts
    bot_init();
    leds_init();
    pwm_init();
    display_init();
    gfx_init();
}

int main()
{
    Init();
 
    leds_set_displaylight(1000);

    float Ubatt = SupplyVoltage();
    char text[6];
    float2string(Ubatt,2,text);     

    textout(4,10,"Versorgungsspannung:",0);
    textout(4,24,text,0);
    textout(60,24,"Volt",0);

    while(1);
    return 0;
}




Foto: E. Henkes (Versorgungsspannung)

Mit acht Akkus (8*1,2 V = 9,6 V) besitzt Nibo ein gewaltiges Kraftpaket, das er aber auch benötigt. Man sollte sich einen zweiten oder gar dritten Akkusatz besorgen, wenn man nicht auf dem Trockenen sitzen will. Nibo ist ein echter Stromfresser, dafür aber auch ein tolles Kraftpaket.


Motorsteuerung mit Hilfe des Motorcontrollers (ATtiny44)

Als nächstes wenden wir uns der Steuerung der beiden Motoren zu. Folgende Variablen und Funktionen findet man in der C/C++-Bibliothek des Nibo:

/*!
 * Aktuelle Geschwindigkeit des linken Rades
 */

extern int16_t motco_speed_l;

/*!
 * Aktuelle Geschwindigkeit des rechten Rades
 */

extern int16_t motco_speed_r;

/*!
 * Aktuelle Inkrements des linken Rades
 */

extern int16_t motco_ticks_l;

/*!
 * Aktuelle Inkrements des rechten Rades
 */

extern int16_t motco_ticks_r;

/*!
 * Aktualisiert alle Daten
 */

uint8_t motco_update();

/*!
 * Motoren anhalten
 */

uint8_t motco_stop();

/*!
 * PWM Werte für die beiden Motoren setzen. Die PWM Werte sind bei idealisierter
 * Mechanik und Elektonik proportional zum Drehmoment.
 * @param left Wert für linkes Rad (-1024 ... +1024)
 * @param right Wert für rechets Rad (-1024 ... +1024)
 */

uint8_t motco_setPWM(int16_t left, int16_t right);

/*!
 * Geschwindigkeit für die beiden Motoren setzen. Die Werte werden in Ticks/Sekunde
 * angegeben. Zwanzig Ticks entsprechen einer Radumdrehung.
 * @param left Sollwert für linkes Rad
 * @param right Sollwert für rechets Rad
 */

uint8_t motco_setSpeed(int16_t left, int16_t right);

/*!
 * Regelungsparameter setzen. Weitere Infos im Wiki unter
 * http://nibo.editthis.info/wiki/Motorcontroller-Firmware
 * @param ki Integralanteil
 * @param kp Proportionalanteil
 * @param kd Differentialanteil
 */

uint8_t motco_setParameters(int8_t ki, int8_t kp, int8_t kd);

/*!
 * Odometriewerte auf angegebene Werte zurücksetzen
 * @param left Wert für linkes Rad
 * @param right Wert für rechets Rad
 */

uint8_t motco_resetOdometry(int16_t left, int16_t right);


Hier ein erstes Programm, das den Nibo mittels einer Funktion Go(distance,speed) geradeaus vorwärts bewegt. Das Programm ist noch im Experimentierstadium:

#include <avr/interrupt.h>

#include "nibo/niboconfig.h"

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

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


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

void Go(int dist, int cm_pro_sec)
{
    const float TICKS_PER_CM = 1.75f;

    motco_resetOdometry(0,0);
    motco_update();
   
    int limit = dist * TICKS_PER_CM;
   
   
    int pwm   = cm_pro_sec * 80;
    if (pwm >  1024)  pwm =  1024;
    if (pwm < -1024)  pwm = -1024;

    motco_setPWM(pwm,pwm);
    motco_setSpeed(cm_pro_sec * TICKS_PER_CM , cm_pro_sec * TICKS_PER_CM);
    motco_update();

    delay(3000); //TODO: muss weg  

    do
    {   
        motco_update(); /* hoffentlich geradeaus fahren */
    }
    while(motco_ticks_l < limit);
}

int main()
{
    sei(); // enable interrupts
    bot_init();
    leds_init();
    pwm_init();
    display_init();
    gfx_init();
 
    leds_set_displaylight(800);

    textout(10,10,"Nibo meldet:",0);
    textout(10,24,"Motor an!",0);
    textout(10,38,"Licht an!",0);
       
    leds_set_headlights(512);
     
    int i,j;
    for(i=1;i<=6;++i)
    {   
        for(j=0;j<i;++j)
            leds_set_status(LEDS_GREEN,j);
       
        Go(50,10*i);
    }

    leds_set_headlights(0);
   
    motco_stop();
    motco_update();
   
    for(j=0;j<6;++j)
        leds_set_status(OFF,j);
   
    textout(10,24,"Motor aus!",0);
    textout(10,38,"Licht aus!",0);
   
    while(1);
    return 0;
}


Hier das zum Programm zugehörige Video.


Abstandsmessung mit Hilfe des Infrarotcontrollers (ATtiny44)

Die C/C++-Bibliothek des Nibo bietet momentan folgende Variablen und Funktionen:

/*!
 * Aktueller Modus des IR-Controllers
 */
extern uint8_t irco_mode;

/*!
 * Aktuelle Reflexionswerte der Distanzsensoren
 */
extern uint8_t irco_distance[5];

/*!
 * Zuletzt empfangenes RC5 Kommando
 */
extern uint16_t irco_rc5_cmd;

/*!
 * Daten des IRCO aktualisieren
 */
uint8_t irco_update();

/*!
 * Aussenden von IR-Licht stoppen, nur Empfang
 */
uint8_t irco_stop();

/*!
 * Reflexionsmessung starten
 */
uint8_t irco_startMeasure();

/*!
 * RC5 Code übertragen
 * @param rc5 RC5 Code
 */
uint8_t irco_transmitRC5(uint16_t rc5);



Wir testen die Möglichkeiten der Abstandsmessung mit den fünf IR-Sensoren:

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

#include "nibo/niboconfig.h"

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

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

void float2string(float value, int decimal, char* valuestring)
{
 //... siehe oben!
}

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 Init(void)
{
    sei(); // enable interrupts
    bot_init();
    leds_init();
    pwm_init();
    display_init();
    gfx_init();
}

int main()
{
    Init();
 
    leds_set_displaylight(1000);

    while(1)
    {
        float Ubatt = SupplyVoltage();
        char text[6];
        float2string(Ubatt,2,text);     

        textout( 0,0,text,  0);
        textout(35,0,"Volt",0);

        irco_update();
        irco_startMeasure();
        irco_update();
       
        char irco_string[5][5];
        int i;
       
        for(i=0; i<5; ++i)
        {
             textout(i*21,8,"      ",0); //löschen
        }

        for(i=0; i<5; ++i)
        {
             itoa(irco_distance[i],irco_string[i],10);         
             textout(i*21,8,irco_string[i],0);
        }
        delay(200);
    }

    while(1);
    return 0;
}


Fünf IR-Abstandsmessungen für ein erstes Antikollisions-Programm verwenden

Das Raumgefühl des Nibo, dass man durch den Einsatz von fünf IR-Sensoren erreicht, hat seinen Vorteil. Hier finden Sie ein Programm, das einen gewichteten Sensorschwerpunkt (Summe der Sensorwerte mal zehn mal Einheitsvektoren mal Gewichtungsfaktor) berechnet und darauf sehr einfach reagiert:

/********************************************
*                                           *
*  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)
{
      /* do it yourself */
}


Damit kann Nibo bereits Hindernissen ausweichen und sich auf diese Art prinzipiell ungerichtet im Raum bewegen. Die Reaktionen auf den "gewichteten Sensorschwerpunkt" und die Grenzwerte (absolut oder relativ) kann man selbstverständlich weiter optimieren. Dies ist lediglich eine erste Demonstration der prinzipiellen Möglichkeiten des Nibo.

RC5-Code

Eine der Herausforderungen besteht darin RC5-Code an den Nibo zu senden und mit diesem zu empfangen. Dafür benötigt man zunächst eine Fernbedienung, die den sogenannten RC5-Code ausstrahlt. Bei mir war es so, dass keine im Haushalt funktionierte, auch eine Philips-Fernbedienung nicht! Ich habe mir daraufhin eine kostengünstige Universalfernbedienung vom Typ SupportPlus 8in1 SP-URC-81-ME besorgt. Folgende Einstellungen führten beim Nibo zum Erfolg:

1) Auf SET und TV1 gleichzeitig drücken, bis LED erlischt.
2) 0 2 3 eingeben (einer der vielen Codes für Philips, 001 und 004 funktionierten nicht!)
3) RC5-Code austrahlen. Funktioniert bei ca. 5 Meter noch sicher.
 
Testen kann man dies beim Nibo mit der Initializer.hex, die den empfangenen RC5-Code im Grafikdisplay anzeigt.

Die Nibo-Bibliothek ist auf den Empfang und auf das Senden von RC5-Codes bereits vorbereitet.

/*!
 * Zuletzt empfangenes RC5 Kommando
 */
extern uint16_t irco_rc5_cmd;


/*!
 * RC5 Code übertragen
 * @param rc5 RC5 Code
 */
uint8_t irco_transmitRC5(uint16_t rc5);


#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/floor.h"  

#define LOBYTE(x)        (uint8_t)((uint16_t)x)
#define HIBYTE(x)        (uint8_t)(((uint16_t)x)>>8)

// Hilfsfunktion
void print_hex (uint8_t val);


int main()
{
   
    sei(); // enable interrupts

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

    irco_startMeasure();
    leds_set_displaylight(1000);
   
     while(1)
    {
        irco_update();   
       
        gfx_move(0,0);
        gfx_print_text("RC5: ", 1);
       
        gfx_move(30,0);
        print_hex(HIBYTE(irco_rc5_cmd));
        print_hex(LOBYTE(irco_rc5_cmd));

        // Fernbedienung Einstellung: TV1, 023
        switch(irco_rc5_cmd)
        {
            case 0x3801: //1 (Ziffernblock der Fernbedienung)
                for(i=0; i<6; ++i)
                    leds_set_status(LEDS_GREEN,i);
                break;
   
            case 0x3002: //2
                for(i=0; i<6; ++i)
                    leds_set_status(LEDS_ORANGE,i);
                break;

            case 0x3803: //3
                for(i=0; i<6; ++i)
                    leds_set_status(LEDS_RED,i);
                break;
            case 0x3000: //0
                for(i=0; i<6; ++i)
                    leds_set_status(LEDS_OFF,i);
                break;
            // etc.
        }
    }


    return 0;
}

void print_hex (uint8_t val)
{
  char c1=val/16;
  char c2=val%16;
  if (c1<10) c1+='0'; else c1+='a'-10;
  if (c2<10) c2+='0'; else c2+='a'-10;
  gfx_print_char(c1, 0);
  gfx_print_char(c2, 0);
}


Mit diesem einfachen Programm kann Nibo bereits RC5-Code empfangen. Damit ist eine Fernsteuerung bzw. Kommunikation auf diesem Wege über Entfernungen von einigen Metern möglich. Wie man sieht, kann man sich damit auf einfache Weise eigene Test- bzw. Steuerprogramme schreiben.

C++-Programmierung des Nibo

Die 4096 Bytes SRAM des ATmega128 bieten ausreichend Platz für die in C++ typischen dynamischen Speicheranforderungen. Allerdings werden new/delete bzw. new[]/delete[] vom aktuellen WinAVR noch nicht unterstützt. Bei abstrakten Klassen kommt es zu Problemen mit "pure virtual". Beide Punkte lassen sich aber leicht umgehen:

// Hilfskonstruktionen
//
// pure virtual wird dennoch ausgeführt:
extern "C"{
void __cxa_pure_virtual() {} }
//
// Ersatz für new, new[], delete und delete[] der fehlenden C++-Standard-Bibliothek
void* operator new      (size_t size) { return malloc(size); }
void* operator new[]    (size_t size) { return malloc(size); }
void  operator delete   (void* ptr)   { free(ptr); }
void  operator delete[] (void* ptr)   { free(ptr); }
//

Die C++-Schlüsselwörter new/delete werden durch das klassische malloc/free aus C ersetzt und für den ins Leere führenden Aufruf der Funktion __cxa_pure_virtual()schafft man eine Sprungmarke mit einem leeren Rumpf.

Etwas schwierig gestaltet sich die Erstellung eines C++-Programms mit AVR Studio und WinAVR, denn ersteres ist nicht wirklich für C++ eingerichtet.
Das beginnt mit der nicht aufgeführten Endung cpp und hört damit auf, dass man das Makefile erst exportieren, dann manipulieren und als externes Makefile verwenden muss. Dies bedeutet, dass man das Makefile manuell selbst bei weiteren Änderungen pflegen muss. Ohne Kenntnis der elementaren Syntax des Makefiles kommt man also hier nicht weiter. Das hat aber auch seine Vorteile, weil man sich detailliert mit der Entstehung des Binärprogramms beschäftigt.

Bezüglich der Gestaltung des Makefile findet man hier eine Übersicht: http://www.mikrocontroller.net/arti.....utorial#Exkurs:_Makefiles

Ein Makefile ist nichts anderes als eine Sammlung von "Steueranweisungen" für Compiler, Assembler, Linker/Locator, Konverter und ggf. Flashprogramm. Hinzu kommen können Kopier- und Löschbefehle.

Man sollte den Gesamtprozess des Erstellens eines Maschinenprogramms in Ruhe analysieren. Hier findet man einige Links:

http://www.roboternetz.de/wissen/index.php/Bild:Avr-gcc-1.png

http://www.roboternetz.de/wissen/in.....blauf_der_Codegenerierung

Verfolgt man den Weg vom C/C++ Sourcecode (Quellcode) bis zum Programm im Flash-Speicher, so findet man folgenden "Reaktionspfad":

C/C++ -> Assembler -> Objektcode -> elf-Datei (ELF = Executable and Linking Format)
-> hex-Datei -> Binäres Programm im Flash-Speicher


Der Linker/Locator verbindet mehrere Objektdateien (o) und Archivdateien (a) zu einer einzigen Datei, replaziert ihre Daten und verbindet ihre Symbolreferenzen.

C/C++ -> Assembler Compiler C:\WinAVR\bin\avr-gcc.exe 
C:\WinAVR\bin\avr-g++.exe
Assembler -> Objektcode Assembler C:\WinAVR\bin\avr-as.exe
Objektcode -> elf-Datei Linker / Locator C:\WinAVR\bin\ld.exe
elf-Datei -> hex-Datei Konverter
C:\WinAVR\bin\avr-objcopy.exe
hex-Datei "flashen" Flash-Programm
C:\WinAVR\bin\avrdude.exe

Den gesamten Prozess kann man im Makefile nachvollziehen. Nachfolgend finden Sie das Makefile eines Projektes in C++ im Rahmen einer ersten Bewegungsprogrammierung mit Hilfe des "state pattern design":

###############################################################################
# Makefile for the project Nibo009
###############################################################################

## General Flags
PROJECT = Nibo009
MCU = atmega128
TARGET = Nibo009.elf
CC = avr-c++.exe

## Options common to compile, link and assembly rules
COMMON = -mmcu=$(MCU)

## Compile options common for all C compilation units.
CFLAGS = $(COMMON)
CFLAGS += -Wall -gdwarf-2   -ffunction-sections  -I..   -DF_CPU=16000000UL -Os -fsigned-char -fno-exceptions
CFLAGS += -MD -MP -MT $(*F).o -MF dep/$(@F).d

## Assembly specific flags
ASMFLAGS = $(COMMON)
ASMFLAGS += $(CFLAGS)
ASMFLAGS += -x assembler-with-cpp -Wa,-gdwarf2

## Linker flags
LDFLAGS = $(COMMON)
LDFLAGS +=


## Intel Hex file production flags
HEX_FLASH_FLAGS = -R .eeprom

HEX_EEPROM_FLAGS = -j .eeprom
HEX_EEPROM_FLAGS += --set-section-flags=.eeprom="alloc,load"
HEX_EEPROM_FLAGS += --change-section-lma .eeprom=0 --no-change-warnings


## Include Directories
INCLUDES = -I"C:\Nibo_SRC\Projekte\nibo009\..\..\inc"

## Libraries falls notwendig z.B. -lm (manche Fkt. aus math.h sind dort)

## Objects that must be built in order to link
OBJECTS = adc.o bot.o delay.o display.o floor.o gfx.o i2cmaster.o irco.o leds.o motco.o pwm.o text.o Roboter.o StateFree.o StateObstacleFront.o StateObstacleLeft.o StateObstacleRight.o test.o 

## Objects explicitly added by the user
LINKONLYOBJECTS =

## Build
all: $(TARGET) Nibo009.hex Nibo009.eep size

## Compile
adc.o: ../../../nibo/adc.c
    $(CC) $(INCLUDES) $(CFLAGS) -c  $<

bot.o: ../../../nibo/bot.c
    $(CC) $(INCLUDES) $(CFLAGS) -c  $<

delay.o: ../../../nibo/delay.c
    $(CC) $(INCLUDES) $(CFLAGS) -c  $<

display.o: ../../../nibo/display.c
    $(CC) $(INCLUDES) $(CFLAGS) -c  $<

floor.o: ../../../nibo/floor.c
    $(CC) $(INCLUDES) $(CFLAGS) -c  $<

gfx.o: ../../../nibo/gfx.c
    $(CC) $(INCLUDES) $(CFLAGS) -c  $<

i2cmaster.o: ../../../nibo/i2cmaster.c
    $(CC) $(INCLUDES) $(CFLAGS) -c  $<

irco.o: ../../../nibo/irco.c
    $(CC) $(INCLUDES) $(CFLAGS) -c  $<

leds.o: ../../../nibo/leds.c
    $(CC) $(INCLUDES) $(CFLAGS) -c  $<

motco.o: ../../../nibo/motco.c
    $(CC) $(INCLUDES) $(CFLAGS) -c  $<

pwm.o: ../../../nibo/pwm.c
    $(CC) $(INCLUDES) $(CFLAGS) -c  $<

text.o: ../../../nibo/text.c
    $(CC) $(INCLUDES) $(CFLAGS) -c  $<

test.o: ../test.cpp
    $(CC) $(INCLUDES) $(CFLAGS) -c  $<

Roboter.o: ../Roboter.cpp
    $(CC) $(INCLUDES) $(CFLAGS) -c  $<   

StateFree.o: ../StateFree.cpp
    $(CC) $(INCLUDES) $(CFLAGS) -c  $<
   
StateObstacleFront.o: ../StateObstacleFront.cpp
    $(CC) $(INCLUDES) $(CFLAGS) -c  $<

StateObstacleLeft.o: ../StateObstacleLeft.cpp
    $(CC) $(INCLUDES) $(CFLAGS) -c  $<

StateObstacleRight.o: ../StateObstacleRight.cpp
    $(CC) $(INCLUDES) $(CFLAGS) -c  $<   
   
##Link
$(TARGET): $(OBJECTS)
     $(CC) $(LDFLAGS) $(OBJECTS) $(LINKONLYOBJECTS) $(LIBDIRS) $(LIBS) -o $(TARGET)

%.hex: $(TARGET)
    avr-objcopy -O ihex $(HEX_FLASH_FLAGS)  $< $@

%.eep: $(TARGET)
    -avr-objcopy $(HEX_EEPROM_FLAGS) -O ihex $< $@ || exit 0

%.lss: $(TARGET)
    avr-objdump -h -S $< > $@

size: ${TARGET}
    @echo
    @avr-size -C --mcu=${MCU} ${TARGET}

## Clean target
.PHONY: clean
clean:
    -rm -rf $(OBJECTS) Nibo009.elf dep/* Nibo009.hex Nibo009.eep

## Other dependencies
-include $(shell mkdir dep 2>/dev/null) $(wildcard dep/*)



Hier sind die im Makefile aufgeführten Source-Dateien dieses Projektes:

#ifndef NIBO_H
#define NIBO_H

#include <avr/interrupt.h>
#include "nibocc/niboconfig.hpp"
#include "nibocc/i2cmaster.hpp"
#include "nibocc/adc.hpp"
#include "nibocc/bot.hpp"
#include "nibocc/delay.hpp"
#include "nibocc/display.hpp"
#include "nibocc/floor.hpp"
#include "nibocc/graphicdisplay.hpp"
#include "nibocc/irco.hpp"
#include "nibocc/leds.hpp"
#include "nibocc/motco.hpp"
#include "nibocc/pwm.hpp"
#include "nibocc/textdisplay.hpp"
#include <stdlib.h>
#include <stdint.h>
#include <string.h>

#endif


#ifndef ROBOTER_H
#define ROBOTER_H

#include "State.h"

const int SPEEDFACTOR   = 35;

const int FREE          =  0;
const int OBSTACLEFRONT =  1;
const int OBSTACLELEFT  =  2;
const int OBSTACLERIGHT =  3;

class Roboter
{
   
    int condition;
    State* m_pCurrentState;
 
 public:
   
    Roboter();
    ~Roboter(){};
   
    int  getCondition(){return condition;}
    void setCondition(int val){condition=val;}
   
    void MoveAhead();
    void MoveBack();
    void MoveLeft();
    void MoveRight();
   
    void Update();
    void ChangeState(State* pNewState);
}; 

#endif

#ifndef STATE_H
#define STATE_H

class Roboter;

class State // abstrakte Klasse
{
public:
  virtual ~State(){}
  virtual void Execute(Roboter*)=0;
};

#endif

#ifndef STATE_FREE_H
#define STATE_FREE_H

#include "StateObstacleFront.h"
#include "StateObstacleLeft.h"
#include "StateObstacleRight.h"
#include "Roboter.h"

class State_Free : public State
{
public:
  State_Free(){}    
  virtual void Execute(Roboter* bot);
};

#endif

#ifndef STATE_OBSTACLEFRONT_H
#define STATE_OBSTACLEFRONT_H

#include "Roboter.h"
#include "StateObstacleLeft.h"
#include "StateObstacleRight.h"
#include "StateFree.h"

class State_ObstacleFront : public State
{
public:
  State_ObstacleFront(){}
  virtual void Execute(Roboter* bot);
};

#endif

#ifndef STATE_OBSTACLELEFT_H
#define STATE_OBSTACLELEFT_H

#include "Roboter.h"
#include "StateFree.h"
#include "StateObstacleFront.h"
#include "StateObstacleRight.h"

class State_ObstacleLeft : public State
{
public:
  State_ObstacleLeft(){}
  virtual void Execute(Roboter* bot);
};

#endif

#ifndef STATE_OBSTACLERIGHT_H
#define STATE_OBSTACLERIGHT_H

#include "Roboter.h"
#include "StateFree.h"
#include "StateObstacleFront.h"
#include "StateObstacleLeft.h"

class State_ObstacleRight : public State
{
public:
  State_ObstacleRight(){}
  virtual void Execute(Roboter* bot);
};

#endif

#ifndef UTIL_H
#define UTIL_H

#include "nibo.h"
using namespace nibocc;

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

void float2string(float value, int decimal, char* valuestring)
{
 
      /* do it yourself */
}

void textout(int x, int y, char* str, int ft)
{
   Graficdisplay::move(x,y);
   Graficdisplay::print_text(str, ft);
}

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);
}

void Init()
{
    sei();
     Bot::init();
    I2CMaster::init();  
    Pwm::init();
    Leds::init();
    Floor::init();
    Display::init();
    Graficdisplay::init();
}

// Hilfskonstruktionen
// pure virtual wird dennoch ausgeführt:
extern "C"{
void __cxa_pure_virtual() {} }

// Ersatz für new, new[], delete und delete[] der fehlenden C++-Standard-Bibliothek
void* operator new      (size_t size) { return malloc(size); }
void* operator new[]    (size_t size) { return malloc(size); }
void  operator delete   (void* ptr)   { free(ptr); }
void  operator delete[] (void* ptr)   { free(ptr); }


#endif

//Roboter.cpp

#include "nibo.h"

#include "Roboter.h"
#include "StateFree.h"
using namespace nibocc;

void static 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);
}


Roboter::Roboter() : condition (FREE), m_pCurrentState (new State_Free())
{     
}


void Roboter::Update()
{
    m_pCurrentState->Execute(this);
}

void Roboter::ChangeState(State* pNewState)
{
    delete m_pCurrentState;
    m_pCurrentState = pNewState;
}

void Roboter::MoveAhead()
{

    //entry
    leds_set_status_all(LEDS_OFF, LEDS_OFF, LEDS_GREEN, LEDS_GREEN, LEDS_OFF, LEDS_OFF);
    //do
    MotCo::set_speed( 3*SPEEDFACTOR, 3*SPEEDFACTOR ); 
    MotCo::update();
   
    //exit
}

void Roboter::MoveBack()
{
    //entry
    leds_set_status_all(LEDS_OFF, LEDS_OFF, LEDS_RED, LEDS_RED, LEDS_OFF, LEDS_OFF);     
    //do
    MotCo::set_speed(-2*SPEEDFACTOR,-2*SPEEDFACTOR );
    MotCo::update();   
    //exit
}


void Roboter::MoveLeft()
{
    //entry
    leds_set_status_all(LEDS_OFF, LEDS_OFF, LEDS_OFF, LEDS_OFF, LEDS_ORANGE, LEDS_ORANGE);
    //do
    MotCo::set_speed(  -SPEEDFACTOR,   SPEEDFACTOR ); 
    MotCo::update();
    //exit
}

void Roboter::MoveRight()
{
    //entry
    leds_set_status_all(LEDS_ORANGE, LEDS_ORANGE, LEDS_OFF, LEDS_OFF, LEDS_OFF, LEDS_OFF);
    //do
    MotCo::set_speed(   SPEEDFACTOR,  -SPEEDFACTOR ); 
    MotCo::update();
    //exit
}

//StateFree.cpp

#include "StateObstacleLeft.h"
#include "StateObstacleRight.h"
#include "StateObstacleFront.h"
#include "StateFree.h"


void State_Free::Execute(Roboter* bot)
{
    if ( (bot->getCondition()) == FREE )
    {
        bot->MoveAhead();
    }


    if ( (bot->getCondition()) == OBSTACLEFRONT )
    {
        bot->ChangeState(new State_ObstacleFront());
    }

    if ( (bot->getCondition()) == OBSTACLELEFT )
    {
        bot->ChangeState(new State_ObstacleLeft());
    }

    if ( (bot->getCondition()) == OBSTACLERIGHT )
    {
        bot->ChangeState(new State_ObstacleRight());
    }
}


//StateObstacleFront.cpp

#include "StateObstacleLeft.h"
#include "StateObstacleRight.h"
#include "StateObstacleFront.h"
#include "StateFree.h"

void State_ObstacleFront::Execute(Roboter* bot)
{
    if ( (bot->getCondition()) == FREE )
    {
        bot->ChangeState(new State_Free());
    }

    if ( (bot->getCondition()) == OBSTACLEFRONT )
    {
        bot->MoveBack();
    }

    if ( (bot->getCondition()) == OBSTACLELEFT )
    {
        bot->ChangeState(new State_ObstacleLeft());
    }

    if ( (bot->getCondition()) == OBSTACLERIGHT )
    {
        bot->ChangeState(new State_ObstacleRight());
    }
}


//StateObstacleLeft.cpp

#include "StateObstacleLeft.h"
#include "StateObstacleRight.h"
#include "StateObstacleFront.h"
#include "StateFree.h"

void State_ObstacleLeft::Execute(Roboter* bot)
{
    if ( (bot->getCondition()) == FREE )
    {
        bot->ChangeState(new State_Free());
    }

    if ( (bot->getCondition()) == OBSTACLEFRONT )
    {
        bot->ChangeState(new State_ObstacleFront());
    }

    if ( (bot->getCondition()) == OBSTACLELEFT )
    {
        bot->MoveRight();
    }

    if ( (bot->getCondition()) == OBSTACLERIGHT )
    {
        bot->ChangeState(new State_ObstacleRight());
    }
}


//StateObstacleRight.cpp

#include "StateObstacleLeft.h"
#include "StateObstacleRight.h"
#include "StateObstacleFront.h"
#include "StateFree.h"


void State_ObstacleRight::Execute(Roboter* bot)
{
    if ( (bot->getCondition()) == FREE )
    {
        bot->ChangeState(new State_Free());
    }

    if ( (bot->getCondition()) == OBSTACLEFRONT )
    {
        bot->ChangeState(new State_ObstacleFront());
    }

    if ( (bot->getCondition()) == OBSTACLELEFT )
    {
        bot->ChangeState(new State_ObstacleLeft());
    }

    if ( (bot->getCondition()) == OBSTACLERIGHT )
    {
        bot->MoveLeft();
    }
}


//test.c

/********************************************

*                                           *
*  N I B O   -   A N T I K O L L I S I O N  *
*                                           *
********************************************/

// Stand: 10.08.2007
// Erhard Henkes
// Programmiersprache C++

// Geometrischer Sensorschwerpunkt
// Gewichtung der Sensoren
// state pattern design

// TODO: wird noch unter Bürostuhlbein eingeklemmt
//       schwingt ab und zu zwischen zwei Zuständen
//       Absolute Grenzen durch relative ersetzen

// Header 
#include "nibo.h" // Zusammenfassung der typischen Header für Nibo
#include "Roboter.h"
#include "utilities.h"
using namespace nibocc;

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

/****************************************************************/

int main()
{
    Init();

    Roboter Nibo;

    // Variablen
    uint16_t Vektor[5][2];                // Einheitsvektoren (*10) [0] ist x- und [1] ist y-Wert
    uint8_t  weightfactor[5];             // Gewichtungsfaktor
    uint16_t VektorMalSensor[5][2];       // Sensorwert * Einheitsvektor (*10)
    uint16_t VektorMalSensorSumme[2];     // Sensorschwerpunkt (x,y) für Auswertung
   
    // Einheitsvektor (* 10) gerundet
    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

    // Gewichtung der Sensoren in Abhängigkeit von der Richtung
    weightfactor[LINKS]        = 1;
    weightfactor[VORNE_LINKS]  = 2;
    weightfactor[VORNE]        = 3;
    weightfactor[VORNE_RECHTS] = 2;
    weightfactor[RECHTS]       = 1;

    // Vorbereitungen
    Leds::set_displaylight(1000);
    Leds::set_headlights(256);
    Floor::enable_ir();
    MotCo::set_pwm(512,512);
    MotCo::set_speed(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);  

    // Laufvariablen
    int i,j;

    while(true)
    {
        // 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::start_measure();
        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];

        /*
           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  / EVTL. AUSWEICHEN
      uint16_t GrenzeY2 =  5000; // Zustandsgrenze: EVTL. AUSWEICHEN / FREI
     
      // GrenzenX
      uint16_t GrenzeXlinks  = -2000; // Zustandsgrenze: LINKS  / GERADEAUS
      uint16_t GrenzeXrechts =  2000; // Zustandsgrenze: RECHTS / GERADEAUS

         
      /*  Curr.State  Condition                                                                     State Transition
      //  ----------------------------------------------------------------------------------------------------------------------
      //  (alle)      (Y>=GrenzeY1)                                                                 BLOCKIERT
      //  (alle)      ((Y<GrenzeY1) && (Y>= GrenzeY2) && (X<GrenzeXlinks))                          HINDERNISLINKS
      //  (alle)      ((Y<GrenzeY1) && (Y>= GrenzeY2) && (X>GrenzeXlinks))                          HINDERNISRECHTS
      //  (alle)      ((Y<GrenzeY1) && (Y>= GrenzeY2) && (X>=GrenzeXlinks) && (X<=GrenzeXrechts))   FREI
      //  (alle)      (Y<GrenzeY2)                                                                  FREI          
      */
     
      // Condition ermitteln und entsprechende Klasse State... als m_pCurrentState setzen
      // y-Wert
      if( VektorMalSensorSumme[1] >=GrenzeY1)
      {
          Nibo.setCondition(OBSTACLEFRONT);
      }

      if( (VektorMalSensorSumme[1] < GrenzeY1) && (VektorMalSensorSumme[1] >=GrenzeY2) )
      {
             // x-Wert
            if( VektorMalSensorSumme[0] < GrenzeXlinks  ) 
            {
                Nibo.setCondition(OBSTACLELEFT);
            }
       
            if( VektorMalSensorSumme[0] > GrenzeXrechts ) 
            {
                Nibo.setCondition(OBSTACLERIGHT);
            }
       
            if( (VektorMalSensorSumme[0] >= GrenzeXlinks) && (VektorMalSensorSumme[0] <= GrenzeXrechts) )
            {
                Nibo.setCondition(FREE);
            }
      }
    
      if ( (VektorMalSensorSumme[1] < GrenzeY2) || (irco_distance[2] < 150) )
      {
         Nibo.setCondition(FREE);
      }
             
      // Auf Zustand reagieren
      Nibo.Update();

    } // end of while

   return 0;
}





Nibo2

Inzwischen wird der Nachfolger Nibo2 angeboten:
http://www.nibo-roboter.de/wiki/NIBO_2
http://www.nibo-roboter.de/wiki/Datei:Nibo2_Detailansicht.jpg