Weiterentwicklung des "program8" aus dem NiboBee-T

Tach zusammen,

ich poste mein Programm, welches ich in den letzten Wochen für die NiboBee entwickelt habe.

Qualitätsurteil der freiwilligen Selbstkontrolle: mittelgut

Eine Beschreibung zum Programm folgt hier:

Voraussetzungen:
Die eingebundene NiboBeeLib von Nicai Systems, siehe Tutorial von Nicai-Systems.

Anwendung:
Dieses Programm ist eine Erweiterung von program8 aus dem Nibobee-Tutorial.

program8 bietet folgende Funktionen:
* Rückwärts-Kurvenfahrt bei Sensorkontakt, je nach Sensor entweder Links- oder Rechtskurve

Erweiterungen:
* Dunkler Untergrund wird vermieden
* Kurze Abschnitte von dunklem Untergrund werden nicht vermieden (z.B. Isolierbandstriefen)
* P-Regler

ToDo:
* Rückwärtsfahrten über eine bestimmte Strecke statt während einer bestimmten Zeit
* Zeitmessung per Timer-Interrupts statt über das Zählen der Schleifendurchläufe
* Die Programmstruktur ist nicht so prima, es muss mehr in Methoden rein

Bedienungsanleitung:
Stellen Sie Ihre NiboBee auf einen hellen Bodenbelag und drücken Sie einen Fühler, um die NiboBee zu starten. Während der ersten Sekunde der ersten Geradeausfahrt ermittelt das Programm die durchschnittliche Bodenreflexion. Sollte der Boden während der Fahrt dunkler werden als dieser anfängliche Vergleichswert, fährt die NiboBee für einige Zeit rückwärts.

Weitere Erklärungen entnehmen Sie bitte den Programmkommentaren.

Code:

---

/*
 * Mit diesem Programm kann der NIBObee umherfahren. Wenn mit den Fühlern
 * Hindernisse detektiert werden, versucht der NIBObee diesen auszuweichen.
 */

//10 ********************************** Deklarationen **************************************************

#include <nibobee/iodefs.h>
#include <nibobee/motpwm.h>
#include <nibobee/delay.h>
#include <nibobee/sens.h>
#include <nibobee/line.h>
#include <nibobee/led.h>
#include <stdio.h>
#include <nibobee/odometry.h>

//Prozedurdeklarationen
uint8_t perform_check(uint8_t mode);
uint8_t do_stop();
uint8_t do_drive();
uint8_t do_back();
uint8_t do_far_back();
uint8_t do_steer_r();
uint8_t do_steer_l();
uint8_t do_avoid_r();
uint8_t do_avoid_l();
uint8_t do_wait_for_light();
void motReg_set(uint8_t isRight);
void getInitLineValues();
uint8_t darkerFloor(uint8_t isRight);
uint8_t darkerFloorForALongTime(uint8_t isRight);

enum
{
        MODE_STOP,
        MODE_DRIVE,
        MODE_BACK,
  MODE_FAR_BACK,
        MODE_STEER_R,
        MODE_STEER_L,
        MODE_AVOID_R,
        MODE_AVOID_L,
        MODE_WAIT_FOR_LIGHT
};

struct _linesensor
{
        uint32_t initValue_L;
        uint32_t initValue_C;
        uint32_t initValue_R;
        //Anzahl der aufeinanderfolgenden Loops mit dunklem Untergrund
        uint32_t darkerFloorLoopCount_L;
        uint32_t darkerFloorLoopCount_R;
};

struct _linesensor linesensor = {0, 0, 0, 0, 0};

struct _odo
{
        uint32_t loopCountAtLastImpulse;
        uint32_t loopCountSinceLastImpulse;
        int32_t impulseCount;
        float i_ipl; //Ist-Geschwindigkeit in impulses per loop
        int8_t i_direction; //Soll-Richtung: -1= rückwärts, 1=vorwärts
        float s_ipl; //Soll-Geschwindigkeit in impulses per loop
        uint8_t s_ipl_reached;
        float e_ipl; //Regelabweichung
        float y_ipl; //Stellgröße
        float motpwm;        //PWM-Wert für Motorsteuerung als float
        int8_t newSpeedMeasuring;
        uint32_t zeroSpeedLoopCount; //Anzahl aufeinanderfolgender Loops mit Geschwindigkeit 0;
};

struct _odo odo_L = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
struct _odo odo_R = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};

