import brian.sensors as sensors
import brian.motors as motors
import time

print("Opening S2 as a gyro sensor and A&B as motors")
gyro = sensors.EV3.GyroSensorEV3(sensors.SensorPort.S1)
gyro_mode = sensors.EV3.GyroSensorEV3.Mode.ANGLE_AND_SPEED
motor_L = motors.EV3LargeMotor(motors.MotorPort.B)
motor_R = motors.EV3LargeMotor(motors.MotorPort.A)

while not gyro.is_ready():
    continue

gyro.set_mode(gyro_mode)

time.sleep(0.5)

phi_offset = 6
Ts = 0.02 # period

K_phi = 3.5524/2
K_dphi = 0.5850/2
K_theta = 0.0271/2
K_dtheta = 0.0672/2

Kp_yaw = 0.05

theta_offset = 0
dtheta_offset = 0

prev_time = time.time_ns()

while True:
    (phi, dphi) = gyro.angle_and_speed()

    theta_error_L = motor_L.current_angle()
    theta_error_R = motor_R.current_angle()
    theta = (theta_error_L+theta_error_R)/2
    yaw = theta_error_R-theta_error_L
    
    dtheta_error_L = motor_L.current_speed()
    dtheta_error_R = motor_R.current_speed()
    dtheta = (dtheta_error_L+dtheta_error_R)/2

    if phi > 50 or phi < -40:
        break

    phi_error = phi - phi_offset
    
    u_rot = Kp_yaw*yaw

    u = K_phi*phi_error + K_dphi*dphi + K_theta*(theta-theta_offset) + K_dtheta*(dtheta-dtheta_offset)

    motor_L.run_at_voltage(u + u_rot)
    motor_R.run_at_voltage(u - u_rot)

    sleep_diff = (time.time_ns() - prev_time)*1e-9
    time.sleep(Ts - sleep_diff) # make all cycles equally long
    
    prev_time = time.time_ns()