#if 1

void motor_forward_fixed(uint8 speed, uint32 delay)
{
    float ratio = 1.05; // pitää säätää, 1.3 vähän liikaa

    uint8 l_speed = speed;
    uint8 r_speed = (float)speed / ratio;

    printf("l_speed: %d, r_speed: %d\n", l_speed, r_speed); // tulostaa nopeudet

    motor_turn(l_speed, r_speed, delay);
}

void motor_turn_right_90dg()
{
    motor_turn(155, 0, 600);
}

void zmain(void)
{

    motor_start(); // käynnistetään moottori
    motor_forward(0, 0); // asetetaan moottori pysähtyneeksi


    for(;;)
    {
        while(SW1_Read() == 1) // loopetaan kunnes nappia painetaan
        {
            vTaskDelay(200); // odotetaan 200ms
        }

        // nappia painettu, odotetaan sekunti
        vTaskDelay(1000);

        motor_forward_fixed(100, 4000);
        motor_turn_right_90dg();
        motor_forward_fixed(100, 3000);
        motor_turn_right_90dg();
        motor_forward_fixed(100, 3600);

        motor_turn(155, 0, 750); // yli 90 astetta käännös

        motor_turn(100, 70, 1000);
        motor_turn(100, 45, 2500);

        motor_forward_fixed(100, 1700);

        motor_forward(0, 0);
    }
 }

#endif