uint16_t counter_ms;
uint32_t loops=0;

//Anzahl Loops für die Messung der durchschnittlichen Bodenreflexion
uint32_t lineInitCounter = 1000;

//Anzahl Loops, die bei zeroSpeed vergehen müssen, bevor die Fahrtrichtung gewechselt werden soll:
static uint32_t wheelBlockedLoopLimit = 500;

// Anzahl der Loops, die ohne neuen Impuls vergehen müssen, um die Geschwindigkeit als 0 ipl zu deuten:
static uint32_t zeroSpeedLoopLimit =300;

//Anzahl der Loops mit dunklem Untergrund, bis darauf reagiert wird (dadurch werden kurze Dunkelzeiten
static uint32_t darkerFloorLoopLimit=300;

//Standardgeschwindigkeit
static float straight_s_ipl=0.09;

//Kurvengeschwindigkeit langsames Rad
static float crv_min_s_ipl= 0.03;

//Kurvengeschwindigkeit schnelles Rad
static float crv_max_s_ipl= 0.09;

//Faktor für Proportionalanteil
static float pfactor=500;

//Dauer der Rückwartsfahrt beim Vermeiden (avoid) in ms
static uint16_t avoidtime=1000;

uint8_t previous_mode = MODE_STOP;
uint8_t mode = MODE_STOP;

//20 ********************************** Hauptroutine **************************************************

