#ТЕЛО КОНТРОЛЯ СКОРОСТИ
print("______________________________________________________")
print("НАЧАЛО ПЛАНИРОВАНИЯ СКОРОСТИ")
print("______________________________________________________")
segments_FControl = [] #список сегментов после контроля скорости
velList = [] #для графиков
tList = [] #для графиков
vst_i = 0 #стартовая скорость
for i in range (len(segments_GeomAn)):
print("______________________________________________________")
print("Участок " + str(segments_GeomAn[i].Num+1) + ".")
if len(segments_GeomAn) - i < N:
N -= 1
#функция построения профиля скорости и предпросмотра, формирует профиль скорости на участке
#передаются: N участков, начальная скорость, системные параметры
seg = AccDecControl.FeedControl (vst_i, segments_GeomAn[i:i+N], N, T, v_max, acc_max, jerk_max, err_max, err_con, err_ch)
segments_FControl.append(seg)
vst_i = seg.Vel[-1]
velList += seg.Vel
#Вывод графиков
#профиль скорости
for i in range (len(velList )):
tList.append(T*i)
Graphs.Graph_01 (tList, velList , "T, с", "V, мм/c", "Профиль скорости", "Скорость инструмента", "Профиль скорости") #график
#ТЕЛО ИНТЕРПОЛЯЦИИ
print("______________________________________________________")
print("НАЧАЛО ИНТЕРПОЛЯЦИИ")
print("______________________________________________________")
acceleration = [25, 25] #allowable acceleration for each block
deceleration = [25, 25] #allowable deceleration for each block
jerk = [50, 10]
tsample = 0.01
#tolerance
linearErr = 0.01
#corner parameters
#расчет длины пути
blocklen = Interpolation.LengthLinear(trajectory[0], trajectory[1])
print("Длина пути: " + str(blocklen) + " мм.")
#предварительный расчет длины разгона и торможения
Nm = floor(acceleration[0]/(jerk[0]*tsample))
LengthAcc = AccDecControl.AccDecDisplacement (3, feedrate[1], jerk[0], acceleration[0], tsample, Nm)
#LengthDec = AccDecControl.AccDecDisplacement (feedrate[1], 0, jerk[0], acceleration[0], tsample, Nm)
print("Предварительная длина разгона: " + str(LengthAcc) + " мм.")
#print("кол-во шагов на разгоне " + str(LengthAcc[4][0]))
#print("кол-во шагов на постоянн " + str(LengthAcc[4][1]))
#print("кол-во шагов на торможе " + str(LengthAcc[4][2]))
Profile = AccDecControl.AccVelProfiles (3, feedrate[1], jerk[0], acceleration[0], tsample, Nm)
#вывод графического материала
timelist = []
time = 0
for i in range (len(Profile[1])):
time += tsample
timelist.append(time)
Graphs.Graph_01 (timelist, Profile[1],
'Время, с', 'Скорость, мм/с', 'Профиль скорости', 'Скорость инструмента')
#input data
move_type = [0] #type of trajectory for each block (0 - linear, 1 - circular, 2 - curve)
trajectory = [[0,0], [100, 100], [100, 90], [100, 0]] #trajectory points
feedrate = [30, 25, 30] #list of feedrates for each block
feedrate_max = [30, 30, 30]
acceleration = [25, 25] #allowable acceleration for each block
deceleration = [25, 25] #allowable deceleration for each block
jerk = [10, 10]
tsample = 0.01
#tolerance
linearErr = 0.01
#corner parameters
#расчет длины пути
blocklen = Interpolation.LengthLinear(trajectory[1], trajectory[2])
print("Длина пути: " + str(blocklen) + " мм.")
#предварительный расчет длины разгона и торможения
Nm = floor(acceleration[0]/(jerk[0]*tsample))
for i in range (0, 30):
LengthAcc = AccDecControl.AccDecDisplacement (0, i, jerk[0], acceleration[0], tsample, Nm)
#LengthDec = AccDecControl.AccDecDisplacement (feedrate[0], 0, jerk[0], acceleration[0], tsample, Nm)
print("Предварительная длина разгона: " + str(LengthAcc[0]) + " мм.")
print("Предварительная длина торможения: " + str(LengthAcc[2]) + " мм.")
print("Ошибка: " + str(LengthAcc[0]-LengthAcc[2]))
print("______________________________________________________")
radiusErr = Decimal(0.005).quantize(Decimal("1.0000"))
#corner parameters
velocity_list = []
for i in range(0, 2):
#расчет длины пути
blocklen = Decimal(
Interpolation.LengthLinear(trajectory[i], trajectory[i + 1])).quantize(
Decimal("1.0000"))
print("Участок " + str(i + 1) + ". Длина пути: " + str(blocklen) + " мм.")
#print("Участок " + str(i+1) + ".")
#предварительный расчет длины разгона и торможения
Nm = floor(acceleration[i] / (jerk[i] * tsample))
LengthAcc = AccDecControl.AccDecDisplacement(0, feedrate[i], jerk[i],
acceleration[i], tsample, Nm)
LengthDec = AccDecControl.AccDecDisplacement(feedrate[i], 0, jerk[i],
acceleration[i], tsample, Nm)
print(" Длина разгона: " + str(LengthAcc) + " мм.")
print(" Длина торможения: " + str(LengthDec) + " мм.")
"""
#расчет реальной максимальной скорости
MaxFeedrate = AccDecControl.RealMaxFeedrate (0, 0, feedrate[i], blocklen, Nm, LengthAcc, LengthDec, linearErr, jerk[i], acceleration[i], tsample)
print("Макс. скорость: " + str(MaxFeedrate[0]) + " мм/c.")
print("Длина разгона и торможения: " + str(MaxFeedrate[1]) + " мм.")
#построение профиля скорости
Profile = AccDecControl.AccDecType (0, 0, MaxFeedrate[0], blocklen, jerk[i], acceleration[i], tsample, Nm)
velocity_list += Profile[0]+Profile[1]+Profile[2]
feedrate_max = [30, 30, 30]
acceleration = [25, 25] #allowable acceleration for each block
deceleration = [25, 25] #allowable deceleration for each block
jerk = [10, 10]
tsample = 0.01
#tolerance
linearErr = 0.01
#corner parameters
#расчет длины пути
blocklen = Interpolation.LengthLinear(trajectory[1], trajectory[2])
print("Длина пути: " + str(blocklen) + " мм.")
#предварительный расчет длины разгона и торможения
Nm = floor(acceleration[0] / (jerk[0] * tsample))
LengthAcc = AccDecControl.AccDecDisplacement(0, feedrate[0], jerk[0],
acceleration[0], tsample, Nm)
LengthDec = AccDecControl.AccDecDisplacement(feedrate[0], 5, jerk[0],
acceleration[0], tsample, Nm)
print("Предварительная длина разгона: " + str(LengthAcc[0]) + " мм.")
print("Предварительная длина торможения: " + str(LengthDec[0]) + " мм.")
#расчет реальной максимальной скорости
MaxFeedrate = AccDecControl.RealMaxFeedrate(0, 6, feedrate[0], blocklen, Nm,
LengthAcc[0], LengthDec[0],
linearErr, jerk[0],
acceleration[0], tsample)
print("Максимальная скорость сегмента: " + str(MaxFeedrate[0]) + " мм/c.")
print("Длина разгона и торможения: " + str(MaxFeedrate[1]) + " мм.")
#tolerance
linearErr = Decimal(0.001).quantize(Decimal("1.0000"))
radiusErr = Decimal(0.005).quantize(Decimal("1.0000"))
#corner parameters
velocity_list = []
for i in range (1,2):
#расчет длины пути
blocklen = Decimal(Interpolation.LengthLinear(trajectory[i], trajectory[i+1])).quantize(Decimal("1.0000"))
print("Участок " + str(i+1) + ". Длина пути: " + str(blocklen) + " мм.")
#предварительный расчет длины разгона и торможения
Nm = floor(acceleration[i]/(jerk[i]*tsample))
LengthAcc = AccDecControl.AccDecDisplacement (4, feedrate[i], jerk[i], acceleration[i], tsample, Nm)
LengthDec = AccDecControl.AccDecDisplacement (feedrate[i], 5, jerk[i], acceleration[i], tsample, Nm)
print(" Длина разгона: " + str(LengthAcc) + " мм.")
print(" Длина торможения: " + str(LengthDec) + " мм.")
#расчет реальной максимальной скорости
MaxFeedrate = AccDecControl.RealMaxFeedrate (4, 5, feedrate[i], blocklen, Nm, LengthAcc, LengthDec, linearErr, jerk[i], acceleration[i], tsample)
print(" Макс. скорость: " + str(MaxFeedrate[0]) + " мм/c.")
#print("Длина разгона и торможения: " + str(MaxFeedrate[1]) + " мм.")
#построение профиля скорости
Profile = AccDecControl.AccDecType (4, 5, MaxFeedrate[0], blocklen, jerk[i], acceleration[i], tsample, Nm)
velocity_list += Profile[0]+Profile[1]+Profile[2]
#вывод графического материала
timelist = []
linearErr = Decimal(0.001)
#corner parameters
velocity_list = []
for i in range (1,2):
#расчет длины пути
blocklen = Decimal(Interpolation.LengthLinear(trajectory[i], trajectory[i+1])).quantize(Decimal("1.0000"))
print("Блок " + str(i+1) + ". Длина пути: " + str(blocklen) + " мм.")
#предварительный расчет длины разгона и торможения
Nm = floor(acceleration[i]/(jerk[i]*tsample))
Velstart = 4 #начальная скорость на сегменте
Velfinish = 5 #конечная скорость на сегменте
LengthAcc = AccDecControl.AccDecDisplacement (Velstart, feedrate[i], jerk[i], acceleration[i], tsample, Nm) #длина разгона
LengthDec = AccDecControl.AccDecDisplacement (feedrate[i], Velfinish, jerk[i], acceleration[i], tsample, Nm) #длина торможения
print("Предварительная длина разгона: " + str(LengthAcc) + " мм.")
print("Предварительная длина торможения: " + str(LengthDec) + " мм.")
#расчет реальной максимальной скорости
MaxFeedrate = AccDecControl.RealMaxFeedrate (Velstart, Velfinish, feedrate[i], blocklen, Nm, LengthAcc, LengthDec, linearErr, jerk[i], acceleration[i], tsample)
print("Максимальная скорость сегмента: " + str(MaxFeedrate[0]) + " мм/c.")
print("Длина разгона и торможения: " + str(MaxFeedrate[1]) + " мм.")
#построение профиля скорости
Profile = AccDecControl.AccDecType (Velstart, Velfinish, MaxFeedrate[0], blocklen, jerk[i], acceleration[i], tsample, Nm)
velocity_list += Profile[0]+Profile[1]+Profile[2]
"""
#вывод графического материала