SLO

from ev3dev.ev3 import*
import math


def jazda(motor1, motor2, srednica, cm, predkosc,):
    obwod = srednica*math.pi
    stopni_na_cm = 360/obwod
    motor1.run_to_rel_pos(speed_sp= predkosc, position_sp=stopni_na_cm*cm)
    motor2.run_to_rel_pos(speed_sp= predkosc, position_sp=stopni_na_cm*cm)
    motor1.wait_while("running")
    motor2.wait_while("running")

def reset(gyro):
    gyro.mode = "GYRO-ANG"
    gyro.mode = "GYRO-RATE"
    gyro.mode = "GYRO-ANG"

def skret(motor_skret, motor_stop, gyro, stopnie, predkosc):
    reset(gyro)
    while abs(gyro.value()) < stopnie:
        print(abs(gyro.value()))
        motor_skret.run_forever(speed_sp=predkosc)
        motor_stop.stop(stop_action = "brake")
    motor_skret.stop(stop_action = "brake")

if __name__ == "__main__":
    lewy = LargeMotor(OUTPUT_A)
    prawy = LargeMotor(OUTPUT_B)
    gyro = GyroSensor()

    jazda(lewy,prawy, 5.9, 20, 800)
    skret(lewy,prawy, gyro, 90, 800)
    skret(prawy,lewy, gyro, 180, 800)
    jazda(lewy,prawy, 5.9, -20, 800)