int main()
{
 
        //Initialisierung der Motoren
        motpwm_init();
        //Initialisierung der Tastsensoren
        sens_init();
        //Initialisierung der Bodensensoren
        line_init();
        //Initialisierung der LEDs
        led_init();
        //Initialisierung der Odometrie
        odometry_init();
  odometry_reset();
        enable_interrupts();

        //Erst starten, wenn ein Fühler gedrückt wurde
        while(sens_getLeft()==0 && sens_getRight()==0)
        {
            delay(1);
        }

//30 ********************************** Hauptschleife *****************************************************
        for(;;)
        {

                enable_interrupts();

                //Verzögerung von 1ms ergibt ca. 1000 Schleifendurchläufe pro Sekunde
            delay(1);

                //Durchschnittswerte der Liniensensoren während der ersten Sekunde der ersten Geradeausfahrt ermitteln
                getInitLineValues();

                if (darkerFloor(0))
                {
                        linesensor.darkerFloorLoopCount_L++;
                }
                else
                {
                        linesensor.darkerFloorLoopCount_L=0;
                }

                if (darkerFloor(1))
                {
                        linesensor.darkerFloorLoopCount_R++;
                }
                else
                {
                        linesensor.darkerFloorLoopCount_R=0;
                }


                //Letzten Modus vor dem eventuellen Umschalten auf einen neuen Modus merken
                previous_mode=mode; //storing previous mode       

                //Prüfen, ob eventuell beide Fühler nach hinten oder beide Fühler nach vorn gebogen sind:
            mode = perform_check(mode);
   
                //Aufrufen des jeweiligen Fahrmodus anhand von mode:
                  switch (mode)
                        {
              case MODE_STOP:                    mode = do_stop(); break;
              case MODE_DRIVE:                  mode = do_drive(); break;
              case MODE_BACK:                    mode = do_back(); break;
              case MODE_FAR_BACK:          mode = do_far_back(); break;
              case MODE_STEER_R:                mode = do_steer_r(); break;
              case MODE_STEER_L:                mode = do_steer_l(); break;
              case MODE_AVOID_R:                mode = do_avoid_r(); break;
              case MODE_AVOID_L:                mode = do_avoid_l(); break;
              case MODE_WAIT_FOR_LIGHT: mode = do_wait_for_light(); break;
            }

                //Steuern der Motorgeschwindigkeiten anhand des neu zurückgegebenen mode:
          switch (mode)
                {
              case MODE_STOP:                    odo_L.s_ipl =    0;                                                odo_R.s_ipl =    0; break;
              case MODE_DRIVE:                  odo_L.s_ipl =  straight_s_ipl;        odo_R.s_ipl =  straight_s_ipl; break;
              case MODE_BACK:                    odo_L.s_ipl = -straight_s_ipl;        odo_R.s_ipl = -straight_s_ipl; break;
              case MODE_FAR_BACK:                odo_L.s_ipl = -straight_s_ipl;        odo_R.s_ipl = -straight_s_ipl; break;
              case MODE_STEER_R:                odo_L.s_ipl =  crv_max_s_ipl;                odo_R.s_ipl =  crv_min_s_ipl; break;
              case MODE_STEER_L:                odo_L.s_ipl =  crv_min_s_ipl;        odo_R.s_ipl =  crv_max_s_ipl; break;
              case MODE_AVOID_R:                odo_L.s_ipl = -crv_min_s_ipl;        odo_R.s_ipl = -crv_max_s_ipl; break;
              case MODE_AVOID_L:                odo_L.s_ipl = -crv_max_s_ipl;        odo_R.s_ipl = -crv_min_s_ipl; break;
                                case MODE_WAIT_FOR_LIGHT:        odo_L.s_ipl =    0;                                        odo_R.s_ipl =    0; break;
            }


        //LINKS

                odo_L.newSpeedMeasuring=0;
                //Bisherige Loops seit dem letzten Impuls
                odo_L.loopCountSinceLastImpulse = loops - odo_L.loopCountAtLastImpulse;


                //Ist-Geschwindigkeit links ermitteln
                if (odometry_getLeft(0) != 0)
                {
                //Neuer Impuls
                        odo_L.newSpeedMeasuring=1;
                       
                        //Wert speichern und Reset des Zählers
                        odo_L.impulseCount= odometry_getLeft(1);

                        //Ist-Geschwindigkeit =  1/(loops bei diesem Impuls - loops beim letzten Impuls)
                        odo_L.i_ipl=(float)1/(float)(loops - odo_L.loopCountAtLastImpulse);
                       
                        //Richtung bestimmen und Vorzeichen für Regelabweichung setzen
                        if (odo_L.impulseCount > 0)
                        {
                                odo_L.i_direction=1;
                        }
                        else // <
                        {
                                odo_L.i_direction=-1;
                                odo_L.i_ipl *= (float)-1;
                        }

                        odo_L.loopCountAtLastImpulse=loops;                       
                        odo_L.zeroSpeedLoopCount=0;

                }
                else
                //Kein neuer Impuls
                {
                        if (odo_L.loopCountSinceLastImpulse > zeroSpeedLoopLimit)
                        {
                                odo_L.newSpeedMeasuring=1;
                                odo_L.i_direction=0;
                                odo_L.i_ipl=0;
                                odo_L.zeroSpeedLoopCount++;
                        }

                }

                led_set(LED_L_RD,(odo_L.i_direction==0));
                led_set(LED_L_YE, darkerFloorForALongTime(0));

                //Sollgeschwindigkeit setzen, falls es ein neues Messergebnis gab:
                if (odo_L.newSpeedMeasuring == 1)
                {
                  motReg_set(0);
                }

        //RECHTS

                odo_R.newSpeedMeasuring=0;

                //Bisherige Loops seit dem letzten Impuls
                odo_R.loopCountSinceLastImpulse = loops - odo_R.loopCountAtLastImpulse;

                //Ist-Geschwindigkeit rechts ermitteln


                if (odometry_getRight(0) != 0)
                {
                //Neuer Impuls
                        odo_R.newSpeedMeasuring=1;
                        odo_R.i_ipl=(float)1/(float)(loops - odo_R.loopCountAtLastImpulse);       

                        //Wert speichern und Reset des Zählers
                        odo_R.impulseCount= odometry_getRight(1);

                        //Richtung bestimmen und Vorzeichen für Regelabweichung setzen
                        if (odo_R.impulseCount > 0)
                        {
                                //Ist-Geschwindigkeit =  (loops bei diesem Impuls - loops beim letzten Impuls)
                                odo_R.i_direction=1;
                        }
                        else // <
                        {
                                //Ist-Geschwindigkeit = (loops bei diesem Impuls - loops beim letzten Impuls)
                                odo_R.i_direction=-1;
                                odo_R.i_ipl *= (float)-1;
                        }

                        odo_R.loopCountAtLastImpulse=loops;                       
                        odo_R.zeroSpeedLoopCount=0;

                }
                else
                //Kein neuer Impuls       
                {
                        if (odo_R.loopCountSinceLastImpulse > zeroSpeedLoopLimit)
                        {
                                odo_R.newSpeedMeasuring=1;
                                odo_R.i_direction=0;
                                odo_R.i_ipl=0;
                                odo_R.zeroSpeedLoopCount++;
                        }
                }

                led_set(LED_R_RD,(odo_R.i_direction==0));
                led_set(LED_R_YE, darkerFloorForALongTime(1));

                //Sollgeschwindigkeit setzen, falls es ein neues Messergebnis gab (entweder einen Impuls oder nach Ablauf von zeroSpeedLoopLimit):
                if (odo_R.newSpeedMeasuring == 1)
                {
                  motReg_set(1);
                }

                //********** Vorbereitung für nächsten Schleifendurchlauf ******************************************

                //Schleifenzähler um 1 erhöhen
                loops++;

  }
  return 0;
}

