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()
本文章为转载内容，我们尊重原作者对文章享有的著作权。如有内容错误或侵权问题，欢迎原作者联系我们进行内容更正或删除文章。
