python 直线插补、圆弧插补、树莓派+步进电机

1. python3 实现直线插补、圆弧插补，并用matplotlib画出插补轨迹，简单模拟。运行程序前 确认以python3执行，且安装numpy库、matplotlib库。

运行后选择“l”并输入终点坐标即可模拟直线插补

选择“c”并输入圆心和终点坐标即可模拟圆弧插补，若圆弧超过一个象限，须用坐标轴将其划分为多段执行。

选择“a”自动执行文件”point_set.txt“中的点，格式如下：

 

 

  1 #!usr/bin/env/ python
  2 # -*- coding:utf-8 -*-
  3 # Author: XiaoFeng
  4 import time
  5 import numpy as np
  6 import matplotlib.pyplot as plt
  7 import matplotlib.animation as animation
  8 import sys
  9 
 10 
 11 # 实际坐标
 12 rel_x = 0
 13 rel_y = 0
 14 # 方向
 15 dir_x = 1
 16 dir_y = 1
 17 # 脉冲频率
 18 delay_high = 0.001
 19 delay_low = 0.001
 20 # 脉冲数
 21 times = 4
 22 # 工动点
 23 Xs = 0
 24 Ys = 0
 25 # 点痕迹
 26 point_x = []
 27 point_y = []
 28 # 放大系数
 29 k = 4
 30 
 31 
 32 def plot_point():
 33     # 画图
 34     fig, ax = plt.subplots()
 35     x = np.divide(point_x, k)
 36     y = np.divide(point_y, k)
 37     ax.plot(x, y)  # 绘制曲线 y1
 38     ax.set(xlabel='Distance_X (mm)', ylabel='Distance_Y (mm)',
 39            title='The trail of the point')
 40     ax.grid()
 41     # Move the left and bottom spines to x = 0 and y = 0, respectively.
 42     ax.spines["left"].set_position(("data", 0))
 43     ax.spines["bottom"].set_position(("data", 0))
 44     # Hide the top and right spines.
 45     ax.spines["top"].set_visible(False)
 46     ax.spines["right"].set_visible(False)
 47     # 坐标轴上加箭头
 48     ax.plot(1, 0, ">k", transform=ax.get_yaxis_transform(), clip_on=False)
 49     ax.plot(0, 1, "^k", transform=ax.get_xaxis_transform(), clip_on=False)
 50     plt.show()
 51 
 52 
 53 def puls_maker_x():
 54     """
 55     产生脉冲
 56     :param delay: 电平持续时间
 57     :param times: 脉冲个数
 58     :return:
 59     """
 60     print("xxx")
 61     print("方向:", dir_x)
 62     for i in range(times):
 63         print("11111")
 64         time.sleep(delay_high)
 65         print("00000")
 66         time.sleep(delay_low)
 67 
 68 
 69 def puls_maker_y():
 70     """
 71     产生脉冲
 72     :param delay: 电平持续时间
 73     :param times: 脉冲个数
 74     :return:
 75     """
 76     print("yyyyyy")
 77     print("方向:", dir_y)
 78     for i in range(times):
 79         print("11111")
 80         time.sleep(delay_high)
 81         print("00000")
 82         time.sleep(delay_low)
 83 
 84 
 85 def line_interpolation(x_e, y_e):
 86     """
 87     直线插补
 88     :param x_e: 目标X
 89     :param y_e: 目标Y
 90     :return:
 91     """
 92     global Xs, Ys
 93     global dir_x, dir_y
 94     global point_x, point_y
 95     f_line = 0
 96     x = 0
 97     y = 0
 98     delta_x = x_e - Xs
 99     delta_y = y_e - Ys
