/*********************************************************************/
/*   Fachpraktikum Mobile Roboter                                    */
/*                                                                   */
/*   Aufgabe 1                                                       */
/*   Matthias Grossmann, Birgit Graf                                 */
/*********************************************************************/


/* Konstanten */
float anfangsgeschwindigkeit=30.0;
float DELTA=4.0;

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

/* Globale Variablen */
int lclicks=0;
int rclicks=0;
float links=0.0;
float rechts=0.0;
int dmax=0;

float geschwindigkeit;


void alert_tune()
{
  tone(1046.5, 0.200);
  tone(1396.9, 0.200);
  tone(1046.5, 0.200);
  tone(698.5, 0.200);
}


int abs(int x)
{
 if (x<0)
  return 0-x;
 else
  return x;
}


void clicks()
{
 init_velocity();
 while(TRUE)
 {
  if (rechts>0.0)
   rclicks+=get_right_clicks();
  else
   rclicks-=get_right_clicks();
  if (links>0.0)
   lclicks+=get_left_clicks();
  else
   lclicks-=get_left_clicks();
  printf("l: %d  r: %d\n",lclicks,rclicks);
 }
}


void fahre_geradeaus()
{
 int d;

 while(TRUE)
 {
  d=rclicks-lclicks;
  if (abs(d)>abs(dmax)) dmax=d;
  links= geschwindigkeit+DELTA*(float) d;
  rechts=geschwindigkeit+DELTA*(float) d;
  drive(geschwindigkeit,DELTA*(float)-d);
  sleep(0.1);
 }}


void main()
{
  int pid_clicks;
  int pid_fahre;
  test_number=!test_number;
  if (test_number)
  {
   sleep(1.0);
   alert_tune();
   pid_clicks=start_process(clicks());
   pid_fahre=start_process(fahre_geradeaus());

   geschwindigkeit = anfangsgeschwindigkeit;
   while (rclicks < 50) {}
   geschwindigkeit = 0.0;
   tone(1046.5, 0.200);
   sleep(2.0);

   geschwindigkeit = -anfangsgeschwindigkeit;
   while (rclicks > 0) {}
   geschwindigkeit = 0.0;
   tone(1046.5, 0.200);  tone(698.5, 0.200);
   sleep(2.0);

   kill_process(pid_fahre);
   kill_process(pid_clicks);
   printf("max. Abw.: %d",dmax);
  }
  else
   printf("----HALT----\n");
}
ÿ