from machine import Pin, ADC
from time import sleep


DIR_1 = Pin(12, Pin.OUT)                       # направление движения 1 - по часовой, 0 - против
STEP_1 = Pin(14, Pin.OUT)                      # один шаг двигателя
INPUT_1 = ADC(Pin(26))                         # подключение пина для ручки
INPUT_1.atten(ADC.ATTN_11DB)

DIR_2 = Pin(33, Pin.OUT)
STEP_2 = Pin(25, Pin.OUT)
INPUT_2 = ADC(Pin(27))
INPUT_2.atten(ADC.ATTN_11DB)

'''
DIR_3 = Pin(..., Pin.OUT)
STEP_3 = Pin(..., Pin.OUT)

DIR_4 = Pin(..., Pin.OUT)
STEP_4 = Pin(..., Pin.OUT)

DIR_5 = Pin(..., Pin.OUT)
STEP_5 = Pin(..., Pin.OUT)

DIR_6 = Pin(..., Pin.OUT)
STEP_6 = Pin(..., Pin.OUT)
'''

#DIR = [DIR_1, DIR_2, DIR_3, DIR_4, DIR_5, DIR_6]
#STEP = [STEP_1, STEP_2, STEP_3, STEP_4, STEP_5, STEP_6]

DIR = [DIR_1, DIR_2, 0, 0, 0, 0]
STEP = [STEP_1, STEP_2, 0, 0, 0, 0]
INPUT = [INPUT_1, INPUT_2, 0, 0, 0, 0]

Motors_steps_in_deegres = [0.1125, 0.1125, 0.05625, 0.05625, 0.05625, 0.05625]    # градус на один шаг

'''
for DRV8825
M0 M1 M2 steps
0  0  0   1.8
1  0  0   0.9
0  1  1   0.45
1  1  0   0.225
0  0  1   0.1125
1  0  1   0.05625
0  1  1   0.05625
1  1  1   0.05625   (+)   1 градус в 0,036 секунд, то есть 360 градусов это 12,96 секунд

for A4988
MS1 MS2 MS3 steps
0   0   0   1.8
1   0   0   0.9
0   1   0   0.45
1   1   0   0.225
1   1   1   0.1125  (+)
'''

def steps(direct, degrees, index):                           # шаги : направление и количество шагов по 1,8 градусов
    count_of_steps = (degrees // Motors_steps_in_deegres[index])
    if degrees % Motors_steps_in_deegres[index] > Motors_steps_in_deegres[index] // 2:
        count_of_steps += 1
    DIR[index].value(direct)                                 # установка направления
    for step in range(count_of_steps):                       # цикл шагов
        STEP[index].value(1)                                 # подать напряжение на мотор
        sleep(0.0001)                                         # задержка 0,01 секунда
        STEP[index].value(0)                                 # снять напряжение с мотора
        sleep(0.0001)                                         # чем быстрее идет смена подачи напрядения, тем выше скорость поворота

MOTOR_deg = [0, 0, 0, 0, 0, 0]

def find_motor():
    for j in range(2):
        find = INPUT[j].read()
        if find != MOTOR_deg[j]:
            movement(j)
            
    

def movement(index):
    start = 90
    now = MOTOR_deg[index] *(180/4095)  # начальное положение мотора

    Motor = INPUT[index].read()    # диапазон значений 0 - 4095
    Motor /= (4095/180)       # новый диапазон 0 - 180
    
    new_deg = Motor - start  # новое положение мотора после поворота ручки
    
                # расчет движения мотора
    if new_deg >= 0 and new_deg >= now:
        deg = new_deg - now
        
        steps(1, deg, index)
        now = new_deg
    elif new_deg >= 0 and new_deg < now:
        deg = now - new_deg
        
        steps(0, deg, index)
        now = new_deg
    elif new_deg < 0 and new_deg >= now:
        deg = new_deg - now
        
        steps(1, deg, index)
        now = new_deg
    elif new_deg <0 and new_deg < now:
        deg = now - new_deg
        
        steps(0, deg, index)
        now = new_deg
        
    MOTOR_deg[index] = now * (4095/180)
    sleep(0.1)
        

#main
#potoc1 = _thread.start_new_thread(movement, [0])            # поток для первого мотора
#potoc2 = _thread.start_new_thread(movement, [1])            # поток для второго мотора

while True:
    find_motor()
'''
if keyboard_input == 2:
        pause = input('установите положение потенциометров и нажмине ENTER\n')
        input_data = [((INPUT[i].read() * 180 / 4095) - 90) for i in range(motors_number) ]