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 # 去字符串 空格 和 并返回列表 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()
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 # 去字符串 空格 和 并返回列表 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("--------------功能列表-------------- 自动原点：>>>>>>I 直线插补：>>>>>>L 圆弧插补：>>>>>>C 自动轨迹：>>>>>>A 速度调节：>>>>>>V 清理画布：>>>>>>K 请选择控制模式:") 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()