100     cnt = abs(delta_x) + abs(delta_y)
101     if delta_x > 0:
102         dir_x = 1
103         x = 1
104         print("右")
105     elif delta_x < 0:
106         dir_x = 0
107         x = -1
108         print("左")
109     elif delta_x == 0 and delta_y > 0:
110         dir_y = 1
111         y = 1
112         print("上")
113         while cnt > 0:
114             puls_maker_y()
115             Ys += y
116             point_x.append(Xs)
117             point_y.append(Ys)
118             print("X动点坐标：", Xs, Ys)
119             cnt -= 1
120     elif delta_x == 0 and delta_y < 0:
121         dir_y = 0
122         y = -1
123         print("下")
124         while cnt > 0:
125             puls_maker_y()
126             Ys += y
127             point_x.append(Xs)
128             point_y.append(Ys)
129             print("X动点坐标：", Xs, Ys)
130             cnt -= 1
131     if delta_y > 0:
132         dir_y = 1
133         y = 1
134         print("上")
135     elif delta_y < 0:
136         dir_y = 0
137         y = -1
138         print("下")
139     elif delta_y == 0 and delta_x > 0:
140         dir_x = 1
141         x = 1
142         print("右")
143         while cnt > 0:
144             puls_maker_x()
145             Xs += x
146             point_x.append(Xs)
147             point_y.append(Ys)
148             print("X动点坐标：", Xs, Ys)
149             cnt -= 1
150     elif delta_y == 0 and delta_x < 0:
151         dir_x = 0
152         x = -1
153         print("左")
154         while cnt > 0:
155             puls_maker_x()
156             Xs += x
157             point_x.append(Xs)
158             point_y.append(Ys)
159             print("X动点坐标：", Xs, Ys)
160             cnt -= 1
161     while cnt > 0:
162         if f_line >= 0:
163             puls_maker_x()
164             f_line -= abs(delta_y)
165             Xs += x
166             point_x.append(Xs)
167             point_y.append(Ys)
168             print("X动点坐标：", Xs, Ys)
169         else:
170             puls_maker_y()
171             f_line += abs(delta_x)
172             Ys += y
173             point_x.append(Xs)
174             point_y.append(Ys)
175             print("Y动点坐标：", Xs, Ys)
176         cnt -= 1
177     Xs = x_e
178     Ys = y_e
179     print("实时坐标：", Xs / k, Ys / k)
180     print("插补结束")
181 
182 
183 def arc_interpolation(x_c, y_c, xe, ye):
184     """
185     圆弧插补
186     :param x_c: 弧心X
187     :param y_c: 弧心Y
188     :param xe: 绝对目标X
189     :param ye: 绝对目标Y
190     :return:
191     """
192     global Xs, Ys
193     global dir_x, dir_y
194     global point_x, point_y
195     # 绝对坐标(xs,ys),(xe,ye)转相对坐标(x_s,y_s),(x_e,y_e)
196     x_s = Xs - x_c
197     y_s = Ys - y_c
198     x_e = xe - x_c
199     y_e = ye - y_c
200     f = 0
201     cnt = abs(x_e - x_s) + abs(y_e - y_s)
202     x2 = (x_s + x_e) / 2
203     y2 = (y_s + y_e) / 2
204     # 判断顺弧还是逆弧
205     n = x_s * (y_e - y_s) - (x_e - x_s) * y_s
206     # 判断象限
207     if x2 > 0 and y2 > 0:
208         print("第一象限")
209         if n > 0:
210             print("逆圆")
211             dir_x = 0
212             dir_y = 1
213             print("左上")
214             while cnt > 0:
215                 if f >= 0:
216                     puls_maker_x()
217                     f = f - 2 * x_s + 1
218                     x_s -= 1
219                     print("X动点坐标：", x_s + x_c, y_s + y_c)
220                     point_x.append(x_s + x_c)
221                     point_y.append(y_s + y_c)
222                 else:
223                     puls_maker_y()
224                     f = f + 2 * y_s + 1
225                     y_s += 1
226                     print("Y动点坐标：", x_s + x_c, y_s + y_c)
227                     point_x.append(x_s + x_c)
228                     point_y.append(y_s + y_c)
229                 cnt -= 1
230         elif n < 0:
231             print("顺圆")
232             dir_x = 1
233             dir_y = 0
234             print("右下")
235             while cnt > 0:
236                 if f >= 0:
237                     puls_maker_y()
238                     f = f - 2 * y_s + 1
239                     y_s -= 1
240                     print("Y动点坐标：", x_s + x_c, y_s + y_c)
241                     point_x.append(x_s + x_c)
242                     point_y.append(y_s + y_c)
243                 else:
244                     puls_maker_x()
245                     f = f + 2 * x_s + 1
246                     x_s += 1
247                     print("X动点坐标：", x_s + x_c, y_s + y_c)
248                     point_x.append(x_s + x_c)
249                     point_y.append(y_s + y_c)
250                 cnt -= 1
251         else:
252             print("直线")
253     elif y2 > x2 < 0:
254         print("第二象限")
255         if n > 0:
256             print("逆圆")
257             dir_x = 0
258             dir_y = 0
259             print("左下")
260             while cnt > 0:
261                 if f >= 0:
262                     puls_maker_y()
263                     f = f - 2 * y_s + 1
264                     y_s -= 1
265                     print("Y动点坐标：", x_s + x_c, y_s + y_c)
266                     point_x.append(x_s + x_c)
267                     point_y.append(y_s + y_c)
268                 else:
269                     puls_maker_x()
270                     f = f - 2 * x_s + 1
271                     x_s -= 1
272                     print("X动点坐标：", x_s + x_c, y_s + y_c)
273                     point_x.append(x_s + x_c)
274                     point_y.append(y_s + y_c)
275                 cnt -= 1
276         elif n < 0:
277             print("顺圆")
278             dir_x = 1
279             dir_y = 1
280             print("右上")
281             while cnt > 0:
282                 if f >= 0:
283                     puls_maker_x()
284                     f = f + 2 * x_s + 1
285                     x_s += 1
286                     print("X动点坐标：", x_s + x_c, y_s + y_c)
287                     point_x.append(x_s + x_c)
288                     point_y.append(y_s + y_c)
289                 else:
290                     puls_maker_y()
291                     f = f + 2 * y_s + 1
292                     y_s += 1
293                     print("X动点坐标：", x_s + x_c, y_s + y_c)
294                     point_x.append(x_s + x_c)
295                     point_y.append(y_s + y_c)
296                 cnt -= 1
297         else:
298             print("直线")
299     elif x2 < 0 and y2 < 0:
300         print("第三象限")
301         if n > 0:
302             print("逆圆")
303             dir_x = 1
304             dir_y = 0
305             print("右下")
306             while cnt > 0:
307                 if f >= 0:
308                     puls_maker_x()
309                     f = f + 2 * x_s + 1
310                     x_s += 1
311                     print("X动点坐标：", x_s + x_c, y_s + y_c)
312                     point_x.append(x_s + x_c)
313                     point_y.append(y_s + y_c)
314                 else:
315                     puls_maker_y()
316                     f = f - 2 * y_s + 1
317                     y_s -= 1
318                     print("Y动点坐标：", x_s + x_c, y_s + y_c)
319                     point_x.append(x_s + x_c)
320                     point_y.append(y_s + y_c)
321                 cnt -= 1
322         elif n < 0:
323             print("顺圆")
324             dir_x = 0
325             dir_y = 1
326             print("左上")
327             while cnt > 0:
328                 if f >= 0:
329                     puls_maker_y()
330                     f = f + 2 * y_s + 1
331                     y_s += 1
332                     print("Y动点坐标：", x_s + x_c, y_s + y_c)
333                     point_x.append(x_s + x_c)
334                     point_y.append(y_s + y_c)
335                 else:
336                     puls_maker_x()
337                     f = f - 2 * x_s + 1
338                     x_s -= 1
339                     print("X动点坐标：", x_s + x_c, y_s + y_c)
340                     point_x.append(x_s + x_c)
341                     point_y.append(y_s + y_c)
342                 cnt -= 1
343         else:
344             print("直线")
345     elif x2 > 0 > y2:
346         print("第四象限")
347         if n > 0:
348             print("逆圆")
349             dir_x = 1
350             dir_y = 1
351             print("右上")
352             while cnt > 0:
353                 if f >= 0:
354                     puls_maker_y()
355                     f = f + 2 * y_s + 1
356                     y_s += 1
357                     print("Y动点坐标：", x_s + x_c, y_s + y_c)
358                     point_x.append(x_s + x_c)
359                     point_y.append(y_s + y_c)
360                 else:
361                     puls_maker_x()
362                     f = f + 2 * x_s + 1
363                     x_s += 1
364                     print("X动点坐标：", x_s + x_c, y_s + y_c)
365                     point_x.append(x_s + x_c)
366                     point_y.append(y_s + y_c)
367                 cnt -= 1
368         elif n < 0:
369             print("顺圆")
370             dir_x = 0
371             dir_y = 0
372             print("左下")
373             while cnt > 0:
374                 if f >= 0:
375                     puls_maker_x()
376                     f = f - 2 * x_s + 1
377                     x_s -= 1
378                     print("X动点坐标：", x_s + x_c, y_s + y_c)
379                     point_x.append(x_s + x_c)
380                     point_y.append(y_s + y_c)
381                 else:
382                     puls_maker_y()
383                     f = f - 2 * y_s + 1
384                     y_s -= 1
385                     print("X动点坐标：", x_s + x_c, y_s + y_c)
386                     point_x.append(x_s + x_c)
387                     point_y.append(y_s + y_c)
388                 cnt -= 1
389         else:
390             print("直线")
391     Xs = x_s + x_c
392     Ys = y_s + y_c
393     print("实时坐标：", Xs / k, Ys / k)
394     print("插补结束")
395 
396 
397 def control():
398     """
399     手动控制电机
400     :return:
401     """
402     print("调试开始：")
403     while 1:
404         mode = input("请选择插补模式(直线插补：l,圆弧插补：c,自动：a)：")
405         if mode == "l":
406             point_1x = input("请输入直线终点横坐标X_l,-25<t<25：").strip()
407             if point_1x.replace("-", "").replace(".", "").isdigit():
408                 print(float(point_1x))
409                 point_1x = k * float(point_1x)
410             else:
411                 continue
412             point_1y = input("请输入直线终点纵坐标Y_l,-25<t<25：")
413             if point_1y.isdigit():
414                 point_1y = k * float(point_1y)
415             else:
416                 continue
417             line_interpolation(point_1x, point_1y)
418         elif mode == "c":
419             circle_x = input("请输入圆弧圆心横坐标X_r,-25<t<25：")
420             if circle_x.isdigit():
421                 circle_x = k * float(circle_x)
422             else:
423                 continue
424             circle_y = input("请输入圆弧圆心纵坐标Y_r,-25<t<25：")
425             if circle_y.isdigit():
426                 circle_y = k * float(circle_y)
427             else:
428                 continue
429             point_2x = input("请输入圆弧终点横坐标X_e,-25<t<25：")
430             if point_2x.isdigit():
431                 point_2x = k * float(point_2x)
432             else:
433                 continue
434             point_2y = input("请输入圆弧终点纵坐标Y_e,-25<t<25：")
435             if point_2y.isdigit():
436                 point_2y = k * float(point_2y)
437             else:
438                 continue
439             arc_interpolation(circle_x, circle_y, point_2x, point_2y)
440         elif mode == "a":
441             auto_control()
442         else:
443             print("输入错误")
444         # 画图
445         plot_point()
446 
447 
448 def auto_control():
449     """
450     自动控制
451     :return:
452     """
453     # 点集
454     point_set = []
455     # 读取点集TXT文件
456     f = open("point_set.txt", "r+", encoding="utf-8")
457     # 读取所有数据放入列表，，，，，费内存。。。
458     order_list = f.readlines()
459     # 列表转字符串
460     str_order = " ".join(order_list)
461     print("--", str_order)
462     # 去字符串 空格 和 \n 并返回列表
463     str_order = str_order.split()
464     print("===", str_order)
465     # 制作二级列表
466     for n in str_order:
467         point = n.split(",")
468         point_set.append(point)
469     print(point_set)
470     print(len(point_set))
471     f.close()
472     i = 0
473     while i < len(point_set):
474         intpla_type = point_set[i][0]
475         if intpla_type == "l":
476             line_interpolation(k * float(point_set[i][1]), k * float(point_set[i][2]))
477         elif intpla_type == "c":
478             arc_interpolation(k * float(point_set[i][1]), k * float(point_set[i][2]),
479                               k * float(point_set[i + 1][1]), k * float(point_set[i + 1][2]))
480             i += 1
481         else:
482             print("输入错误")
483         i += 1
484     # 画图
485     plot_point()
486 
487 
488 if __name__ == '__main__':
489    control()
View Code
 

