#if 1

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

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

        motor_turn(l_speed, r_speed, delay);
    }

void motor_tank_turn_90dg(int left_dir, int right_dir)
    {
        MotorDirLeft_Write(left_dir);
        MotorDirRight_Write(right_dir);
        PWM_WriteCompare1(60);
        PWM_WriteCompare2(60);
        vTaskDelay(1500);

    }
void motor_random_turn ()
{
    int direction = rand() % 2;

    if (direction==0)
    {
        motor_turn(100, 70, 500);

    }
    else
    {
        motor_turn(70, 100, 500);

    }
}

void zmain(void)
{
    struct accData_ data;
    motor_start();
    motor_forward(0,0);


    while (SW1_Read == 1)
    {
        vTaskDelay(200);
    }

    vTaskDelay(1000);

    motor_forward_fixed(200,0);

    for(;;)
    {
        LSM303D_Read_Acc(&data);

        if (data.accX <0)
        {
            motor_backward(100,500);
            int suunta = rand() % 2;

            if(suunta==0)
            {
                motor_tank_turn_90dg(0,1);
            }
            else
            {
                motor_tank_turn_90dg(1,0);
            }
        }

        int randomTurn = rand() % 10;

         if (randomTurn == 0)
         {
            motor_random_turn();
            motor_forward_fixed(200,0);
        }
        vTaskDelay(50);
    }
}

#endif