运动学原理
进行几何逆求解
速度控制原理
梯形加速
插补思路
直线插补
注意细分步进精度，理论上越高越好，但需要根据性能和应用场景来
由于本文已完成由点逆求解出点和各轴角度的相对应关系，故细分至空间直线的各个点位置即可
圆弧插补
圆弧插补分为顺时针圆弧插补和逆时针圆弧插补，以适应不同的插补过程。同时应考虑尾数误差导致的圆弧死循环。
//已知两点和圆心求圆方程
void circle_r_XZ(float cir_x2,float cir_z2,float cir_x0,float cir_z0,uint8_t Arc_direction)
{
float cir_xi,cir_zi,cir_r,cir_r1,cir_end_x2 = 1;
float cir_x1 = x0,cir_z1 = z0;
float cir_step = 0.05,cir_error_margin; //步进，允许误差
uint16_t cir_step_count ; //步数
cir_error_margin = cir_step*0.9;
cir_x0 = x0+cir_x0;
cir_z0 = z0+cir_z0;
cir_r = sqrt((cir_x1- cir_x0) * (cir_x1- cir_x0) + (cir_z1- cir_z0) * (cir_z1- cir_z0) );
cir_r1 = sqrt((cir_x2- cir_x0) * (cir_x2- cir_x0) + (cir_z2- cir_z0) * (cir_z2- cir_z0) );
if(cir_r >= cir_r1 + 0.15 || cir_r <= cir_r1 - 0.15 )
{
USART1_SendNByte("it cant build the arc", 21);
}
else{
cir_xi = cir_x1;
cir_zi = cir_z1;
if(Arc_direction == Clockwise) //顺圆运动
{
do{
if(cir_zi >= cir_z0 && cir_xi > cir_x0) //第一象限
{
cir_zi = cir_zi + cir_step;
cir_xi = sqrt(cir_r*cir_r-(cir_zi - cir_z0)*(cir_zi - cir_z0))+ cir_x0;
}
else if(cir_zi < cir_z0 && cir_xi >= cir_x0) //第二象限
{
cir_zi = cir_zi + cir_step;
cir_xi = sqrt(cir_r*cir_r-(cir_zi - cir_z0)*(cir_zi - cir_z0))+ cir_x0;
}
else if(cir_zi <= cir_z0 && cir_xi < cir_x0) //第三象限
{
cir_zi = cir_zi - cir_step;
cir_xi =cir_x0- sqrt(cir_r*cir_r-(cir_zi - cir_z0)*(cir_zi - cir_z0)) ;
}
else if(cir_zi > cir_z0 && cir_xi <= cir_x0) //第四象限
{
cir_zi = cir_zi - cir_step;
cir_xi =cir_x0- sqrt(cir_r*cir_r-(cir_zi - cir_z0)*(cir_zi - cir_z0)) ;
}
Point_Move(cir_xi,y0,cir_zi);
if(fabs(cir_zi - cir_z2) < cir_step*2)
{
Point_Move(cir_x2,y0,cir_z2);
cir_end_x2 = 0;
}
if(Stop_flag == 1 || edge_flag == 1 || jerk_flag == 1) break;
}while(cir_end_x2 != 0);
}
else if(Arc_direction == Counterclockwise) //逆圆运动
{
while(cir_end_x2 != 0 ){
if(cir_zi > cir_z0 && cir_xi >= cir_x0) //第一象限
{
cir_zi = cir_zi - cir_step;
cir_xi = sqrt(cir_r*cir_r-(cir_zi - cir_z0)*(cir_zi - cir_z0))+ cir_x0;
}
else if(cir_zi <= cir_z0 && cir_xi > cir_x0) //第二象限
{
cir_zi = cir_zi - cir_step;
cir_xi = sqrt(cir_r*cir_r-(cir_zi - cir_z0)*(cir_zi - cir_z0))+ cir_x0;
}
else if(cir_zi < cir_z0 && cir_xi <= cir_x0) //第三象限
{
cir_zi = cir_zi + cir_step;
cir_xi =cir_x0- sqrt(cir_r*cir_r-(cir_zi - cir_z0)*(cir_zi - cir_z0)) ;
}
else if(cir_zi >= cir_z0 && cir_xi < cir_x0) //第四象限
{
cir_zi = cir_zi + cir_step;
cir_xi =cir_x0- sqrt(cir_r*cir_r-(cir_zi - cir_z0)*(cir_zi - cir_z0)) ;
}
Point_Move(cir_xi,y0,cir_zi);
if(fabs(cir_zi - cir_z2) < cir_step+0.05)
{
Point_Move(cir_x2,y0,cir_z2);
cir_end_x2 = 0;
}
if(Stop_flag == 1 || edge_flag == 1 || jerk_flag == 1) break;
}
}
}
}
PCB设计思路
电机模块
电机我选用的是42步进电机，电机驱动模块我买的是A4988，仅需要控制A4988模块的step、dir、enable、MS1/2/3即可驱动电机运动。
step：接收单片机脉冲，直接决定电机运转速度
dir：决定电机转动方向
enable：控制模块开关，低电平有效
MS1/2/3：控制电机的细分数，可参考产品说明设置
磁编码器 AS5600模块
AS5600为磁编码器，可读取电机转动的角度，可通过IIC、脉冲计数等方式通讯，我选用的是通过IIC进行通讯
IIC通讯主要控制SCL、SDA两个通道
AS5600主要接口VCC、GND、DIR、SCL、SDA
VCC：电源接口
GND：接地
DIR：控制磁编码器读数方向
SCL：连接单片机SCL
SDA：连接单片机SDA
电源变压模块
需要注意电容的方向，具体电容选取需参考电压大小
串口通讯模块
此处为DDL转232设计电路。注意tx，rx，以及232公母口差异
上位机程序
G code解译
G code解译本质就是做文本解析。需要做的就是将G code转化为下位机自身的通讯协议。我主要用了相关的文件处理和正则表达。
正则表达式参考博客：正则表达
#上位机用的库文件
import numpy as np
import re
import tkinter as tk
from tkinter import ttk
from tkinter.filedialog import *
import os
import serial
import serial.tools.list_ports
import threading
import time
import copy
def Readpath(self): #G_code转译
global save_path
filepath = askopenfilename(title = '选择轨迹文件',
filetypes=[('G_code', '*.ngc'),('所有文件','*')])
# 选择打开什么文件，返回文件名
filename.set(filepath) #tk显示
(foldname, name) = os.path.split(filepath) #将文件名和文件夹路径分割
(subfilename, exe) = os.path.splitext(name) #将文件名前后缀分割
exe = '.txt'
save_path = foldname + '/' + subfilename + exe #新增txt文件
print(save_path)
f1 = open(filepath, 'r') # 打开选择的文件，只读
data1 = f1.readlines() # 获取内部数据
f1.close() # 关闭文件，减少内存
i = 0
with open(save_path, "w") as f: #打开新建的txt文件，写入
f.write('(point:0,30,0)\r')
for i in range(len(data1)): #提取每一行数据
data2 = data1[i]
if (data2.find("G01") != -1 or data2.find("G1") != -1): #解译G01
X = re.findall(r"X(.+?) Y", data2) #获取X到 Y直接的数据
Z = re.findall(r"Y(.+?) Z", data2)
if (data2.find("F") != -1):
Y = re.findall(r"Z(.+?) F", data2)
else:
Y = re.findall(r"Z(.+?)$", data2)
if (data2.find("X") != -1 and data2.find("Y") != -1):
f.write('(point:{:},{:},{:})\r'.format(X, Y, Z)) #转移后写入txt文件中
elif (data2.find("Z") != -1):
f.write('(tidao:{:})\r'.format(Y))
elif (data2.find("G02") != -1):
X = re.findall(r"X(.+?) Y", data2)
Z = re.findall(r"Y(.+?) Z", data2)
X0 = re.findall(r"I(.+?) J", data2)
if (data2.find("F") != -1):
Z0 = re.findall(r"J(.+?) F", data2)
else:
Z0 = re.findall(r"J(.+?)$", data2)
f.write('(XZFarc:{:},{:},{:},{:})\r'.format(X, Z, X0, Z0))
elif (data2.find("G03") != -1):
X = re.findall(r"X(.+?) Y", data2)
Z = re.findall(r"Y(.+?) Z", data2)
X0 = re.findall(r"I(.+?) J", data2)
if (data2.find("F") != -1):
Z0 = re.findall(r"J(.+?) F", data2)
else:
Z0 = re.findall(r"J(.+?)$", data2)
f.write('(XZCarc:{:},{:},{:},{:})\r'.format(X, Z, X0, Z0))
elif (data2.find("G00") != -1):
X = re.findall(r"X(.+?) Y", data2)
Z = re.findall(r"Y(.+?)$", data2)
if(data2.find("Z") != -1):
Y = re.findall(r"Z(.+?)$", data2)
G0_Y = Y
G0_X = X
G0_Z = Z
if (data2.find("X") != -1 and data2.find("Y") != -1):
f.write('(point:{:},{:},{:})\r'.format(X,G0_Y,Z))
elif (data2.find("Z") != -1):
f.write('(tidao:{:})\r'.format(Y))
i = i + 1
f.write('(ResetZero)\r')
f.close()
# 去掉'
f4 = open(save_path, 'r')
data3 = f4.read()
f4 = data3.replace("'", '')
f5 = open(save_path, 'w')
f5.write(f4)
f5.close()
# 去掉[
f4 = open(save_path, 'r')
data3 = f4.read()
f4 = data3.replace("[", '')
f5 = open(save_path, 'w')
f5.write(f4)
f5.close()
# 去掉]
f4 = open(save_path, 'r')
data3 = f4.read()
f4 = data3.replace("]", '')
f5 = open(save_path, 'w')
f5.write(f4)
f5.close()
# 替换0.125
f4 = open(save_path, 'r')
data3 = f4.read()
f4 = data3.replace("0.125", '3.0')
f5 = open(save_path, 'w')
f5.write(f4)
f5.close()
串口
class SerialPort:
def __init__(self,port,buand):
global message
self.port = serial.Serial(port,buand)
self.port.close()
if not self.port.isOpen():
self.port.open()
def port_open(self): #打开串口
if not self.port.isOpen():
self.port.open()
def port_close(self): #关闭串口
self.port.close()
print("port have close")
def send(self,data): #常规串口发送
self.port_open()
self.port.write(data.encode()) #编码发送
print('send:', data)
count = self.port.inWaiting() #串口接收
time.sleep(0) #等待通常在此时
rec = self.port.read(count)
while (rec.decode().find('ok') == -1): #自行决定是否永久等待‘ok’信号
count = self.port.inWaiting()
rec11 = self.port.read(count)
rec = rec + rec11
print('receive:', rec.decode()) #解码
def updata_com(): #检测串口更新
global port_change
global old_port_list_name
global port_list_name
old_port_list_name = copy.copy(port_list_name) #浅复制
port_list_name.clear()
port_list = list(serial.tools.list_ports.comports())
for itms in port_list:
port_list_name.append(itms.device)
if(len(old_port_list_name) == len(port_list_name)):
port_change = 0
elif(len(old_port_list_name) == 0 and len(port_list_name) == 1):
print(port_list_name)
port_change = 1
root.quit()
elif(1):
print(port_list_name)
port_change = 1
# root.quit()
print('窗口刷新')
comboxlist["values"] = port_list_name #更新tk下拉框
if(len(port_list_name)):
mSerial_w.port_close()
root.after(10,updata_com) #每10ms进入一次该程序
可视化窗口控制
我使用的是python的tkinter库，也可以使用pyqt，原理都差不多。
tk参考博客：tk
tkinter Canvas参考博客：Canvas教程
root = tk.Tk()
#文本框
Setup_data_L1length = tk.Entry(root,textvariable=L1length,show = None) #文本框
Setup_data_L1length.grid(row=4, column=1, padx=3, pady=3)
num = Setup_data_L1length.get() #获取文本框内容
#下拉框
comboxlist = ttk.Combobox(root,textvariable = comvalue,state = "readonly") #下拉框
comboxlist.grid(row=1, column=1, padx=3, pady=3)
comboxlist["values"] = port_list_name
comboxlist.current(0) # 选择第一个
comboxlist.bind("<<ComboboxSelected>>",mSerial_w.Select_com) # 下拉列表框被选中时，绑定mSerial_w.Select_com()函数)
Start_Button = tk.Button(root, text='开始', command=mSerial_w.Start) #按钮，command为激活函数
Start_Button.grid(row=4, column=3, padx=20, pady=5)
Start_Button['text'] = '停止' #修改按钮内容
#标签
tk.Label(root, text='选择文件').grid(row=2, column=0, padx=5, pady=5) #普通文本标签
pic = tk.Label(root) #显示图片
pic.grid(row=1, column=4, padx=5, pady=5,rowspan = 50)
img = Image.open('C:/ori_pic.png') #加载初始图片
im = ImageTk.PhotoImage(img.resize((640,480)))
pic['image'] = im
img_open = Image.open(path) #PIL打开图片，cv打开2需格式
img13 = ImageTk.PhotoImage(img_open)
pic.config(image = img13) #更新图片
pic.image = img13 #两行，缺一不可
#滑块
var = DoubleVar()
scl = Scale(root,orient =HORIZONTAL,length=300, #滑块
from_=60.0,to=150.0,tickinterval=15,resolution=1,variable=var)
#横向摆放，长度300像素，最小60，最大150，坐标轴15一刻度，最小单位为1，var为滑块数据
scl.grid(row=2, column=1, padx=5, pady=5,rowspan = 1,columnspan = 3)
scl.bind('<ButtonRelease-1>',mSerial_w.Change_speed) #拖动松开鼠标生效
print(var.get()) #获取滑块数据
#键盘激活
root.bind_all('<KeyPress-Up>',mSerial_w.moveRect) #键盘管理函数
root.bind_all('<KeyPress-Down>', mSerial_w.moveRect) #前者为激活条件，后者为响应函数
root.bind_all('<KeyPress-Left>', mSerial_w.moveRect)
root.bind_all('<KeyPress-Right>', mSerial_w.moveRect)
root.bind_all("w", mSerial_w.moveRect)
root.bind_all("s", mSerial_w.moveRect)
def moveRect(self,event): #示教模式
if event.keysym == 'Up': # 当你按下向上键的事件时
data = "(xfrun)"
print('send:', data)
self.port.write(data.encode())
elif event.keysym == 'Down':
data = "(xcrun)"
print('send:', data)
self.port.write(data.encode())
elif event.keysym == 'Left':
data = "(zcrun)"
print('send:', data)
self.port.write(data.encode())
elif event.keysym == 'Right':
data = "(zfrun)"
print('send:', data)
self.port.write(data.encode())
elif event.keysym == 'w':
data = "(yfrun)"
print('send:', data)
self.port.write(data.encode())
elif event.keysym == 's':
data = "(ycrun)"
print('send:', data)
self.port.write(data.encode())
#画布
canvas = Canvas(root,width = 700) #画布初始化
canvas.grid(row=5, column=0,columnspan = 4,rowspan = 1)
canvas.create_arc(10, 10, 200, 200, fill='#FFFFCC', outline='#009966', start=0, extent=180, style=PIESLICE,
tags = '合格')
#前2位为外矩形左上顶点坐标，后两位为右下点坐标，fill为填充颜色，outline为边框颜色
#start为起始角度，extend为起始与末位夹角，Style为圆弧类型
canvas.create_text(320, 85, text="合格率", font="TKMenuFont", fill="#ff8000",tags = '合格')
canvas.create_rectangle(250, 72, 270, 92, fill='#FFFFCC', outline='#CCFFFF')
canvas.delete('合格') #删除tag为“合格”的所有canvas对象
root.mainloop()
1万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
