# Библиотека для работы с сервоприводами
from gpiozero import AngularServo

# Библиотека для работы с платой Troyka HAT
import troykahat

# Библиотека для работы со временем
from time import sleep

# Создаём объект для работы с сервоприводом
# - pin: номер пина, к которому подключён сервопривод
# WPIx, нумерация WiringPi, где x — номер пина
# GPIOx, нумерация GPIO/BCM, где x — номер пина
# - min_angle: минимальный угол поворота сервопривода
# Опциональный и по умолчанию стоит: -90°
# - min_angle: максимальный угол поворота сервопривода
# Опциональный и по умолчанию стоит: 90°
# - min_pulse_width: ширина импульса, соответствующая минимальному углу поворота сервопривода
# Опциональный и по умолчанию стоит: 0.001 c = 1000 мкс
# - max_pulse_width: ширина импульса, соответствующая максимальному углу поворота сервопривода
# Опциональный и по умолчанию стоит: 0.002 c = 2000 мкс
# Подробности нумерации: https://gpiozero.readthedocs.io/en/stable/recipes.html#pin-numbering
# Каждая модель сервопривода имеет свои характеристики
servo = AngularServo(
    "WPI6", min_angle=0, max_angle=180, min_pulse_width=0.000544, max_pulse_width=0.0024
)

# Назначаем константное имя пину 5 из группы "Analog IO",
# Подключите потенциометр к этому пину.
PIN_AP_SERVO_FEEDBACK = 5
# Создаём объект ap для работы с пинами,
# помеченными как «Analog IO» на плате Troyka HAT.
# Это пины, подключенные к встроенному на плате I²C-расширителю
# на микроконтроллере STM32F030F4P6.
ap = troykahat.analog_io()

# Шаг сервопривода
step_angle_servo = 1


def remap(x, min_in, max_in, min_out, max_out):
    return (x - min_in) * (max_out - min_out) / (max_in - min_in) + min_out


# Функция для считывания «сырых» данных положения сервопривода
# и перевода их в угол поворота
def read_position_angle(pin):
    position_raw = ap.analogRead(pin)
    position_angle = round(remap(position_raw, 0.12, 0.7, 0, 180))
    return position_angle


while True:
    for val in range(servo.min_angle, servo.max_angle, step_angle_servo):
        servo.angle = val
        print(f"Servo position = {read_position_angle(PIN_AP_SERVO_FEEDBACK)} degrees")
        sleep(0.02)
    for val in range(servo.max_angle, servo.min_angle, -step_angle_servo):
        servo.angle = val
        print(f"Servo position = {read_position_angle(PIN_AP_SERVO_FEEDBACK)} degrees")
        sleep(0.02)