//50 ********************************** weitere Routinen **************************************************

//P-Regler
void motReg_set(uint8_t isRight)
{

        if (isRight)
        {

                //Regelabweichung ermitteln
                odo_R.e_ipl = odo_R.i_ipl - odo_R.s_ipl;

                //Regelgröße ermitteln
                odo_R.motpwm -= (odo_R.e_ipl * pfactor);


                if (odo_R.motpwm <-1022)
                {
                        odo_R.motpwm=-1022;
                }

               
                if (odo_R.motpwm >1022)
                {
                        odo_R.motpwm=1022;
                }

                //PWM setzen. Werte über 1022 oder unter -1022 werden automatisch begrenzt
                motpwm_setRight((int)odo_R.motpwm);
        }
        else
        {
                //Regelabweichung ermitteln
                odo_L.e_ipl = odo_L.i_ipl - odo_L.s_ipl;

                //Regelgröße ermitteln
                odo_L.motpwm -= (odo_L.e_ipl * pfactor);

                if (odo_L.motpwm <-1022)
                {
                        odo_L.motpwm=-1022;
                }
                               
                if (odo_L.motpwm >1022)
                {
                        odo_L.motpwm=1022;
                }

                //PWM setzen. Werte über 1022 oder unter -1022 werden automatisch begrenzt
                motpwm_setLeft((int)odo_L.motpwm);
        }
}

//Diese Prozedur misst während der ersten 1000 ms die Reflexion der Liniensensoren.
//Danach setzt sie die Variablen lineLInitValue und lineRInitValue, welche einen
//Durchschnittswert aus den Messungen minus 10 Prozent ergeben. Mit diesen Vergleichswerten
//kann im Anschluss an die Messung jederzeit bestimmt werden, ob der gegenwärtige Reflexionsgrad
//geringer ist als während der Messphase nach dem Einschalten des Roboters.
//Mit der Function darkerFloor kann dann geprüft werden, ob der Untergrund dunkler als zu Beginn ist
void getInitLineValues()
{
        if (lineInitCounter >1)
        {
          linesensor.initValue_L+=line_get(LINE_L);
          linesensor.initValue_R+=line_get(LINE_R);
                lineInitCounter--;
        }

        if (lineInitCounter==1)
        {
                //berechnen
                lineInitCounter=0;
                linesensor.initValue_L /= 1200; //1000 + 200 :-)
                linesensor.initValue_R /= 1200;
        }
}

uint8_t darkerFloor(uint8_t isRight)
{
        if (isRight)
        {
                return        ((lineInitCounter==0) && (line_get(LINE_R)< linesensor.initValue_R));
        }
        else
        {
                return        ((lineInitCounter==0) && (line_get(LINE_L)< linesensor.initValue_L));
        }
}

uint8_t darkerFloorForALongTime(uint8_t isRight)
{
        if (isRight)
        {
                return (lineInitCounter==0) && (linesensor.darkerFloorLoopCount_R > darkerFloorLoopLimit);
        }
        else
        {
                return (lineInitCounter==0) && (linesensor.darkerFloorLoopCount_L > darkerFloorLoopLimit);
        }

}

//40********************************** Modus-Routinen **************************************************


//Modus-Routine für "beide Fühler vorne" oder "beide Fühler hinten"
uint8_t perform_check(uint8_t mode)
{

        if ((sens_getLeft()==1) && (sens_getRight()==1))
        {
                mode = MODE_STOP;
    }

        if ((sens_getLeft()==-1) && (sens_getRight()==-1))
        {
                mode = MODE_BACK;
        }

        if (darkerFloorForALongTime(0) && darkerFloorForALongTime(1))
        {
                mode = MODE_FAR_BACK;
        }

  return mode;
}

