#define rechtsan 270+10
#define linksan 469-10
#define mitte 376
#define rechts 1
#define links 0


int  richtung, altrichtung, winkel,motorc;


task main()
{
    SetSensorMode(SENSOR_1,SENSOR_MODE_RAW);
    SetSensorMode(SENSOR_2,SENSOR_MODE_RAW);
    SetSensorMode(SENSOR_3,SENSOR_MODE_RAW);
    
    start fuehler;
    start boden;
} //task main

sub rechtsdrehen()
{
    PlayTone(3000,10); // wegdrehen
    Float(OUT_A+OUT_B); 
    Wait(60);
    OnRev(OUT_B);
    OnFwd(OUT_A);
    Wait(180);

}
sub linksdrehen()
{
    PlayTone(6000,10); // wegdrehen

    Float(OUT_A+OUT_B); 
    Wait(60);
    OnRev(OUT_A);
    OnFwd(OUT_B);
    Wait(180);
}

sub rueckwaerts()
{
    PlayTone(1000,10); // wegdrehen

    Float(OUT_A+OUT_B); 
    Wait(60);
    OnFwd(OUT_A+OUT_B);
    Wait(120);
    OnRev(OUT_A);
    OnFwd(OUT_B);
    Wait(60);
}
void vorwaerts()
{
    OnRev(OUT_A+OUT_B);
}
void halt()
{
    Float(OUT_A+OUT_B); 
    Wait(120);  
}




task boden()
{
  int licht,altlicht;
  licht=0;
  altlicht=0;
  
    
  while(true) // endlos
    { 
   // Wait(10);
    
    licht=SENSOR_1;
    
   // if (licht > altlicht +80 && motorc ==1)
    if (licht > 500 && motorc ==1)
           {

        
       if (winkel<mitte-10 && richtung == rechts) // rechts ausen beim rechts tasten
             {
             linksdrehen();
              
             }
       else //3
            {
            if (winkel>mitte+10 && richtung == links) // links ausen beim links tasten
               {
               rechtsdrehen();
               }
            else //2
               {
               if (winkel<mitte-10 && richtung == links) // rechts ausen beim rechts tasten
                  {
                  rechtsdrehen();
                  }
               else //1
                  {
                   
                  if (winkel>mitte+10 && richtung == rechts) // links ausen beim links tasten
                     {
                     
                     linksdrehen();
                     }
                  else
                     {
                    
                     rueckwaerts();
                     }
                     
                  }//else 1
                  
               } // else 2
               
            } //else 3     
       }//if licht  
     
    altlicht=licht;
  //  vorwaerts();
    } // while       
} // task boden





task fuehler()
{
  richtung=0;
  altrichtung=0;
  motorc=0;
  SetPower(OUT_C,7);
  OnFwd(OUT_C);
//-------------------------  hier hin und her des Fuehlers --------------------------
while(true) // endlos
    {   
        winkel=SENSOR_2;   
        if(winkel < rechtsan)
          {
          richtung=links; 
          }
        if(winkel > linksan)
          {
          richtung=rechts;// zur anderenseite drehen den wenn an grenze gekommen
          }
   
        if(altrichtung != richtung)
          {
          motorc=0;
          Float(OUT_C); // damit etwas uslauf vorhanden ist und nicht abrupt umgepolt 
          Wait(30);
          altrichtung = richtung;

          if(richtung == links)
            {
            OnFwd(OUT_C);
            }
          else
            {
            OnRev(OUT_C);
            }
          motorc=1;
          }//if atrichtung
//----------------------- abfrage -------------------

        if( ExternalMotorRunning()>60)
          {
          motorc=0;
          Float(OUT_C); 
         // PlayTone(300,10);
          Wait(30);
    
          if (winkel<mitte-10 && richtung == rechts) // rechts ausen beim rechts tasten
             {
             linksdrehen();
             }
          else //3
            {
            if (winkel>mitte+10 && richtung == links) // links ausen beim links tasten
               {
               rechtsdrehen();
               }
            else //2
               {
               if (winkel<mitte-10 && richtung == links) // rechts ausen beim rechts tasten
                  {
                  rechtsdrehen();
                  }
               else //1
                  {
                  if (winkel>mitte+10 && richtung == rechts) // links ausen beim links tasten
                     {
                     linksdrehen();
                     }
                  else
                     {
                     rueckwaerts();
                     }
                  }//else 1
               } // else 2
            } //else 3     
          On(OUT_C);
          motorc=1;
        
          }// if extmotor
        //  vorwaerts();
    } // while enlos      
} // task