---

import time
import pymurapi as mur

auv = mur.mur_init()

def clamp(v, lo=-100, hi=100):
    return max(lo, min(hi, v))

def angle_diff(target, current):
    d = target - current
    while d > 180:  d -= 360
    while d < -180: d += 360
    return d

KP_DEPTH = 80
KP_YAW = 0.6

def hold(depth, yaw, forward=0, duration=3):
    t0 = time.time()
    while time.time() - t0 < duration:
        # вертикальные моторы (2, 3)
        u_z = clamp(KP_DEPTH * (depth - auv.get_depth()))
        auv.set_motor_power(2, u_z)
        auv.set_motor_power(3, u_z)
        # горизонтальные моторы (0, 1)
        u_yaw = clamp(KP_YAW * angle_diff(yaw, auv.get_yaw()))
        auv.set_motor_power(0, clamp(forward - u_yaw))
        auv.set_motor_power(1, clamp(forward + u_yaw))
        time.sleep(0.05)

yaw0 = auv.get_yaw()
hold(1.5, yaw0, duration=4)                 # погружение
hold(1.5, yaw0 + 90, duration=4)            # поворот
hold(1.5, yaw0 + 90, forward=40, duration=6)