2.结合树莓派3B+、两个42步进电机、两个步进电机驱动器（型号：锐特DM542）设置为1600细分、220转24V变压器、XY两轴移动平台 实际操作成功。

具体操作流程如下：

　　1.使用UG10.0在草图环境绘制轨迹曲线(由直线和圆弧组成)

　　2.开启端点、圆心、象限点捕捉器、配合“焊点向导”功能按路径顺序依次选择路径节点作为焊点，再导出焊点CSV文件以获得路径节点坐标(也是焊点坐标)，用EXCELL排序把点按选择先后的顺序排序，并删除其他无用信息，只保留ID，X，Y坐标，分别在圆弧插补段和直线插补段用"c"和“l”代替ID，最后改名为.txt文件。

　　3.将获得的点坐标txt文件传入树莓派

　　4.执行程序并选择"a"即可运行。

  1 #!usr/bin/env/ python
  2 # -*- coding:utf-8 -*-
  3 # Author: XiaoFeng
  4 import RPi.GPIO as GPIO
  5 import time
  6 import matplotlib.pyplot as plt
  7 import numpy as np
  8 import threading
  9 
 10 
 11 # 选区GPIO引脚（物理编号pin）
 12 PUL_X = 38
 13 DIR_X = 40
 14 PUL_Y = 35
 15 DIR_Y = 33
 16 C_X = 3     # 1 触碰 X轴接近开关
 17 C_Y = 15    # 1 触碰 Y轴接近开关
 18 # 取消异常报错
 19 GPIO.setwarnings(False)
 20 # 设置引脚编码方式位物理引脚
 21 GPIO.setmode(GPIO.BOARD)
 22 # 设置引脚为输出模式
 23 GPIO.setup(PUL_X, GPIO.OUT)
 24 GPIO.setup(DIR_X, GPIO.OUT)
 25 GPIO.setup(PUL_Y, GPIO.OUT)
 26 GPIO.setup(DIR_Y, GPIO.OUT)
 27 # 设置引脚为输入模式
 28 GPIO.setup(C_X, GPIO.IN)
 29 GPIO.setup(C_Y, GPIO.IN)
 30 
 31 # 方向
 32 dir_x = 1
 33 dir_y = 1
 34 # 脉冲频率
 35 delay_high = 0.00005
 36 delay_low = 0.00005
 37 # 脉冲数
 38 times = 1
 39 # 工动点
 40 Xs = 0
 41 Ys = 0
 42 # 点痕迹
 43 point_x = []
 44 point_y = []
 45 # 放大系数
 46 k = 3200
 47 # 限位标志
 48 init_x = 1
 49 init_y = 1
 50 #单边最大行程
 51 max_travel = 25
 52 
 53 
 54 def plot_point():
 55     """
 56     画图
 57     :return:
 58     """
 59     fig, ax = plt.subplots()
 60     x = np.divide(point_x, k)
 61     y = np.divide(point_y, k)
 62     ax.plot(x, y)  # 绘制曲线 y1
 63     ax.set(xlabel='Distance_X (mm)', ylabel='Distance_Y (mm)',
 64            title='The trail of the point')
 65     ax.grid()
 66     # Move the left and bottom spines to x = 0 and y = 0, respectively.
 67     ax.spines["left"].set_position(("data", 0))
 68     ax.spines["bottom"].set_position(("data", 0))
 69     # Hide the top and right spines.
 70     ax.spines["top"].set_visible(False)
 71     ax.spines["right"].set_visible(False)
 72     # 坐标轴上加箭头
 73     ax.plot(1, 0, ">k", transform=ax.get_yaxis_transform(), clip_on=False)
 74     ax.plot(0, 1, "^k", transform=ax.get_xaxis_transform(), clip_on=False)
 75     plt.show()
 76 
 77 
 78 def puls_maker_x():
 79     """
 80     产生脉冲
 81     :return:
 82     """
 83     # print("xxxxxxx")
 84     GPIO.output(DIR_X, dir_x)
 85     for i in range(times):
 86         GPIO.output(PUL_X, 0)
 87         time.sleep(delay_low)
 88         GPIO.output(PUL_X, 1)
 89         time.sleep(delay_high)
 90 
 91 
 92 
 93 def puls_maker_y():
 94     """
 95     产生脉冲
 96     :return:
 97     """
 98     # print("yyyyyy")
 99     GPIO.output(DIR_Y, dir_y)
