Labyrintti alpha 0.2.5 rikkinäinen

#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();
   int arvo;

    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;
    reflectance_digital(&dig);
    d = Ultra_GetDistance();

    while(((dig.r2 == 1 && dig.r3 == 1) || (dig.l2 == 1 && dig.l3 == 1)) && este == 0) {  // Eteenpäin ajo, kunnes viiva.

        reflectance_digital(&dig);

                motor_forward(0, 10);   // Risteykseen ajo

                while(dig.l2 == 1 || dig.r2 == 1){
                motor_forward(60, 10);
                reflectance_digital(&dig);
                }

                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);
                motor_forward(100, 50);
                risteys = 1;
                d = Ultra_GetDistance();

                    if(d > 20) {
                motor_forward(100, 20);
                motor_forward(0, 0);
                este = 0;
                }
                    else if(d <= 20) {
                motor_forward(0 , 0);
                este = 1;
                }
                reflectance_digital(&dig);
                Beep(100, 100);
            }



        while(dig.l2 == 0 && dig.l1 == 1 && dig.r1 == 1 && dig.r2 == 0 && risteys == 0){
                motor_forward(100, 10);
                reflectance_digital(&dig);
                }

        while(dig.l2 == 1 && risteys == 0 && dig.r2 == 0) {
                motor_turn(0, 100, 10); // Korjaus vasemmalle
                reflectance_digital(&dig);
                }

        while(dig.l2 == 0 && risteys == 0 && dig.r2 == 1) {
                motor_turn(100, 0, 10); // Korjaus oikealle
                reflectance_digital(&dig);
                }

        while(dig.l1 == 1 && dig.r1 == 0 && risteys == 0) {
                motor_turn(40, 100, 10); // Korjaus vasemmalle
                reflectance_digital(&dig);
                }

        while(dig.l1 == 0 && dig.r1 == 1 && risteys == 0) {
                motor_turn(100, 40, 10); // Korjaus oikealle
                reflectance_digital(&dig);
                }
        while(dig.l3 == 0 && dig.l2 == 0 && dig.l1 == 0 && dig.r1 == 0 && dig.r2 == 0 && dig.r3 == 0 && y > 12){
        motor_forward(0, 1000);
        reflectance_digital(&dig);
        }

                reflectance_digital(&dig);

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

            reflectance_digital(&dig);

            if(arvo == 0){
            motor_hardturnleft(60, 60, 300);
            printf("Vasemmalle  eka if");
            arvo = 1;
            }
            else if (dig.r1 == 0){
            motor_hardturnleft(60, 60, 1);
            printf("Vasemmalle  toka if");
            }
            else if (dig.r1 == 1){
            risteys = 0;
            suuntaluku--;
            printf("Vasemmalle valmis: %d, %d\n", suuntaluku, suunta);
            este = 0;
            d = Ultra_GetDistance();

            if(d > 20) {
                motor_forward(60, 20);
                este = 0;
                }
                else if(d <= 20) {
                este = 1;
            }
        }
            reflectance_digital(&dig);
    }
        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) ||
             (suunta == 3 && y > 10 && x == 0 && risteys == 1) ||
             (este == 1 && suunta == 0 && risteys == 1)) {  //  Risteys oikealle

                reflectance_digital(&dig);
                if (arvo == 0){
                motor_hardturnright(60, 60, 300);
                arvo = 1;
                }
                else if (dig.l1 == 0){
                motor_hardturnright(60, 60, 1);
                }
                else if (dig.l1 == 1){
                risteys = 0;
                suuntaluku++;
                printf("Oikealle valmis: %d, %d\n", suuntaluku, suunta);
                este = 0;
                d = Ultra_GetDistance();

                if(d > 20) {
                    este = 0;
                    }
                    else if(d <= 20) {
                    este = 1;
                    }
        }
            reflectance_digital(&dig);
    }
                        risteys = 0;
}
}

#endif