Turm

Da wir mit nur einem Turmsensor arbeiten, muss das Maximum des Leuchtfeuers in mehreren Schritten gesucht werden. Der Roboter dreht sich so lange bis er die Hälfte des bisher gefunden Maximalwertes (kann auch Anfangswert sein) unterschritten hat. Danach dreht er nach dem gleichen Prinzip in die entgegengesetzte Richtung, um das tatsächliche Maximum zu finden. Im dritten Schritt dreht der Roboter (möglichst genau) auf das gefunde Maximum.

Dreht der Turm zu lange, so wird die Suche abgebrochen. Der Roboter steht danach in einer undefinierten Richtung (Zufallsanteil im Timeout).

RIGHT_TURM ist ein Makro, das den (Helligkeits-) Wert des Turmsensors zurückliefert. Ausgrund von stark schwankenden Werten werden sehr viele Einzelwerte für die Durchschnittsbildung verwendet und dieser dann nocheinmal durch 4 geteilt (Vernachlässigung von Stellen).

#define TURM_THRESHOLD 4   /* Turm-Sensor-Wert durch 4 unterhalb dem nix laeuft */
#define TURM_AVRG_NUM 128  /* Ueber wieviele Werte soll ein Durchschnitt */
                           /* gebildet werden */
#define TURM_AVRG_SHIFT 7  /* Passender Divisor als Shift-Right-Wert */
#define TURM_FW 25
#define TURM_TIMEOUT 4000u /* Timeout falls Turm ausser Reichweite */


/*** Dreht den Roboter in Richtung des Leuchtturms ***/
void finde_turm()
{
unsigned int zeit, to_pm;
int neu, min, max, maxalt;
int i;

  /* timeout finden +-zufall, damit bei nicht sichtbarem Turm */
  /* ein wenig gestreuter gesucht wird */
  to_pm = TURM_TIMEOUT + (((unsigned int)TCNT) % 2000u);

  motorv(LEFT_MOTOR, -TURM_FW);
  motorv(RIGHT_MOTOR, TURM_FW);

  neu = 0;
  timertjp = 0u;
  for(i=0; i<TURM_AVRG_NUM; i++) neu += RIGHT_TURM;
  neu >>= TURM_AVRG_SHIFT + 2;
  max = neu;
  do {
    if(neu > max) max = neu;
    neu = 0;
    for(i=0; i<TURM_AVRG_NUM; i++) neu += RIGHT_TURM;
    neu >>= TURM_AVRG_SHIFT + 2;
    zeit = timertjp; /* Timeout, falls der turm zu weit weg ist */
  } while(((neu > (max-(max>>1))) || (neu < (TURM_THRESHOLD)))
           && (zeit<to_pm));
  maxalt = max;

  /* Turm gefunden oder Timeout? */
  if(zeit<to_pm) {
    /* annersrum */
    stop();
    wait(500);
    motorv(LEFT_MOTOR, TURM_FW);
    motorv(RIGHT_MOTOR, -TURM_FW);

    neu = 0;
    timertjp = 0u;
    for(i=0; i<TURM_AVRG_NUM; i++) neu += RIGHT_TURM;
    neu >>= TURM_AVRG_SHIFT + 2;
    max = neu;
    do {
      if(neu > max) { max = neu; maxalt = 0; } /* neues maximum */
      neu = 0;
      for(i=0; i<TURM_AVRG_NUM; i++) neu += RIGHT_TURM;
      neu >>= TURM_AVRG_SHIFT + 2;
      zeit = timertjp; /* Timeout, falls der turm zu weit weg ist */
    } while(((neu > (max-(max>>1))) || 
             (neu < (TURM_THRESHOLD)) || 
             (neu < (maxalt-1))) 
             && (zeit<to_pm));
    maxalt = max;

    if(zeit<to_pm) {
      /* genau zum Max zurueck */
      stop();
      wait(500);
      motorv(LEFT_MOTOR, -TURM_FW);
      motorv(RIGHT_MOTOR, TURM_FW);

      timertjp = 0u;
      do { 
        neu = 0;
        for(i=0; i<TURM_AVRG_NUM; i++) neu += RIGHT_TURM;
        neu >>= TURM_AVRG_SHIFT + 2;
        zeit = timertjp; /* Timeout, falls der turm zu weit weg ist */
      } while((neu < (maxalt-1)) && (zeit<to_pm));
    }
  }
 
  stop(); /* Nur zur besseren optischen Unterscheidung */
  wait(500);
}