'''
///////////////////////////////////////////////////////////////////////////
// Programa desenvolvido por Cristiano Teixeira                          //
// Sob Licença Apache 2.0                                                //
// https://github.com/ProfessorCristiano                                 //
// Hexapod com MicroPython e ESP32                                       //
// Programa utiliza ESP32 e Servo motores SG90 para controlar um quadpod //
// MePed V2.0, com 8 servos, para 4 patas                                //
// Sendo 4 para movimentos de 'ombro' e 4 para movimentos de 'joelho'.   //
// https://meped.io/mepedv2                                              //
// programa exemplo em:                                                  //
// https://www.meped.io/sites/default/files/2017-06/mePed_IR_Starter_Program_0.ino //
//  e                                                                    //
// https://github.com/keaoli/SPARX/blob/master/CoreMovement.py           //
// As posições iniciais são definidas e os movimentos são sincronizados  //
// com base nessas posições.                                             //
// Veja os comentários para entender como o código funciona.             //
///////////////////////////////////////////////////////////////////////////
'''

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



###############VARIAVEIS-INICIAIS##############
# Intevalo para sincronizar os movimentos
intervalo = 0

###POSIÇÕES-MEMORIZADAS###
#ombro frente direito inicial
ofdi = 90
#ombro frente direito frente varia em +50 do inicial
f = 50
ofdf = ofdi + f
#ombro frente direito tras varia em -30 do inicial
t = -30
ofdt = ofdi + t

ofei = 90
ofef = ofei - f
ofet = ofei - t

otdi = 90
otdf = otdi + f
otdt = otdi + t

otei = 90
otef = otei - f
otet = otei - t

#joelho frente direito normal
jfdn = 90
#joelho frente direito medio varia em +10 do inicial
m = 10
jfdm = jfdn + m 
#joelho frente direito alto varia em +20 do inicial
a = 20
jfda = jfdn + a 
#joelho frente direito baixo varia em -50 do inicial
b = -50
jfdb = jfdn + b 

jfen = 90
jfem = jfen + m 
jfea = jfen + a
jfeb = jfen + b     

jtdn = 90
jtdm = jtdn + m 
jtda = jtdn + a
jtdb = jtdn + b

jten = 90
jtem = jten + m 
jtea = jten + a
jteb = jten + b    
##################################################################


#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)
##########################################

# Criamos a instância para o servo nos pinos  
ofd = Servo(pin=19)
ofe = Servo(pin=21)
otd = Servo(pin=5)  
ote = Servo(pin=4)   

jfd = Servo(pin=32)   
jfe = Servo(pin=33)   
jtd = Servo(pin=13)   
jte = Servo(pin=14)   
###################################

###FUNÇÕES-PARA-SERVOS###################
#Função que faz o movimento
def mexe (motor, x, y):
    if x < y:
        for k in range (x,y,1):
            motor.move(k)
    else:
        for k in range (x,y,-1):
            motor.move(k)
##########################################
#Função que faz o movimento pausado
def mexep (motor1, x1, y1):
    if x1 < y1:
        ik1 = 1
    else:
        ik1 = -1
    k1=x1
    f1=y1
    for j in range (k1,f1,ik1):
        #print(j)
        if(k1 != f1):
            motor1.move(k1)
            k1=k1+ik1
        time.sleep(intervalo)
##########################################
#Função que faz 2 movimentos conjuntos
def mexe2 (motor1, x1, y1, motor2, x2, y2):
    if x1 < y1:
        ik1 = 1
        v1=y1-x1
    else:
        ik1 = -1
        v1=x1-y1
    if x2 < y2:
        ik2 = 1
        v2=y2-x2
    else:
        ik2 = -1
        v2=x2-y2
    k1=x1
    f1=y1
    k2=x2
    f2=y2
    #A variação sempre será a maior de todas
    v=max(v1,v2)
    # A repetição ocorre na maior variação
    for j in range (0,v,1):
        #porém tem o if para saber se um motor move ou se fica parado
        if(k1 != f1):
            motor1.move(k1)
        if(k2 != f2):
            motor2.move(k2)
        k1=k1+ik1
        k2=k2+ik2
        time.sleep(intervalo)