//Modus-Routine für "Stop"
uint8_t do_stop()
{
  if ((sens_getLeft()==0) && (sens_getRight()==0))
  {
    return MODE_DRIVE;
  }
  return MODE_STOP;
}

//Modus-Routine für "Geradaus zurück"
uint8_t do_back()
{

  //Mindestens einer der Motoren steht beim Rückwärtsfahren still -> wieder vorwärts
        if ((odo_L.zeroSpeedLoopCount > wheelBlockedLoopLimit) || (odo_R.zeroSpeedLoopCount > wheelBlockedLoopLimit))
  {
                //Damit kein ständiger Wechsel MODE_BACK -> MODE_DRIVE bei Geschwindigkeit 0 entsteht,
                //müssen die zeroSpeedLoop-Zähler nun zurückgestellt werden:
                odo_L.zeroSpeedLoopCount=0;
                odo_R.zeroSpeedLoopCount=0;
    return MODE_DRIVE;
  }

  if (sens_getLeft()==0)
  {
    return MODE_AVOID_L;
  }
  if (sens_getRight()==0)
  {
    return MODE_AVOID_R;
  }
  return MODE_BACK;
}

uint8_t do_far_back()
{
  if (counter_ms==0)
  {
    counter_ms=avoidtime*2;
  }
  else
  {
    counter_ms--;
  }
  if (counter_ms > 0)
  {
    return MODE_FAR_BACK;
  }
  else
        {
                return MODE_DRIVE;
        }
}


//Modus-Routine für "Geradeaus vorwärts"
uint8_t do_drive()
{
  getInitLineValues();

        //Mindestens einer der Motoren steht beim Vorwärtsfahren still -> rückwärts
        if ((odo_L.zeroSpeedLoopCount > wheelBlockedLoopLimit) || (odo_R.zeroSpeedLoopCount > wheelBlockedLoopLimit))
        {
                //Damit kein ständiger Wechsel MODE_BACK -> MODE_DRIVE bei Geschwindigkeit 0 entsteht,
                //müssen die zeroSpeedLoop-Zähler nun zurückgestellt werden:
                odo_L.zeroSpeedLoopCount=0;
                odo_R.zeroSpeedLoopCount=0;
                return MODE_BACK;
        }
 
        if (sens_getRight()==1)
        {
                return MODE_STEER_L;
        }
        if (sens_getLeft()==1)
        {
                return MODE_STEER_R;
        }

        if ((sens_getRight()==-1) || darkerFloorForALongTime(0))
        {
                return MODE_AVOID_L;
        }

        if ((sens_getLeft()==-1) || darkerFloorForALongTime(1))
        {
                return MODE_AVOID_R;
        }

        return MODE_DRIVE;
}

uint8_t do_steer_r()
{
  if (sens_getLeft()==0)
  {
    return MODE_DRIVE;
  }
  return MODE_STEER_R;
}

uint8_t do_steer_l()
{
  if (sens_getRight()==0)
  {
    return MODE_DRIVE;
  }
  return MODE_STEER_L;
}

uint8_t do_avoid_r()
{
  if (counter_ms==0)
  {
    counter_ms=avoidtime;
  }
  else
  {
    counter_ms--;
  }
  if (counter_ms > 0)
  {
    return MODE_AVOID_R;
  }
  else
  {
                if (darkerFloorForALongTime(0) && darkerFloorForALongTime(1))
                {
            return MODE_WAIT_FOR_LIGHT;
                }
                else
                {
                        return MODE_DRIVE;
                }
  }
}

uint8_t do_avoid_l()
{
  if (counter_ms==0)
  {
    counter_ms=avoidtime;
  }
  else
  {
    counter_ms--;
  }
  if (counter_ms > 0)
  {
    return MODE_AVOID_L;
  }
  else
  {
                if (darkerFloorForALongTime(0) && darkerFloorForALongTime(1))
                {
            return MODE_WAIT_FOR_LIGHT;
                }
                else
                {
                        return MODE_DRIVE;
                }
  }
}

// Diese Prozedur wartet solange, wie der Reflexionsgrad der Liniensensoren unterhalb den Werten aus
// der Vergleichsmessung beim Starten des Roboters liegt.
uint8_t do_wait_for_light()
{
        if (darkerFloorForALongTime(0) && darkerFloorForALongTime(1))       
        {
                return MODE_WAIT_FOR_LIGHT;
        }
        else
        {
                return MODE_DRIVE;
        }
}

---