from machine import PWM, Pin, ADC
import time

# Configuration des broches et des servos
servo_pins = [14, 27, 26, 25]  # Broches GPIO des servos
servos = [PWM(Pin(pin), freq=50) for pin in servo_pins]

pot_pin = ADC(Pin(12))  # Potentiomètre pour commande manuelle (lecture analogique)

# Fonction pour mapper les valeurs
def map_value(x, in_min, in_max, out_min, out_max):
    return int((x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min)

# Fonction pour déplacer un servomoteur à un angle spécifique
def rotate(angle, servo):
    duty = map_value(angle, 0, 180, 1000, 9000)  # Map 0-180° en impulsions PWM (1ms à 9ms)
    servo.duty_u16(duty)

# Fonction pour synchroniser plusieurs servos
def rotate_multiple(angles):
    for i in range(len(servos)):
        if i < len(angles):
            rotate(angles[i], servos[i])
        else:
            rotate(90, servos[i])  # Par défaut, position neutre si angle non spécifié

# Fonction pour exécuter une séquence automatique (balayage simple)
def sweep_servo(servo, start_angle=0, end_angle=180, step=10, delay=0.1):
    for angle in range(start_angle, end_angle + 1, step):
        rotate(angle, servo)
        time.sleep(delay)
    for angle in range(end_angle, start_angle - 1, -step):
        rotate(angle, servo)
        time.sleep(delay)

# Fonction pour simuler un bras robotique attrapant un objet
def robotic_grab():
    print("Simulation : attraper un objet")
    rotate(0, servos[0])  # Base vers 0°
    rotate(90, servos[1])  # Lever le bras
    rotate(45, servos[2])  # Plier l'avant-bras
    rotate(0, servos[3])  # Ouvrir la pince
    time.sleep(1)
    rotate(45, servos[3])  # Fermer la pince
    print("Objet attrapé !")

# Fonction pour relâcher un objet
def robotic_release():
    print("Simulation : relâcher un objet")
    rotate(90, servos[3])  # Ouvrir la pince
    time.sleep(1)

# Boucle principale
while True:
    print("Options :")
    print("1. Commande manuelle avec le potentiomètre")
    print("2. Balayage d'un servo (test)")
    print("3. Déplacement coordonné des servos (test)")
    print("4. Simulation : attraper un objet")
    print("5. Simulation : relâcher un objet")
    print("6. Quitter")
    
    choice = input("Choisissez une option (1-6) : ")
    
    if choice == "1":
        # Commande manuelle avec le potentiomètre
        print("Tournez le potentiomètre pour commander le servo")
        while True:
            pot_value = pot_pin.read_u16()
            angle = map_value(pot_value, 0, 65535, 0, 180)
            rotate(angle, servos[0])  # Applique la commande au premier servo
            print(f"Potentiomètre : {pot_value}, Angle : {angle}")
            time.sleep(0.1)
    
    elif choice == "2":
        # Balayage d'un servo
        print("Balayage du servo 1")
        sweep_servo(servos[0])
    
    elif choice == "3":
        # Déplacement coordonné des servos
        print("Déplacement coordonné des servos")
        angles = [0, 90, 45, 30]  # Angles pour chaque servo
        rotate_multiple(angles)
        time.sleep(2)
        rotate_multiple([90, 45, 90, 90])  # Deuxième position
        time.sleep(2)
    
    elif choice == "4":
        # Simulation : attraper un objet
        robotic_grab()
    
    elif choice == "5":
        # Simulation : relâcher un objet
        robotic_release()
    
    elif choice == "6":
        # Quitter
        print("Au revoir !")
        break
    
    else:
        print("Choix invalide, veuillez réessayer.")