Labyrintti 1.2 väistöllä

#if 1
    // Labyrintti 1.0 valmis ilman väistöä
    void zmain(void)
{
   TickType_t start;                    // Aloita ajanlasku
   start = xTaskGetTickCount();
   IR_Start();
   Ultra_Start();

   struct sensors_ ref;                 // Sensorit päälle
   struct sensors_ dig;
   reflectance_start();
   reflectance_set_threshold(10000, 10000, 12000, 12000, 10000, 10000);

   motor_start();                       // Käynnistetään moottori
   motor_forward(0,0);

   int x = 0;                           // Tarvittavat muuttujat
   int y = -1;
   int este = 0;
   int nappi = 0;
   int nappi1 = 0;
   int kytkin = 0;
   int risteys = 0;
   int valiaika;
   int risteyslupa = 0;
   int suuntaluku = 400;
   int suunta;                              // 0 = ylös, 1 = oikea, 2 = alas, 3 = vasen,
   int d = Ultra_GetDistance();

    while(nappi1 == 0) {
    vTaskDelay(10);
    if(SW1_Read() == 0){                  // Liikkeellelähtö napista
            printf("nappi painettu, lahen menee \n");
            nappi = 1;
            nappi1 = 1;
            }
    }

    while(nappi == 1 && kytkin == 0){       // Odotus ensimmäisellä viivalla kunnes IR
    reflectance_digital(&dig);
    printf("ajossa\n");
    motor_forward(50, 10);
        if (dig.l3 == 1 && dig.l2 == 1 && dig.l1 == 1 && dig.r1 == 1 && dig.r2 == 1 && dig.r3 == 1) {
                motor_forward(0,0);

                printf("olen pauses\n");  //send_mqtt(ready)
                IR_flush();
                IR_wait();
                printf("nappia painettu\n");
                valiaika = xTaskGetTickCount();
                printf("%d\n", valiaika);          //stoptime print
                motor_forward(60, 300);
                kytkin = 1;
            }
    }
while(kytkin == 1 && nappi == 1) {
    suunta = suuntaluku %4;
    printf("eka suuntaluku: %d\n", suuntaluku);
    int arvo = 0;

    while(kytkin == 1 && nappi == 1 && risteys == 0) {  // Eteenpäin ajo, kunnes viiva.

        reflectance_digital(&dig);

        if ((dig.r1 == 1 && dig.r2 == 1 && dig.r3 == 1) ||(dig.l1 == 1 && dig.l2 == 1 && dig.l3 == 1)) {
                motor_forward(0, 10);   // Risteykseen ajo
                d = Ultra_GetDistance();

                while(dig.l2 == 1 && dig.r2 == 1){
                reflectance_digital(&dig);
                motor_forward(60, 10);
                }
                motor_forward(60, 50);
                if(suunta == 0){
                y++;
                }
                else if(suunta == 1){
                x++;
                }
                else if(suunta == 2){
                y--;
                }
                else if(suunta == 3){
                x--;
                }
                printf("X: %d, Y: %d\n", x, y);
                Beep(100, 100);BatteryLed_Write(1);
                risteys = 1;
                }

        else if(dig.l3 == 0 && dig.l2 == 0 && dig.l1 == 1 && dig.r1 == 1 && dig.r2 == 0 && dig.r3 == 0) {
                motor_forward(50, 10);
                }

        else if(dig.l2 == 1) {
                motor_turn(10, 60, 10); // Korjaus vasemmalle
                }

        else if(dig.r2 == 1) {
                motor_turn(60, 10, 10); // Korjaus oikealle
                }
    }

    while((y < 11 && kytkin == 1 && nappi == 1 && risteys == 1 && y<12 && d<16 && suunta == 0 && x != -3) || ( x == 0 && suunta == 1 && risteys == 1 )) {  //  Risteys vasemalle  || (x == 0 && suunta == 1 && kytkin == 1 && nappi == 1 && risteys == 1) || (d<11 && kytkin == 1 && nappi == 1 && risteys == 1)

        reflectance_digital(&dig);

        if(arvo == 0){
        motor_hardturnleft(60, 90, 300);
        printf("Vasemmalle  eka if");
        arvo = 1;
        }
        else if (dig.r1 == 0  || dig.l3 == 1 || dig.l2 == 1){
        motor_hardturnleft(60, 90, 1);
        printf("Vasemmalle  toka if");
        }
        else if (dig.r1 == 1){
        risteys = 0;
        suuntaluku--;
            printf("Vasemmalle valmis: %d, %d\n", suuntaluku, suunta);
        }
    }
        while((kytkin == 1 && nappi == 1 && risteys == 1 && x == -3 && suunta == 3 )|| (d<16 && suunta == 3 && risteys == 1 ) || (risteys == 1 && x == -3 && d<16 ) || (risteys ==1 && suunta ==0 && x<0 && y>=11)) {  //  Risteys oikealle

        reflectance_digital(&dig);
        if (arvo == 0){
        motor_hardturnright(90, 60, 300);
        arvo = 1;
        }
        else if (dig.l1 == 0  || dig.r3 == 1 || dig.r2 == 1){
        motor_hardturnright(90, 60, 1);
        }
        else if (dig.l1 == 1){
        risteys = 0;
        suuntaluku++;
        printf("Oikealle valmis: %d, %d\n", suuntaluku, suunta);
        }
    }
        risteys = 0;
        motor_forward(60, 100);
}
}
#endif