此为毕业设计需要而写
本人毕业设计需要利用差分原理,控制两个电机,达到完成上位机给定的轨迹。
基本思路:
一.通过离散点分别计算出中垂线上与远线等距的两个点,计算可得两个值,分别是(+-d)值
二.选取两个值中的一个
准则1:
(1)计算出第一个双值时,随便选取一个值A。
(2)计算出第二个双值时,分别用A与两值做连线,即计算两个直线方程。
(3)两个直线方程将与原直线分别有一个交点,计算出交点。
(4)计算该两个交点与原直线上的recent_dot的距离。
(5)选择距离大的点的值。
准则2:
(1)计算A点与双值之间的距离
(2)选取距离小的
准则1,准则2相与,可以完成筛选。
Python代码如下:
'''
拟合出第二条线
'''
import math
import sys
import sympy
i = 0
j = 0
cc=[0,0]
output_1=0.0
output_2=0.0
line1_x = 0
line1_y = 0
line2_x = 0
line2_y = 0
x=0
b10 = 0
b11 = 0
b20 = 0
b21 = 0
d1_x = 0
d1_y = 0
d2_x = 0
d2_y = 0
d21_x = 0
d21_y = 0
c1_x = 0
c1_y = 0
c2_x = 0
c2_y = 0
c3_x = 0
c3_y = 0
p_x = 0
p_y = 0
q_x = 0
q_y = 0
with open(r'C:\Users\Administrator\Desktop\chart track\data\line1_data.txt','r') as data,open(r'C:\Users\Administrator\Desktop\chart track\data\line2_Data.txt','a') as output_data:
for line in data.readlines():
line = eval(line)
if(i==0): #获得出c1坐标值
line1_x = line[0]
line1_y = line[-1]
c1_x = line1_x
c1_y = line1_y
if(i>0):
if(i==1): #得到c2值
line2_x = line[0]
line2_y = line[-1]
c2_x = line2_x
c2_y = line2_y
if(i>1): #得到c3值
line2_x = line[0]
line2_y = line[-1]
c3_x = line2_x
c3_y = line2_y
#计算直线Ax+By+C=0中的参数
A = line2_y-line1_y
B = line1_x-line2_x
C = line2_x*line1_y-line1_x*line2_y
x = sympy.Symbol('x')
y = sympy.Symbol('y')
if(A==0):
b10=(line2_x+line1_x)/2+0.111541
b11=line1_y+30+0.115145814
b20=(line2_x+line1_x)/2+0.111541
b21=line1_y-30+0.115145814
else:
a=sympy.solve([B/A*x+(line2_y+line1_y)/2-B*(line2_x+line1_x)/(2*A)-y,(x-(line2_x+line1_x)/2)**2+(y-(line2_y+line1_y)/2)**2-900],[x,y])
b1=a[0]
b10=b1[0]
b11=b1[1]
b2=a[1]
b20=b2[0]
b21=b2[1]
print(a)
if(i==1):
cc[0]=b10
cc[1]=b11
d1_x = b10
d1_y = b11
if(i>1):
d2_x = b10+0.10101101
d2_y = b11+0.11100101
d21_x = b20+0.1101010
d21_y = b21+0.1110111
x1 = sympy.Symbol('x1')
y1 = sympy.Symbol('y1')
p = sympy.solve([(x1-c1_x)*(c3_y-c1_y)-(c3_x-c1_x)*(y1-c1_y),(x1-d1_x)*(d2_y-d1_y)-(d2_x-d1_x)*(y1-d1_y)],[x1,y1])
q = sympy.solve([(x1-c1_x)*(c3_y-c1_y)-(c3_x-c1_x)*(y1-c1_y),(x1-d1_x)*(d21_y-d1_y)-(d21_x-d1_x)*(y1-d1_y)],[x1,y1])
p_x = p[x1]
p_y = p[y1]
q_x = q[x1]
q_y = q[y1]
if((d1_x-d2_x)**2+(d1_y-d2_y)**2<=(d1_x-d21_x)**2+(d1_y-d21_y)**2 and (p_x-c1_x)**2+(p_x-c1_y)**2>=(q_x-c1_x)**2+(q_y-c1_y)**2):
cc[0]=b10
cc[1]=b11
output_data.write(str(cc))
output_data.write('\n')
d1_x = b10
d1_y = b11
else:
cc[0]=b20
cc[1]=b21
output_data.write(str(cc))
output_data.write('\n')
d1_x = b20
d1_y = b21
c1_x = c2_x
c1_y = c2_y
c2_x = c3_x
c2_y = c3_y
line1_x = line2_x
line1_y = line2_y
i = i+1
print("成功计算出",i,"个坐标值")