'''
///////////////////////////////////////////////////////////////////////////
// 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.             //
///////////////////////////////////////////////////////////////////////////
'''

from machine import Pin, PWM
import time

# Configuração dos pinos dos servos
servos = {
    "ombro_frente_direito": PWM(Pin(19), freq=50),
    "joelho_frente_direito": PWM(Pin(32), freq=50),
    "ombro_frente_esquerdo": PWM(Pin(21), freq=50),
    "joelho_frente_esquerdo": PWM(Pin(33), freq=50),
    "ombro_tras_direito": PWM(Pin(5), freq=50),
    "joelho_tras_direito": PWM(Pin(13), freq=50),
    "ombro_tras_esquerdo": PWM(Pin(4), freq=50),
    "joelho_tras_esquerdo": PWM(Pin(14), freq=50),
}

# Função para mover um servo para um ângulo específico
def mover_servo(servo, angulo):
    duty = int((angulo / 180) * 1023)  # Converte ângulo para duty cycle
    servos[servo].duty(duty)

# Posições neutras (iniciais)
posicoes_iniciais = {
    "ombro_frente_direito": 90,
    "joelho_frente_direito": 90,
    "ombro_frente_esquerdo": 90,
    "joelho_frente_esquerdo": 90,
    "ombro_tras_direito": 90,
    "joelho_tras_direito": 90,
    "ombro_tras_esquerdo": 90,
    "joelho_tras_esquerdo": 90,
}

# Função para definir as posições iniciais
def resetar_posicoes():
    for servo, angulo in posicoes_iniciais.items():
        mover_servo(servo, angulo)
    time.sleep(1)

# Função para andar para frente
def andar_para_frente():
    # Levanta a perna direita da frente
    mover_servo("joelho_frente_direito", 60)
    time.sleep(0.2)
    # Move o ombro para frente
    mover_servo("ombro_frente_direito", 120)
    time.sleep(0.2)
    # Abaixa a perna
    mover_servo("joelho_frente_direito", 90)
    time.sleep(0.2)
    # Repete para as outras pernas
    mover_servo("joelho_tras_esquerdo", 60)
    time.sleep(0.2)
    mover_servo("ombro_tras_esquerdo", 120)
    time.sleep(0.2)
    mover_servo("joelho_tras_esquerdo", 90)
    time.sleep(0.2)

# Função para virar à esquerda
def virar_esquerda():
    mover_servo("ombro_frente_direito", 120)
    mover_servo("ombro_tras_direito", 60)
    time.sleep(0.5)
    resetar_posicoes()

# Função para parar
def parar():
    resetar_posicoes()

# Programa principal
resetar_posicoes()
while True:
    andar_para_frente()
    time.sleep(1)
    virar_esquerda()
    time.sleep(1)
    parar()
    time.sleep(2)