/* Fachpraktikum mobile Roboter  Aufgabe 2 */

/* Gerrit Heitsch */

/* Programm wurde auf Roboter "FipsiROB" (gelb auf rot) */
/* erfolgreich getestet */

/* Das Programm faehrt den Roboter an der Wand entlang solange */
/* bis der von den Photowiderstaenden gelesene Wert ueber einem */
/* eingestellen Schwellwert liegt. In diesem Falle wird das Licht */
/* als erkannt ausgegeben, mit einer Tonfolge gemeldet und */
/* angehalten */

/* Die Fahrt an der Wand entlang wird durch das Fahren einer */
/* leichten Linkskurve erreicht und sobald die IR-Sensoren ein */
/* Hindernis erkennen wird nach rechts korrigiert bis nichts */
/* erkannt wird. Da es leider nur Sensoren nach vorne gibt ist */
/* diese Aufgabe nicht einfach zu loesen, vor allem bei Kurven */
/* um in die Bahn hereinragende Hindernisse */

/* Ein Versuch in die Bahn hereinragende Hindernisse zu umfahren */
/* wurde eingebaut. Sobald die Bumper ein Hinderniss melden wird */
/* fuer 1.5 Sekunden rueckwaerts gefahren (in dieser Zeit sind */
/* keine anderen Bewegungen moeglich) und dann nochmal probiert */


/* Konstanten */
float speed = 40.0;
int sound_thresh = 18;
int Bumper_ticks = 5;
int Ir_ticks = 2;

/* IDs fuer Bewegungsarten */
int STOP = 0;
int FORWARD = 1;
int LEFT_ARC = 5;
int RIGHT_ARC = 6;

/* Logische Werte */
int NO_CHANGE = -1;
int TRUE = 1;
int FALSE = 0;

/* Analogeing„nge */
int MICRO = 2;

/* Globale Flags */
int sound_permission = TRUE;
int sound_heard = FALSE;
int sound_work = FALSE;
int drive_status = STOP;
int done = 0;   /* wird 1 wenn Programmende erreicht */
int bump = 0;   /* wird 1 wenn Bumper erkannt wird */

/* Globale Variablen */
int stop_count = 0;
float def_angle = 10.5; /* Wert unterscheidet sich von Roboter */
                       /* zu Roboter und laesst ihn bei korrekter */
                       /* Einstellung leicht nach links fahren */
                       /* Roboter mit rotem Griff: 10.5  */
                       /* Roboter "gelb auf rot" : 1.5 */
                       /* Ausserdem muss noch die Empfindlichkeit */
                       /* der IR-Sensoren korrekt eingestellt werden */
                       /* sonst stimmt der Abstand von der Wand nicht */

float angle = def_angle; /* Arbeitswinkel auf Defaultwinkel einstellen */

void alert_tune()  /* Tonfolge fuer Programmstart und Licht erkannt */
{ tone(1046.5, 0.200);
  tone(1396.9, 0.200);
  tone(1046.5, 0.200);
  tone(698.5, 0.200);
}

int get_micro ()
{
 int count = 0;
 int Anzahl = 64;
 int gesamt = 0;
 int wert;

 for (count = 0; count < Anzahl; count++)
     {
      wert = analog (MICRO);
      if (wert < 128) wert = 128 - wert;
      else wert = wert - 128;
      gesamt = gesamt + wert;
     }
 wert = (int)(gesamt / Anzahl);

 return (wert);
}

void bumper_test ()   /* Abfrage der Bumper, der Roboter haelt an */
                      /* sobald Bumper erkannt werden */
{int wert;
 while (TRUE)
 { wert = bumper ();
   if ((wert & 0b010) || (wert & 0b001))
      {
       stop_count++;
       if (stop_count > Bumper_ticks)
          {
           stop_count = 0;
           bump = 1;
          }
      }
   else stop_count = 0;
 }
}

void check_sound()   /* stellt sicher, dass der Roboter erst nach Eingabe */
                     /* eines Tones (Blasen in das Mikrofon) losfaehrt */
                     /* Ebenso kann man den Roboter durch einen Ton */
                     /* jederzeit anhalten und wieder losfahren lassen */

