from machine import Pin, PWM, Timer #Для управления пинами
from time import sleep_us #Для задержек во время движения
from utime import sleep
from umqtt.simple import MQTTClient #Для подключения MQTT
import network #Для подключения к Wi-Fi
import time #Для установки таймера переподключения к MQTT
import ujson #Для формирования обратных сообщений
import dht #Для подключения датчика DTH22(если установлен)


SERVER = "broker.mqttdashboard.com" #Подключаемся к брокеру(подойдет любой, в данном примере это HiveMQ)
username='' #Имя подключения (можно оставить пустым)
password='' #Пароль подключения (можно оставить пустым)
CLIENT_ID = "micropython-weather-demo" #ID криента можно указать любое
TOPIC="[email protected]_Robot1" #Топик через который осуществляется управление
#(Будет указан в блоке сообщений после добавления устройства в "Мои устройства")

# НАСТРОЙКА УСТРОЙСТВА

# Пины подключения контроллера A4988 к микроконтроллеру
step_pin = Pin(12, Pin.OUT)
dir_pin = Pin(13, Pin.OUT)
enable_pin = Pin(14, Pin.OUT)
servo_pin = Pin(2, Pin.OUT)

#Пин для датчика DTH22
sensor = dht.DHT22(Pin(15))

#Пины светодиодов
forw_led_pin = Pin(33,Pin.OUT)
righ_led_pin = Pin(26,Pin.OUT)
back_led_pin = Pin(32,Pin.OUT)
left_led_pin = Pin(25,Pin.OUT)

#Настройка сервомотора
servo = PWM(servo_pin, freq=45,duty=70)

# Настройки контроллера A4988
microsteps = 16
current = 1000  # Текущая настройка тока в мА

# Включаем контроллер A4988
enable_pin.value(0)

#Переменная для 
dutyCheck = 90;

#ФУНКЦИИ ДЛЯ УПРАВЛЕНИЯ УСТРОЙСТВОМ

#Функция для управления сервомотором
def map(x,in_min,in_max,out_min,out_max):
    return int((x-in_min)*(out_max-out_min)/(in_max-in_min)+out_min)

#Функция левого поворота
def turnLeft(x):
    for i in range(x,0,-12):
        servo.duty(map(i,0,180,20,120))
        x = map(i,0,180,20,120)
        left_led_pin.value(1)
        sleep(0.1)
        left_led_pin.value(0)
        sleep(0.1)

    return x

#Функция правого поворота
def turnRight(x):
    for i in range(x,180,12):
        servo.duty(map(i,0,180,20,120))
        x = map(i,0,180,20,120)
        righ_led_pin.value(1)
        sleep(0.1)
        righ_led_pin.value(0)
        sleep(0.1)
    return x

#Функция возвращения сервомотора в исходное состояние
def turnZero(x):
    servo.duty(x)



# Функция для управления шаговым двигателем
def move(steps, direction, delay):
    dir_pin.value(direction)
    if direction == 1:
        forw_led_pin.value(1)
    else:
        back_led_pin.value(1)


    for i in range(steps):
        step_pin.value(1)
        sleep_us(delay)
        step_pin.value(0)
        sleep_us(delay)

    forw_led_pin.value(0)
    back_led_pin.value(0)


#Подключение к Wi-FI
print("Connecting to WiFi", end="")
sta_if = network.WLAN(network.STA_IF)
sta_if.active(True)
sta_if.connect('Wokwi-GUEST', '') #Подключение к Wi-Fi првый аргумент — имя сети, второй — пароль
while not sta_if.isconnected():
  print(".", end="")
  time.sleep(0.1)
print(" Connected!")

#УПРАВЛЕНИЕ УСТРОЙСТВОМ

try:
    def sub_cb(topic, msg):
        print((topic, msg))

        if topic == TOPIC.encode():

            if msg == b"left":
                turnLeft(dutyCheck)

            if msg == b"right":
                print("right message")
                turnRight(dutyCheck)

            if msg == b"forward":
                print("forward message")
                move(200, 1, 1000)
                turnZero(70)

            if msg == b"backward":
                print("backward message")
                move(200, 0, 1000)
                turnZero(70)
            
            if msg == b"get":
                sensor.measure() 
                message = "Temperature: {}°C, Humidity: {}%".format(str(sensor.temperature()),str(sensor.humidity()))
                print("Reporting to MQTT topic {}: {}".format(TOPIC, message))
                sleep(0.1)
                c.publish(TOPIC, message)

            if msg == b"F1":
                print("Функция 1")

            if msg == b"F2":
                print("Функция 2")

            if msg == b"F3":
                print("Функция 3")

            if msg == b"F4":
                print("Функция 4")

            if msg == b"F5":
                print("Функция 5")

            if msg == b"F6":
                print("Функция 6")

    #Подключение к топику
    c = MQTTClient(CLIENT_ID, SERVER,1883,username,password,keepalive=60)
    c.connect()                             # подключение к mqtt
    c.set_callback(sub_cb)                  # Вызов функции sub_cb(обработка полученых сообщений)
    c.subscribe(TOPIC)                      # "Подписка" на топик
    print("Connected to %s" % SERVER)

    tim1 = Timer(1)                         # Создание таймера для переподключения
    tim1.init(period=20000, mode=Timer.PERIODIC,callback=lambda n:c.ping())     #Переподключение к топику для дальнейшего отслеживания сообщений

    while True:
        c.wait_msg()                        # Ожидание сообщений
        
finally:
    c.disconnect()                          # Отключение в случае сбоя