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

1 #!usr/bin/env/ python

2 #-*- coding:utf-8 -*-

3 #Author: XiaoFeng

4 importRPi.GPIO as GPIO5 importtime6 importmatplotlib.pyplot as plt7 importnumpy as np8 importthreading9

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 =041 Ys =042 #点痕迹

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 defplot_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 defpuls_maker_x():79 """

80 产生脉冲81 :return:82 """

83 #print("xxxxxxx")

84 GPIO.output(DIR_X, dir_x)85 for i inrange(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 defpuls_maker_y():94 """

95 产生脉冲96 :return:97 """

98 #print("yyyyyy")

99 GPIO.output(DIR_Y, dir_y)100 for i inrange(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 defline_interpolation(x_e, y_e):108 """

109 直线插补110 :param x_e: 目标X111 :param y_e: 目标Y112 :return:113 """

114 globalXs, Ys115 globaldir_x, dir_y116 globalpoint_x, point_y117 f_line =0118 x =0119 y =0120 delta_x = x_e -Xs121 delta_y = y_e -Ys122 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 =0129 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 +=y138 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 =0144 y = -1

145 #print("下")

146 while cnt >0:147 puls_maker_y()148 Ys +=y149 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 =0159 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 +=x168 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 =0174 x = -1

175 #print("左")

176 while cnt >0:177 puls_maker_x()178 Xs +=x179 point_x.append(Xs)180 point_y.append(Ys)181 #print("X动点坐标：", Xs, Ys)

182 cnt -= 1

183 while cnt > 0 and (init_x orinit_y):184 if f_line >=0:185 ifinit_x:186 puls_maker_x()187 f_line -=abs(delta_y)188 Xs +=x189 point_x.append(Xs)190 point_y.append(Ys)191 #print("X动点坐标：", Xs, Ys)

192 else:193 ifinit_y:194 puls_maker_y()195 f_line +=abs(delta_x)196 Ys +=y197 point_x.append(Xs)198 point_y.append(Ys)199 #print("Y动点坐标：", Xs, Ys)

200 cnt -= 1

201 Xs =x_e202 Ys =y_e203 print("实时坐标：", Xs / k, Ys /k)204 print("直线插补结束")205 #画图

206 plot_point()207

208

209 defarc_interpolation(x_c, y_c, xe, ye):210 """

211 圆弧插补212 :param x_c: 绝对弧心X213 :param y_c: 绝对弧心Y214 :param xe: 绝对目标X215 :param ye: 绝对目标Y216 :return:217 """

218 globalXs, Ys219 globaldir_x, dir_y220 globalpoint_x, point_y221 #绝对坐标(xs,ys),(xe,ye)转相对坐标(x_s,y_s),(x_e,y_e)

222 x_s = Xs -x_c223 y_s = Ys -y_c224 x_e = xe -x_c225 y_e = ye -y_c226 f =0227 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_s232 #判断象限

233 if x2 > 0 and y2 >0:234 #print("第一象限")

235 if n >0:236 #print("逆圆")

237 dir_x =0238 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 =0260 #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 =0284 dir_y =0285 #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 =0331 #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 =0351 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 =0397 dir_y =0398 #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_c418 Ys = y_s +y_c419 print("实时坐标：", Xs / k, Ys /k)420 print("圆弧插补结束")421 #画图

422 plot_point()423

424

425 defauto_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 instr_order:444 point = n.split(",")445 point_set.append(point)446 print(point_set)447 #print(len(point_set))

448 f.close()449 i =0450 while i

458 else:459 print("输入错误")460 i += 1

461 #画图

462 plot_point()463

464

465 defspeed_control():466 """

467 位移速度调节 mm/s468 """

469 globaldelay_high, delay_low470 v = input("请输入移动速度(默认单轴全速3.125mm/s):")471 if v.replace(".", "").isdigit():472 t = 1 / (2 * k *float(v))473 delay_high =t474 delay_low =t475 print("时间：", t, delay_high)476 else:477 print("请输入数字")478

479

480 defx_callback(C_X):481 globalinit_x482 GPIO.remove_event_detect(C_X)483 print("X限位")484 init_x =0485

486

487 defy_callback(C_Y):488 globalinit_y489 GPIO.remove_event_detect(C_Y)490 print("Y限位")491 init_y =0492

493

494 definit_x_y():495 """

496 利用限位初始化497 """

498 globalinit_x, init_y499 globalXs, Ys500 globalpoint_x, point_y501 #上升沿检测

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 =0508 Ys =0509 line_interpolation(k * 24.20, k * 24.97)510 Xs =0511 Ys =0512 point_x.clear()513 point_y.clear()514

515

516 defcontrol():517 """

518 手动控制电机519 :return:520 """

521 globalpoint_x, point_y522 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()