{ while(TRUE)
  { if(sound_permission)
      {
       if(get_micro () > sound_thresh)
         {
          sound_permission = FALSE;
          sound_heard = TRUE;
          sound_work = FALSE;
         }
      }
    else
      {
       if ((sound_work) && (!(get_micro () > sound_thresh)))
          sound_permission = TRUE;
      }
  }
}

void sound_react()
{ while(TRUE)
  { if(sound_heard)
    { sound_heard = FALSE;
      sound_work = TRUE;
      if ((drive_status == STOP) && (done == 0)) drive_status = FORWARD;

      else drive_status = STOP;
    }
/* Es kann nur losgefahren werden wenn "done" noch nicht erreicht ist */
/* also kein Licht bisher erkannt wurde */

  }
}

int get_ir ()
{
 int wert;
 int gesamt_L = 0, gesamt_R = 0, gesamt_B = 0;
 int count;

 for (count = 0; count < Ir_ticks; count++)
     {
      wert = ir_detect ();
      if (wert == 0b01) gesamt_L++;           /* Left */
      else if (wert == 0b10) gesamt_R++;      /* Right */
           else if (wert == 0b11) gesamt_B++; /* Both */
     }
 if ((gesamt_R == Ir_ticks) | (gesamt_L == Ir_ticks) | (gesamt_B == Ir_ticks))

/* Falls einer oder beide der IR-Sensoren etwas erkennen */
/* wird der Roboter solange auf eine Drehung nach rechts */
/* geschaltet bis die Sensoren wieder freie Fahrt signalisieren */

    {
     if (bump == 0)  /* nur moeglich wenn keine Bumper erkannt */
     {
     angle = -40.0;
     speed = 10.0;
     return (NO_CHANGE);
     }
    }
 else

/* Nichts erkannt, also normales fahren */

    {
     if (bump == 0)  /* nur moeglich falls keine Bumper erkannt */
     {
     angle = def_angle;
     speed = 40.0;
     return (NO_CHANGE);
     }
    }
return (NO_CHANGE); /* Sicherheit */
}

void ir_test ()
{
 int wert;

 while (TRUE)
 {int wert;

  if (drive_status != STOP)
     {
      wert = get_ir ();
      if (wert != NO_CHANGE)
      drive_status = wert;
     }
 }
}

void motor_control()
{ while(TRUE)
  {
   if (drive_status == FORWARD) {drive(speed,angle);}
   else drive (0.0,0.0);
  }
}

void light_check()
{
 int licht1;
 int licht2;
 int licht_max = 0;

 while (TRUE)
 {

/* Auslesen der Photozellen und ermitteln des "helleren" Wertes */
/* der beiden. Die Umrechnung auf groesser = heller wurde nur zum */
/* besseren Verstaendnis des Programmierers gemacht */

   licht1 = 255 - analog(photo_right);
   licht2 = 255 - analog(photo_left);
   if (licht1 > licht_max) licht_max = licht1;
   if (licht2 > licht_max) licht_max = licht2;

   printf("l: %d  r: %d  max: %d\n",licht2,licht1,licht_max);

/* Ausgabe der Werte auf Display zur Ueberwachung */

   if (licht_max > 250)            /* Testen ob Schwellwert erreicht */
    { if (done == 0) alert_tune();
      drive_status = STOP;
      done = 1;
    }
/* Licht wurde erkannt, Ende des Programmes und endgueltiges Abschalten */
/* der Bewegungen und Verhindern von neuen Bewegungen durch done = 1 */

  }
}

void bumper_react()  /* Prozess fuer Bumperbehandlung */
{
float temp1;         /* Speichern die aktuellen Werte fuer */
float temp2;         /* Speed und Angle zwischen */

while(TRUE)
 {
 if (bump == 1)      /* Bumper erkannt? Wenn ja, dann: */
  {
  temp1 = angle;     /* Werte retten */
  temp2 = speed;
  angle = 0.0;       /* Werte fuer rueckwaerts setzen */
  speed = -40.0;
  sleep (1.0);       /* 1.5 Sekunden warten */
  angle = temp1;     /* alte Werte restaurieren */
  speed = temp2;
  bump = 0;          /* normale Bewegung wieder freigeben */
  }
 }

}


void main()
{ sleep(1.0);
  alert_tune();
  start_process(check_sound());
  start_process(sound_react());
  start_process(bumper_test());
  start_process(ir_test());
  start_process(light_check());
  start_process(motor_control());
  start_process(bumper_react());
}