//wykona³ Grzegorz G¹siewski
//pobrano ze strony http://gege01.prv.pl
#define NATEZENIE 45

task main()
{
 SetSensor (SENSOR_1, SENSOR_LIGHT);  //wlasciwy
 SetSensor (SENSOR_2, SENSOR_LIGHT);  //pomocnik
 SetSensor (SENSOR_3, SENSOR_TOUCH);
 start sensor_dotyku;
 start jazda;
}

task jazda()
{
 SetPower (OUT_A + OUT_C,1);
 SetDirection(OUT_A + OUT_C, OUT_FWD);
 SetOutput(OUT_A + OUT_C, OUT_ON);

 while (true)
 {
    if (SENSOR_1>NATEZENIE)
     if (SENSOR_2>NATEZENIE)  //skret w lewo
     {
     SetDirection(OUT_A,OUT_REV);
     Wait(30);
     SetDirection(OUT_A + OUT_C, OUT_FWD);
     }
     else   //skret w prawo
     {
      SetDirection(OUT_C,OUT_REV);
      Wait(30);
      SetDirection(OUT_A + OUT_C, OUT_FWD);
     }

 }
}

task sensor_dotyku()
{
 while (true)
 {
    if(SENSOR_3 == 1)
   {
     Off (OUT_A+OUT_C);

     SetPower (OUT_B,3);   //podnies uchwyt
     SetDirection(OUT_B, OUT_REV);
     SetOutput(OUT_B, OUT_ON);
     Wait(400);

     Off (OUT_B);

     SetDirection(OUT_B, OUT_FWD);  //opusc uchwyt
     SetOutput(OUT_B, OUT_ON);
     Wait(220);
     Off (OUT_B);
     
     SetPower (OUT_A + OUT_C,1);
     SetDirection(OUT_A + OUT_C, OUT_FWD);
     SetOutput(OUT_A + OUT_C, OUT_ON);
   }
 }
}