# Professor Cristiano Teixeira
# Sob Licença Apache 2.0

print("INICIO")
from machine import Pin, PWM, ADC
import time

#Classe Servo do site: https://www.upesy.com/blogs/tutorials/esp32-servo-motor-sg90-on-micropython#
class Servo:
    # these defaults work for the standard TowerPro SG90
    __servo_pwm_freq = 50
    __min_u10_duty = 26 - 0 # offset for correction
    __max_u10_duty = 123- 0  # offset for correction
    min_angle = 0
    max_angle = 180
    current_angle = 0.001


    def __init__(self, pin):
        self.__initialise(pin)


    def update_settings(self, servo_pwm_freq, min_u10_duty, max_u10_duty, min_angle, max_angle, pin):
        self.__servo_pwm_freq = servo_pwm_freq
        self.__min_u10_duty = min_u10_duty
        self.__max_u10_duty = max_u10_duty
        self.min_angle = min_angle
        self.max_angle = max_angle
        self.__initialise(pin)


    def move(self, angle):
        # round to 2 decimal places, so we have a chance of reducing unwanted servo adjustments
        angle = round(angle, 2)
        # do we need to move?
        if angle == self.current_angle:
            return
        self.current_angle = angle
        # calculate the new duty cycle and move the motor
        duty_u10 = self.__angle_to_u10_duty(angle)
        self.__motor.duty(duty_u10)

    def __angle_to_u10_duty(self, angle):
        return int((angle - self.min_angle) * self.__angle_conversion_factor) + self.__min_u10_duty


    def __initialise(self, pin):
        self.current_angle = -0.001
        self.__angle_conversion_factor = (self.__max_u10_duty - self.__min_u10_duty) / (self.max_angle - self.min_angle)
        self.__motor = PWM(Pin(pin))
        self.__motor.freq(self.__servo_pwm_freq)
##########################################

#Função que faz a conversão de um valor
def map(valor, de_min, de_max, para_min, para_max):
    # Verifica se o valor está fora da faixa de entrada
    if valor < de_min:
        valor = de_min
    elif valor > de_max:
        valor = de_max
    
    # Calcula o valor mapeado
    de_variacao = de_max - de_min
    para_variacao = para_max - para_min
    
    valor_mapeado = (valor - de_min) * para_variacao / de_variacao + para_min
    
    return valor_mapeado


# Criamos a instância para o servo nos pinos  
servo1 = Servo(pin=19)
servo2 = Servo(pin=18)   
servo3 = Servo(pin=5)   
servo4 = Servo(pin=4)      
j1_ve = ADC(Pin(12))
j1_ho = ADC(Pin(13))
j1_bt = Pin(14, Pin.OUT)
j2_ve = ADC(Pin(27))
j2_ho = ADC(Pin(26))
j2_bt = Pin(25, Pin.OUT)

# Criamos as variáveis que irão receber os valores. Sei que não precisa, mas é para fins didáticos.
vj1_ve=0 
vj1_ho=0
vj2_ve=0
vj2_ho=0

while True:
    #print("J1 Vertical", j1_ve.read())
    vj1_ve=map(j1_ve.read(), 0, 4095, 0, 180)
    #print("J1 Horizontal", j1_ho.read())
    vj1_ho=map(j1_ho.read(), 0, 4095, 0, 180)
    #print("J2 Vertical", j2_ve.read())
    vj2_ve=map(j2_ve.read(), 0, 4095, 0, 180)
    #print("J2 Horizontal", j2_ho.read())
    vj2_ho=map(j2_ho.read(), 0, 4095, 0, 180)
    servo1.move(int(vj1_ve))
    servo2.move(int(vj1_ho))
    servo3.move(int(vj2_ve))
    servo4.move(int(vj2_ho))
    time.sleep(0.1)