import time
import math
import machine as m
import os
import sys
import _thread
from machine import Pin
from machine import PWM

servo_pin1 = Pin(18)
servo_pin2 = Pin(19)
servo1 = PWM(servo_pin1, freq=50)
servo2 = PWM(servo_pin2, freq=50)
#
start_duty = 20
end_duty = 80
step = 1
delay = 0.05  
#
#Fonction servo 1
def aller_servo1():
    for duty in range(end_duty, start_duty - 1, -step):
        servo1.duty(duty)
        time.sleep(delay)
def retour_servo1():
    for duty in range(start_duty, end_duty + 1, step):
        servo1.duty(duty)
        time.sleep(delay)

#Fonction servo 2
def aller_servo2():
    for duty in range(end_duty, start_duty - 1, -step):
        servo2.duty(duty)
        time.sleep(delay)
def retour_servo2():
    for duty in range(start_duty, end_duty + 1, step):
        servo2.duty(duty)
        time.sleep(delay)
class MyCNC :
    def __init__(self):
        #Moteur X
        self.dir_pin_x = m.Pin(27, m.Pin.OUT)
        self.step_pin_x = m.Pin(13, m.Pin.OUT)
        self.step_pin_x.off() 
        #enable_pin1 =
        
        #Moteur Y = Y_Left
        self.dir_pin_yl = m.Pin(26, m.Pin.OUT)
        self.step_pin_yl = m.Pin(12, m.Pin.OUT)
        self.step_pin_yl.off()
        #enable_pin2 =
        
        #Moteur Z = Y_Right
        self.dir_pin_yr = m.Pin(25, m.Pin.OUT)
        self.step_pin_yr = m.Pin(14, m.Pin.OUT)
        self.step_pin_yr.off()
        #enable_pin3 =
        
        #Time of delay
        self.DELAY_X = 0.001
        self.DELAY_Y = 0.001
        
        #Current position
        self.current_pos_x = 0
        self.current_pos_y = 0
        
        #Coversion from mm to step
        self.ONE_MM_TO_STEP_X = 2
        self.ONE_MM_TO_STEP_Y = 2
        
    def move_step_x(self, pos_step_x, clockwise=False):
        if clockwise:
            self.dir_pin_x.on()
        else :
            self.dir_pin_x.off()
            
        for i in range(pos_step_x):
            self.step_pin_x.on()
            time.sleep(self.DELAY_X)
            self.step_pin_x.off()
            time.sleep(self.DELAY_X)


    def move_step_y(self, pos_step_y, clockwise_right_Motor=False):
        if clockwise_right_Motor:
            self.dir_pin_yr.on()
            self.dir_pin_yl.off()
        else :
            self.dir_pin_yr.off()
            self.dir_pin_yl.on()
            
        for i in range(pos_step_y):
            self.step_pin_yl.on()
            self.step_pin_yr.on()
            time.sleep(self.DELAY_Y)
            self.step_pin_yl.off()
            self.step_pin_yr.off()
            time.sleep(self.DELAY_Y)
    
    def goto_mm(self, pos_mm_x = 0, pos_mm_y = 0):
        
        step_x = (pos_mm_x - self.current_pos_x) * self.ONE_MM_TO_STEP_X
        step_y = (pos_mm_y - self.current_pos_y) * self.ONE_MM_TO_STEP_X
        
        if step_x < 0 :
            self.move_step_x(-1*step_x, True)
        else:
            self.move_step_x(step_x, False)
        
        if step_y < 0 :
            self.move_step_y(-1*step_y, True)
        else:
            self.move_step_y(step_y, False)
        
        self.current_pos_x = pos_mm_x
        self.current_pos_y = pos_mm_y
        

#Controling stepper motor of a CNC.

cnc = MyCNC()






#####Fonction de position et de détermination de l'angle pour la mise en marche du moteur de la pince####

# Liste des coordonnées avec catégories
coordinates = [((150, 150), 1), ((150, 200), 1), ((100, 250), 3), ((250, 150), 2), ((300, 250), 1)]

# Boucle pour exécuter les déplacements
for coord, categorie in coordinates:
    pos_mm_x, pos_mm_y = coord
    # Exécution du déplacement
    cnc.goto_mm(pos_mm_x, pos_mm_y)
    
    # Fonction pince
    time.sleep(4)
    aller_servo2()
    time.sleep(4)
    aller_servo1()
    time.sleep(4)
    retour_servo2()
    time.sleep(4)
    retour_servo1()
    time.sleep(4)
       
    # Vérification de la catégorie
    if categorie == 1:
        cnc.goto_mm(100, 375)
        # Appeler la fonction de la pince pour la catégorie 1
        time.sleep(2)
        aller_servo2()
        time.sleep(2)
        retour_servo2()
        time.sleep(2)

        
    elif categorie == 2:
        cnc.goto_mm(200, 375)
        # Appeler la fonction de la pince pour la catégorie 2
        time.sleep(2)
        aller_servo2()
        time.sleep(2)
        retour_servo2()
        time.sleep(2)
        
    elif categorie == 3:
        cnc.goto_mm(300, 375)
        # Appeler la fonction de la pince pour la catégorie 3
        time.sleep(2)
        aller_servo2()
        time.sleep(2)
        retour_servo2()
        time.sleep(2)
        
    elif categorie == 4:
        cnc.goto_mm(400, 375)
        # Appeler la fonction de la pince pour la catégorie 4
        time.sleep(2)
        aller_servo2()
        time.sleep(2)
        retour_servo2()
        time.sleep(2)
        
    # Attendre 2 secondes avant le prochain déplacement
    time.sleep(4)

# Retour à l'origine (0,0)
cnc.goto_mm(0, 0)