robert pelottaa

#if 1
    //Labyrintti 1.0 10.12.2019 Robert

void zmain(void)
{
   TickType_t start;                    // Aloita ajanlasku
   start = xTaskGetTickCount();
   IR_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 suunta =0;

   Ultra_Start();                          // Ultra Sonic Start function
    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(100, 300);
                kytkin = 1;
            }
    }
while(kytkin == 1 && nappi == 1) {
    while(kytkin == 1 && nappi == 1 && risteys == 0) {  // Eteenpäin ajo, kunnes viiva.

        reflectance_digital(&dig);

        if (dig.l3 == 1 && dig.l2 == 1 && dig.l1 == 1 && dig.r1 == 1 && dig.r2 == 1 && dig.r3 == 1 && suunta== 0) {
                motor_forward(0, 10);   // Risteykseen ajo
                y++;
                Beep(100, 100);BatteryLed_Write(1);
                risteys = 1;
                }
        else if (dig.l3 == 0 && dig.l2 == 0 && dig.l1 == 0 && dig.r1 == 0 && dig.r2 == 1 && dig.r3 == 1 && suunta== 1) {
                motor_forward(50, 100);   // x lisää
                x++;
                Beep(100, 100);BatteryLed_Write(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.l3 == 0 && dig.l2 == 1 && dig.l1 == 1 && dig.r1 == 0 && dig.r2 == 0 && dig.r3 == 0) {
                motor_turn(30, 50, 10); // Korjaus vasemmalle
                }

        else if(dig.l3 == 0 && dig.l2 == 0 && dig.l1 == 0 && dig.r1 == 1 && dig.r2 == 1 && dig.r3 == 0) {
                motor_turn(50, 30, 10); // Korjaus oikealle
                }
    }
    while(kytkin == 1 && nappi == 1 && risteys == 1) {  //  Risteys

        reflectance_digital(&dig);

        if (dig.l3 == 1 && dig.l2 == 1 && dig.l1 == 1 && dig.r1 == 1 && dig.r2 == 1 && dig.r3 == 1) {
                motor_forward(50, 150);
                motor_hardturnleft(60, 90, 150);
                }

        else if (dig.r1 == 0){
        motor_hardturnleft(60, 90, 1);
        }
        else if (dig.r1 == 1){
        risteys = 0;
        suunta =1;
        }
    }
}
}
#endif