100     for i in range(times):
101         GPIO.output(PUL_Y, 0)
102         time.sleep(delay_low)
103         GPIO.output(PUL_Y, 1)
104         time.sleep(delay_high)
105 
106 
107 def line_interpolation(x_e, y_e):
108     """
109     直线插补
110     :param x_e: 目标X
111     :param y_e: 目标Y
112     :return:
113     """
114     global Xs, Ys
115     global dir_x, dir_y
116     global point_x, point_y
117     f_line = 0
118     x = 0
119     y = 0
120     delta_x = x_e - Xs
121     delta_y = y_e - Ys
122     cnt = abs(delta_x) + abs(delta_y)
123     if delta_x > 0:
124         dir_x = 1
125         x = 1
126        # print("右")
127     elif delta_x < 0:
128         dir_x = 0
129         x = -1
130        # print("左")
131     elif delta_x == 0 and delta_y > 0:
132         dir_y = 1
133         y = 1
134        # print("上")
135         while cnt > 0:
136             puls_maker_y()
137             Ys += y
138             point_x.append(Xs)
139             point_y.append(Ys)
140           #  print("X动点坐标：", Xs, Ys)
141             cnt -= 1
142     elif delta_x == 0 and delta_y < 0:
143         dir_y = 0
144         y = -1
145        # print("下")
146         while cnt > 0:
147             puls_maker_y()
148             Ys += y
149             point_x.append(Xs)
150             point_y.append(Ys)
151          #   print("X动点坐标：", Xs, Ys)
152             cnt -= 1
153     if delta_y > 0:
154         dir_y = 1
155         y = 1
156       #  print("上")
157     elif delta_y < 0:
158         dir_y = 0
159         y = -1
160        # print("下")
161     elif delta_y == 0 and delta_x > 0:
162         dir_x = 1
163         x = 1
164       #  print("右")
165         while cnt > 0:
166             puls_maker_x()
167             Xs += x
168             point_x.append(Xs)
169             point_y.append(Ys)
170         #    print("X动点坐标：", Xs, Ys)
171             cnt -= 1
172     elif delta_y == 0 and delta_x < 0:
173         dir_x = 0
174         x = -1
175        # print("左")
176         while cnt > 0:
177             puls_maker_x()
178             Xs += x
179             point_x.append(Xs)
180             point_y.append(Ys)
181            # print("X动点坐标：", Xs, Ys)
182             cnt -= 1
183     while cnt > 0 and (init_x or init_y):
184         if f_line >= 0:
185             if init_x:
186                 puls_maker_x()
187             f_line -= abs(delta_y)
188             Xs += x
189             point_x.append(Xs)
190             point_y.append(Ys)
191          #   print("X动点坐标：", Xs, Ys)
192         else:
193             if init_y:
194                 puls_maker_y()
195             f_line += abs(delta_x)
196             Ys += y
197             point_x.append(Xs)
198             point_y.append(Ys)
199          #   print("Y动点坐标：", Xs, Ys)
200         cnt -= 1
201     Xs = x_e
202     Ys = y_e
203     print("实时坐标：", Xs / k, Ys / k)
204     print("直线插补结束")
205     # 画图
206     plot_point()
207 
208 
209 def arc_interpolation(x_c, y_c, xe, ye):
210     """
211     圆弧插补
212     :param x_c: 绝对弧心X
213     :param y_c: 绝对弧心Y
214     :param xe: 绝对目标X
215     :param ye: 绝对目标Y
216     :return:
217     """
218     global Xs, Ys
219     global dir_x, dir_y
220     global point_x, point_y
221     # 绝对坐标(xs,ys),(xe,ye)转相对坐标(x_s,y_s),(x_e,y_e)
222     x_s = Xs - x_c
223     y_s = Ys - y_c
224     x_e = xe - x_c
225     y_e = ye - y_c
226     f = 0
227     cnt = abs(x_e - x_s) + abs(y_e - y_s)
228     x2 = (x_s + x_e) / 2
229     y2 = (y_s + y_e) / 2
230     # 判断顺弧还是逆弧
231     n = x_s * (y_e - y_s) - (x_e - x_s) * y_s
232     # 判断象限
233     if x2 > 0 and y2 > 0:
234         #print("第一象限")
235         if n > 0:
236           #  print("逆圆")
237             dir_x = 0
238             dir_y = 1
239            # print("左上")
240             while cnt > 0:
241                 if f >= 0:
242                     puls_maker_x()
243                     f = f - 2 * x_s + 1
244                     x_s -= 1
245                    # print("X动点坐标：", x_s + x_c, y_s + y_c)
246                     point_x.append(x_s + x_c)
247                     point_y.append(y_s + y_c)
248                 else:
249                     puls_maker_y()
250                     f = f + 2 * y_s + 1
251                     y_s += 1
252                   #  print("Y动点坐标：", x_s + x_c, y_s + y_c)
253                     point_x.append(x_s + x_c)
254                     point_y.append(y_s + y_c)
255                 cnt -= 1
256         elif n < 0:
257          #   print("顺圆")
258             dir_x = 1
259             dir_y = 0
260           #  print("右下")
261             while cnt > 0:
262                 if f >= 0:
263                     puls_maker_y()
264                     f = f - 2 * y_s + 1
265                     y_s -= 1
266                 #    print("Y动点坐标：", x_s + x_c, y_s + y_c)
267                     point_x.append(x_s + x_c)
268                     point_y.append(y_s + y_c)
269                 else:
270                     puls_maker_x()
271                     f = f + 2 * x_s + 1
272                     x_s += 1
273                  #   print("X动点坐标：", x_s + x_c, y_s + y_c)
274                     point_x.append(x_s + x_c)
275                     point_y.append(y_s + y_c)
276                 cnt -= 1
277         else:
278             print("直线")
279     elif y2 > x2 < 0:
280      #   print("第二象限")
281         if n > 0:
282          #   print("逆圆")
283             dir_x = 0
284             dir_y = 0
285          #   print("左下")
286             while cnt > 0:
287                 if f >= 0:
288                     puls_maker_y()
289                     f = f - 2 * y_s + 1
290                     y_s -= 1
291                  #   print("Y动点坐标：", x_s + x_c, y_s + y_c)
292                     point_x.append(x_s + x_c)
293                     point_y.append(y_s + y_c)
294                 else:
295                     puls_maker_x()
296                     f = f - 2 * x_s + 1
297                     x_s -= 1
298                  #   print("X动点坐标：", x_s + x_c, y_s + y_c)
299                     point_x.append(x_s + x_c)
300                     point_y.append(y_s + y_c)
301                 cnt -= 1
302         elif n < 0:
303           #  print("顺圆")
304             dir_x = 1
305             dir_y = 1
306           #  print("右上")
307             while cnt > 0:
308                 if f >= 0:
309                     puls_maker_x()
310                     f = f + 2 * x_s + 1
311                     x_s += 1
312                  #   print("X动点坐标：", x_s + x_c, y_s + y_c)
313                     point_x.append(x_s + x_c)
314                     point_y.append(y_s + y_c)
315                 else:
316                     puls_maker_y()
317                     f = f + 2 * y_s + 1
318                     y_s += 1
319                   #  print("X动点坐标：", x_s + x_c, y_s + y_c)
320                     point_x.append(x_s + x_c)
321                     point_y.append(y_s + y_c)
322                 cnt -= 1
323         else:
324             print("直线")
325     elif x2 < 0 and y2 < 0:
326        # print("第三象限")
327         if n > 0:
328           #  print("逆圆")
329             dir_x = 1
330             dir_y = 0
331            # print("右下")
332             while cnt > 0:
333                 if f >= 0:
334                     puls_maker_x()
335                     f = f + 2 * x_s + 1
336                     x_s += 1
337                 #    print("X动点坐标：", x_s + x_c, y_s + y_c)
338                     point_x.append(x_s + x_c)
339                     point_y.append(y_s + y_c)
340                 else:
341                     puls_maker_y()
342                     f = f - 2 * y_s + 1
343                     y_s -= 1
344                  #   print("Y动点坐标：", x_s + x_c, y_s + y_c)
345                     point_x.append(x_s + x_c)
346                     point_y.append(y_s + y_c)
347                 cnt -= 1
348         elif n < 0:
349          #   print("顺圆")
350             dir_x = 0
351             dir_y = 1
352          #   print("左上")
353             while cnt > 0:
354                 if f >= 0:
355                     puls_maker_y()
356                     f = f + 2 * y_s + 1
357                     y_s += 1
358                 #    print("Y动点坐标：", x_s + x_c, y_s + y_c)
359                     point_x.append(x_s + x_c)
360                     point_y.append(y_s + y_c)
361                 else:
362                     puls_maker_x()
363                     f = f - 2 * x_s + 1
364                     x_s -= 1
365                 #    print("X动点坐标：", x_s + x_c, y_s + y_c)
366                     point_x.append(x_s + x_c)
367                     point_y.append(y_s + y_c)
368                 cnt -= 1
369         else:
370             print("直线")
371     elif x2 > 0 > y2:
372       #  print("第四象限")
373         if n > 0:
374           #  print("逆圆")
375             dir_x = 1
376             dir_y = 1
377           #  print("右上")
378             while cnt > 0:
379                 if f >= 0:
380                     puls_maker_y()
381                     f = f + 2 * y_s + 1
382                     y_s += 1
383                   #  print("Y动点坐标：", x_s + x_c, y_s + y_c)
384                     point_x.append(x_s + x_c)
385                     point_y.append(y_s + y_c)
386                 else:
387                     puls_maker_x()
388                     f = f + 2 * x_s + 1
389                     x_s += 1
390                   #  print("X动点坐标：", x_s + x_c, y_s + y_c)
391                     point_x.append(x_s + x_c)
392                     point_y.append(y_s + y_c)
393                 cnt -= 1
394         elif n < 0:
395            # print("顺圆")
396             dir_x = 0
397             dir_y = 0
398            # print("左下")
399             while cnt > 0:
400                 if f >= 0:
401                     puls_maker_x()
402                     f = f - 2 * x_s + 1
403                     x_s -= 1
404                  #   print("X动点坐标：", x_s + x_c, y_s + y_c)
405                     point_x.append(x_s + x_c)
406                     point_y.append(y_s + y_c)
407                 else:
408                     puls_maker_y()
409                     f = f - 2 * y_s + 1
410                     y_s -= 1
411                 #    print("X动点坐标：", x_s + x_c, y_s + y_c)
412                     point_x.append(x_s + x_c)
413                     point_y.append(y_s + y_c)
414                 cnt -= 1
415         else:
416             print("直线")
417     Xs = x_s + x_c
418     Ys = y_s + y_c
419     print("实时坐标：", Xs / k, Ys / k)
420     print("圆弧插补结束")
421     # 画图
422     plot_point()
423 
424 
425 def auto_control():
426     """
427     自动控制
428     :return:
429     """
430     # 点集
431     point_set = []
432     # 读取点集TXT文件
433     f = open("point_set.txt", "r+", encoding="utf-8")
434     # 读取所有数据放入列表，，，，，费内存。。。
435     order_list = f.readlines()
436     # 列表转字符串
437     str_order = " ".join(order_list)
438     #print("--", str_order)
439     # 去字符串 空格 和 \n 并返回列表
440     str_order = str_order.split()
441     #print("===", str_order)
442     # 制作二级列表
443     for n in str_order:
444         point = n.split(",")
445         point_set.append(point)
446     print(point_set)
447     #print(len(point_set))
448     f.close()
449     i = 0
450     while i < len(point_set):
451         intpla_type = point_set[i][0]
452         if intpla_type == "l":
453             line_interpolation(k * float(point_set[i][1]), k * float(point_set[i][2]))
454         elif intpla_type == "c":
455             arc_interpolation(k * float(point_set[i][1]), k * float(point_set[i][2]),
456                               k * float(point_set[i + 1][1]), k * float(point_set[i + 1][2]))
457             i += 1
458         else:
459             print("输入错误")
460         i += 1
461     # 画图
462     plot_point()
463 
464 
465 def speed_control():
466     """
467     位移速度调节 mm/s
468     """
469     global delay_high, delay_low
470     v = input("请输入移动速度(默认单轴全速3.125mm/s):")
471     if v.replace(".", "").isdigit():
472         t = 1 / (2 * k * float(v))
473         delay_high = t
474         delay_low = t
475         print("时间：", t, delay_high)
476     else:
477         print("请输入数字")
478 
479 
480 def x_callback(C_X):
481     global init_x
482     GPIO.remove_event_detect(C_X)
483     print("X限位")
484     init_x = 0
485 
486 
487 def y_callback(C_Y):
488     global init_y
489     GPIO.remove_event_detect(C_Y)
490     print("Y限位")
491     init_y = 0
492     
493 
494 def init_x_y():
495     """
496     利用限位初始化
497     """
498     global init_x, init_y
499     global Xs, Ys
500     global point_x, point_y
501     # 上升沿检测
502     GPIO.add_event_detect(C_X, GPIO.RISING, callback=x_callback)
503     GPIO.add_event_detect(C_Y, GPIO.RISING, callback=y_callback)
504     line_interpolation(-60 * k, -60* k)
505     init_x = 1
506     init_y = 1
507     Xs = 0
508     Ys = 0
509     line_interpolation(k * 24.20, k * 24.97)
510     Xs = 0
511     Ys = 0
512     point_x.clear()
513     point_y.clear()
514     
515 
516 def control():
517     """
518     手动控制电机
519     :return:
520     """
521     global point_x, point_y
522     print("****************调试开始****************")
523     while 1:
524         mode = input("--------------功能列表--------------\n自动原点：>>>>>>I\n直线插补：>>>>>>L\n圆弧插补：>>>>>>C\n自动轨迹：>>>>>>A\n速度调节：>>>>>>V\n清理画布：>>>>>>K\n请选择控制模式:")
525         mode = mode.lower()
526         if mode == "l":
527             point_1x = input("请输入直线终点横坐标X_l：").strip()
528             if point_1x.replace(".", "").replace("-", "").isdigit():
529                 if abs(float(point_1x)) <= max_travel:
530                     point_1x = k * float(point_1x)
531                 else:
532                     print("超出行程(单边<=%f)" % max_travel)
533                     continue
534             else:
535                 print("请输入数字")
536                 continue
537             point_1y = input("请输入直线终点纵坐标Y_l：")
538             if point_1y.replace(".", "").replace("-", "").isdigit():
539                 if abs(float(point_1y)) <= max_travel:
540                     point_1y = k * float(point_1y)
541                 else:
542                     print("超出行程(单边<=%f)" % max_travel)
543                     continue
544             else:
545                 print("请输入数字")
546                 continue
547             line_interpolation(point_1x, point_1y)
548         elif mode == "c":
549             circle_x = input("请输入圆弧圆心横坐标X_r：")
550             if circle_x.replace(".", "").replace("-", "").isdigit():
551                 if abs(float(circle_x)) <= max_travel:
552                     circle_x = k * float(circle_x)
553                 else:
554                     print("超出行程(单边<=%f)" % max_travel)
555                     continue
556             else:
557                 print("请输入数字")
558                 continue
559             circle_y = input("请输入圆弧圆心纵坐标Y_r：")
560             if circle_y.replace(".", "").replace("-", "").isdigit():
561                 if abs(float(circle_y)) <= max_travel:
562                     circle_y = k * float(circle_y)
563                 else:
564                     print("超出行程(单边<=%f)" % max_travel)
565                     continue
566             else:
567                 print("请输入数字")
568                 continue
569             point_2x = input("请输入圆弧终点横坐标X_e：")
570             if point_2x.replace(".", "").replace("-", "").isdigit():
571                 if abs(float(point_2x)) <= max_travel:
572                     point_2x = k * float(point_2x)
573                 else:
574                     print("超出行程(单边<=%f)" % max_travel)
575                     continue
576             else:
577                 print("请输入数字")
578                 continue
579             point_2y = input("请输入圆弧终点纵坐标Y_e：")
580             if point_2y.replace(".", "").replace("-", "").isdigit():
581                 if abs(float(point_2y)) <= max_travel:
582                     point_2y = k * float(point_2y)
583                 else:
584                     print("超出行程(单边<=%f)" % max_travel)
585                     continue
586             else:
587                 print("请输入数字")
588                 continue
589             arc_interpolation(circle_x, circle_y, point_2x, point_2y)
590         elif mode == "a":
591             auto_control()
592         elif mode == "v":
593             speed_control()
594         elif mode == "i":
595             init_x_y()
596         elif mode == "k":
597             point_x.clear()
598             point_y.clear()
599         else:
600             print("输入错误")
601 
602 
603 if __name__ == '__main__':
604     control()

 

posted @ 2020-08-24 14:36  晓风小峰  阅读(1140)  评论(0)  编辑  收藏  举报