#define ForwardA -40
#define ForwardC -40
#define rotate90deg 430
#define obstacle 15


byte dist;

task main(){

     SetSensorLowspeed(IN_1);



     OnFwd(OUT_A,ForwardA);
     OnFwd(OUT_C,ForwardC+5);

      while(1)
      {
              dist = SensorUS(IN_1);
              ClearScreen();
              TextOut(0,0,"Distance");
              NumOut(0,10,dist);
              if(dist < obstacle)
              {
                    break;
              }
              else Wait(10);
      }

      Off(OUT_AC);
      RotateMotor(OUT_A,70,rotate90deg/2);
      RotateMotor(OUT_C,70,-rotate90deg/2);

      RotateMotor(OUT_B,70,-90);


      Wait(500);
      ResetTachoCount(OUT_AC);
      OnFwd(OUT_A,ForwardA);
      OnFwd(OUT_C,ForwardC+5);

      while(1)
      {
              dist = SensorUS(IN_1);
              ClearScreen();
              TextOut(0,0,"Distance");
              NumOut(0,10,dist);
              if(dist > obstacle + 10) //napotkal przeszkode
              {
                    break;
              }
              else Wait(10);
      }

      Wait(1000);
      long width1 =  MotorTachoCount (OUT_C);
      long width2 =  MotorTachoCount (OUT_A);
      long width =  (width1+width2)/2;
      ResetTachoCount(OUT_AC);
      Off(OUT_AC);

      RotateMotor(OUT_C,70,rotate90deg/2);
      RotateMotor(OUT_A,70,-rotate90deg/2);



      OnFwd(OUT_A,ForwardA);
      OnFwd(OUT_C,ForwardC+5);
      Wait(4000);

      while(1)
      {
              dist = SensorUS(IN_1);
              ClearScreen();
              TextOut(0,0,"Distance");
              NumOut(0,10,dist);
              if(dist > obstacle) // koniec przeszkody
              {
                    break;
              }
              else Wait(10);
      }



      Wait(1500);
      Off(OUT_AC);


      RotateMotor(OUT_C,70,rotate90deg/2);
      RotateMotor(OUT_A,70,-rotate90deg/2);

      Wait(1000);


      OnFwd(OUT_A,ForwardA);
      OnFwd(OUT_C,ForwardC+5);
      RotateMotor(OUT_B,70,90);

            while(1) {
      long wwidth1 =  MotorTachoCount (OUT_C);
      long wwidth2 =  MotorTachoCount (OUT_A);
      long wwidth =  (wwidth1+wwidth2)/2;
            if (wwidth>width)  break;
      }

      RotateMotor(OUT_A,70,rotate90deg/2);
      RotateMotor(OUT_C,70,-rotate90deg/2);








    /* while(1) {
      RotateMotor   (   OUT_B,70,90);
      Wait(1000);
      RotateMotor   (   OUT_B,70,-90);
      Wait(1000);
      RotateMotor   (   OUT_B,70,-90);
      Wait(1000);
      RotateMotor   (   OUT_B,70,90);
      Wait(1000);
                    }   */
}