##########################################
#Função que faz 4 movimentos conjuntos
def mexe4 (motor1, x1, y1, motor2, x2, y2, motor3, x3, y3, motor4, x4, y4):
    
    if x1 < y1:
        ik1 = 1
        v1=y1-x1
    else:
        ik1 = -1
        v1=x1-y1
    if x2 < y2:
        ik2 = 1
        v2=y2-x2
    else:
        ik2 = -1
        v2=x2-y2
    if x3 < y3:
        ik3 = 1
        v3=y3-x3
    else:
        ik3 = -1
        v3=x3-y3
    if x4 < y4:
        ik4 = 1
        v4=y4-x4
    else:
        ik4 = -1
        v4=x4-y4
    v=max(v1,v2,v3,v4)

    k1=x1
    f1=y1
    k2=x2
    f2=y2
    k3=x3
    f3=y3
    k4=x4
    f4=y4

    for j in range (0,v,1):
        if(k1 != f1):
            motor1.move(k1)
        if(k2 != f2):
            motor2.move(k2)
        if(k3 != f3):
            motor3.move(k3)
        if(k4 != f4):
            motor4.move(k4)
        k1=k1+ik1
        k2=k2+ik2
        k3=k3+ik3
        k4=k4+ik4
        
        
        time.sleep(intervalo)
##########################################

##FUNÇÕES-DE-MOVIENTOS-PRE-DEFINIDOS##
def posicaoinicial():
    #move todos para a Posição inicial:
    ofd.move(ofdi)
    ofe.move(ofei)
    otd.move(otdi)  
    ote.move(otei)   

    jfd.move(jfdn)   
    jfe.move(jfen)   
    jtd.move(jtdn)   
    jte.move(jten)   
##########################################
def alturanormal():
    mexe4(jfd,jfdn,jfdn,jfe,jfen,jfen,jtd,jtdn,jtdn,jte,jten,jten)
##########################################
def medio():
    mexe4(jfd,jfdn,jfdm,jfe,jfen,jfem,jtd,jtdn,jtdm,jte,jten,jtem)
##########################################
def deita():
    mexe4(jfd,jfdn,jfdb,jfe,jfen,jfeb,jtd,jtdn,jtdb,jte,jten,jteb)
##########################################
def sobe():
    mexe4(jfd,jfdn,jfda,jfe,jfen,jfea,jtd,jtdn,jtda,jte,jten,jtea)
##########################################
def inclinafrente():
    mexe4(jfd,jfdn,jfdb,jfe,jfen,jfeb,jtd,jtdn,jtda,jte,jten,jtea)
##########################################
def inclinatras():
    mexe4(jfd,jfdn,jfda,jfe,jfen,jfea,jtd,jtdn,jtdb,jte,jten,jteb)
##########################################
def inclinadireita():
    mexe4(jfd,jfdn,jfdb,jfe,jfen,jfea,jtd,jtdn,jtdb,jte,jten,jtea)
##########################################
def inclinaesquerda():
    mexe4(jfd,jfdn,jfda,jfe,jfen,jfeb,jtd,jtdn,jtda,jte,jten,jteb)
##########################################
def andarfrente():
    print("Andar frente")
    #          o
    #
    #
    #  o
    #levanta as patas
    mexe2(jfd, jfdn, jfdm, jte, jten, jtem)
    time.sleep(0.5)
    mexe4(ofd, ofdf, ofdt, ote, otef, otet, ofe, ofet, ofef, otd, otdt, otdf)
    time.sleep(0.5)
    #abaixa as patas
    mexe2(jfd, jfdm, jfdn, jte, jtem, jten)

    #  o
    #
    #
    #          o    
    #levanta as outras
    mexe2(jfe, jfen, jfem, jtd, jtdn, jtdm)
    time.sleep(0.5)
    mexe4(ofd, ofdt, ofdf, ote, otet, otef, ofe, ofef, ofet, otd, otdf, otdt)
    mexe2(jfe, jfem, jfen, jtd, jtdm, jtdn)

#########################################
def frentetras():
    mexe4(ofd,ofdi,ofdf,ofe,ofei,ofef,otd,otdi,otdf,ote,otei,otef)
    print("frente")
    time.sleep(2)
    mexe4(ofd,ofdf,ofdt,ofe,ofef,ofet,otd,otdf,otdt,ote,otef,otet)
    print("tras")
#########################################
def sobedesce():
    mexe4(jfd,jfdn,jfda,jfe,jfen,jfea,jtd,jtdn,jtda,jte,jten,jtea)
    mexe4(jfd,jfda,jfdn,jfe,jfea,jfen,jtd,jtda,jtdn,jte,jtea,jten)
    mexe4(jfd,jfdn,jfdb,jfe,jfen,jfeb,jtd,jtdn,jtdb,jte,jten,jteb)
    mexe4(jfd,jfdb,jfdn,jfe,jfeb,jfen,jtd,jtdb,jtdn,jte,jteb,jten)
#########################################





########################INICIO#########################
#mexep(jfd,jfdb,jfdb)

while True:
    #frentetras()
    #time.sleep(1)
    #sobedesce()
    #time.sleep(1)
    andarfrente()