from machine import Pin, ADC
from time import sleep
from rotary_irq_esp import RotaryIRQ
r_1 = RotaryIRQ(
pin_num_clk = 18,
pin_num_dt = 5,
reverse = True,
incr = 1,
range_mode = RotaryIRQ.RANGE_UNBOUNDED,
pull_up = True,
half_step = False,
)
r_2 = RotaryIRQ(0, 2, True, 1, RotaryIRQ.RANGE_UNBOUNDED, True, False,)
dtValue_old_2 = r_2.value()
dtValue_old_1 = r_1.value()
BUTTON_Save = Pin(35, Pin.IN, Pin.PULL_UP)
BUTTON_Repeat = Pin(32, Pin.IN, Pin.PULL_UP)
BUTTON_Delete = Pin(34, Pin.IN, Pin.PULL_UP)
encoder_angle = 1 #угол раствора энкодера
keyboard_input = 2 #режим ввода угла поворота (1-клавиатура) (2 - энкодеры)
delay = 0.001 #задержка между шагами
index_motor_now = 0;
index_motor_max = 1;
DIR_1 = Pin(4, Pin.OUT) # направление движения 1 - по часовой, 0 - против
STEP_1 = Pin(16, Pin.OUT) # один шаг двигателя
DIR_2 = Pin(25, Pin.OUT)
STEP_2 = Pin(33, Pin.OUT)
'''
DIR_3 = Pin(25, Pin.OUT)
STEP_3 = Pin(26, Pin.OUT)
DIR_4 = Pin(27, Pin.OUT)
STEP_4 = Pin(14, Pin.OUT)
DIR_5 = Pin(12, Pin.OUT)
STEP_5 = Pin(13, Pin.OUT)
DIR_6 = Pin(2, Pin.OUT)
STEP_6 = Pin(15, 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]
#INPUT = [INPUT_1, INPUT_2, INPUT_3, INPUT_4, INPUT_5, INPUT_6]
DIR = [DIR_1, DIR_2]
STEP = [STEP_1, STEP_2]
Encoders = [r_1, r_2]
DTV = [dtValue_old_1, dtValue_old_2]
BUTTON = [BUTTON_Save, BUTTON_Repeat, BUTTON_Delete]
Motors_steps_in_deegres = [0.1125, 0.1125, 0.05625, 0.05625, 0.05625, 0.05625] # градус на один шаг
#Motors_steps_in_deegres = [0.05625, 0.05625, 0.05625, 0.05625, 0.05625, 0.05625] # градус на один шаг
#Motors_steps_in_deegres = [1.8, 1.8, 1.8, 1.8, 1.8, 1.8]
'''
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 (+)
'''
motors_number = len(STEP)
MOTOR_current_position = [0 for i in range(motors_number)]
SAVE_position = []
for i in range(motors_number):
SAVE_position.append([0])
BUTTON_PULL_UP = [0, 0, 0]
def recvie_angle(): #получение угла поворта
global MOTOR_current_position
global keyboard_input
global SAVE_position
global BUTTON_PULL_UP
global BUTTON
if keyboard_input == 1:
input_data = list(map(int, input('угол?\n').split(' '))) #обработка данных с клавиатуры
if keyboard_input == 2:
input_data = encoder_input()
if BUTTON[0].value() == 1 and BUTTON_PULL_UP[0] == 0:
BUTTON_PULL_UP[0] = 1
Save(input_data)
print(SAVE_position, "\n")
if BUTTON[0].value() == 0:
BUTTON_PULL_UP[0] = 0
if BUTTON[1].value() == 0 and BUTTON_PULL_UP[1] == 0:
BUTTON_PULL_UP[1] = 1
repeat = int(input('Сколько раз повторить?\n'))
for i in range(repeat):
Repeat()
if BUTTON[1].value() == 1:
BUTTON_PULL_UP[1] = 0
if BUTTON[2].value() == 1 and BUTTON_PULL_UP[2] == 0:
BUTTON_PULL_UP[2] = 1
for i in range(motors_number):
SAVE_position[i] = [0]
print(SAVE_position, "\n")
if BUTTON[2].value() == 0:
BUTTON_PULL_UP[2] = 0
angle_processing(input_data)
def angle_processing(input_data):
global MOTOR_current_position
now = MOTOR_current_position
new_angle = input_data # новое положение мотора после поворота ручки
rotation_angle = [abs(new_angle[i] - now[i]) for i in range(motors_number)] #угол, на который нужно повернуть
rotation_direction = [new_angle[i] >= now[i] for i in range(motors_number)] #направление, в котором нужно повернуть
steps(rotation_direction, rotation_angle)
MOTOR_current_position = input_data
sleep(0.01)
def steps(rotation_direction, degrees):
count_of_steps = [round(degrees[i] / Motors_steps_in_deegres[i]) for i in range(motors_number)]
while max(count_of_steps) > 0:
for index in range(motors_number):
if count_of_steps[index] > 0:
DIR[index].value(rotation_direction[index])
STEP[index].value(1)
sleep(delay)
for index in range(motors_number):
if count_of_steps[index] > 0:
STEP[index].value(0)
count_of_steps[index] -= 1
def encoder_input():
global DTV, Encoders, index_motor_now, index_motor_max
input_data = [MOTOR_current_position[i] for i in range(motors_number)]
index = Encoders[0].value()
if index > DTV[0] and index_motor_now < index_motor_max:
index_motor_now += 1
DTV[0] = index
print("now motor number: ", index_motor_now)
if index < DTV[0] and index_motor_now > 0:
index_motor_now -= 1
DTV[0] = index
print("now motor number: ", index_motor_now)
dtValue = Encoders[1].value()
if dtValue > DTV[1]:
input_data[index_motor_now] += encoder_angle
DTV[1] = dtValue
if dtValue < DTV[1]:
input_data[index_motor_now] -= encoder_angle
DTV[1] = dtValue
return input_data
def Save(input_data):
global SAVE_position
for i in range(len(input_data)):
SAVE_position[i].append(input_data[i])
def Repeat():
global SAVE_position
for i in range(len(SAVE_position[0])):
sleep(0.5)
input_data = [SAVE_position[j][i] for j in range(motors_number)]
angle_processing(input_data)
# planing
'''
1 - ввод трех координат: X, Y, Z
2 - находим угол между X и Y и передвигаем нижний мотор на этот угол
3 -
'''
# main
keyboard_input = int(input('выберите режим: 1 or 2\n'))
while True:
recvie_angle()