產(chǎn)品詳情-價(jià)格2400-淘寶開源玩具wavego
單足坐標(biāo)解算
各個(gè)組件的長度(手機(jī)測(cè)量的不準(zhǔn)，估值）
# 長度 單位mm
Linkage_WH = 18; # wave舵機(jī)的支點(diǎn)高度
Linkage_WX = 12; # wave 舵機(jī)的擺動(dòng)距離
Linkage_S = 13; # 前舵機(jī)的和后舵機(jī)的距離
Linkage_L = 40.0; # 舵機(jī)臂
Linkage_D = 30; # 舵機(jī)下半部分的長度
建立坐標(biāo)系
坐標(biāo)方向如圖所示
XYZ坐標(biāo)&旋轉(zhuǎn)方向
W:WAVE->繞X軸旋轉(zhuǎn)舵機(jī)
F:FRONT->前舵機(jī)
B:BACK->后舵機(jī)
坐標(biāo)中心的為：FB中點(diǎn)的X軸位置 和 W舵機(jī)的旋轉(zhuǎn)軸的 作為原點(diǎn)
坐標(biāo)計(jì)算
x1 = sy.sin(fAngel)*Linkage_L+Linkage_S/2
y1 = sy.cos(fAngel)*Linkage_L
x2 = sy.sin(bAngel)*Linkage_L-Linkage_S/2
y2 = sy.cos(bAngel)*Linkage_L
#計(jì)算出tlen1
# tlen1 = sy.sqrt(y2**2+(x2+Linkage_S)**2)
#計(jì)算出tlen2
tlen2 = sy.sqrt((x1-x2)**2+(y1-y2)**2)
# 余弦定理
ang2 = sy.acos((tlen2)/(2*Linkage_L))
ang3 = math.atan((Linkage_E)/(Linkage_L+Linkage_D)) #注意這里是math 結(jié)果是定值
edgeX = x2-x1
edgeY = y2-y1
tailLen = math.sqrt((Linkage_L+Linkage_D)**2+Linkage_D**2)
# 獲得夾角
angTar = ang2+ang3
# 獲得結(jié)果夾角
x3 = (sy.cos(-angTar)*edgeX-sy.sin(-angTar)*edgeY)*(tailLen)/tlen2 + x1
y3 = (sy.sin(-angTar)*edgeX+sy.cos(-angTar)*edgeY)*(tailLen)/tlen2 + y1
#計(jì)算 y 軸時(shí)不要 計(jì)算 x
x4 = x3
y4 = Linkage_WX*sy.sin(-wAngel)+(y3+Linkage_WH)*sy.cos(-wAngel)
z4 = Linkage_WX*sy.cos(-wAngel)-(y3+Linkage_WH)*sy.sin(-wAngel)
#其他
x25 = (sy.cos(-ang2)*edgeX-sy.sin(-ang2)*edgeY)*(Linkage_L)/tlen2 + x1
y25 = (sy.sin(-ang2)*edgeX+sy.cos(-ang2)*edgeY)*(Linkage_L)/tlen2 + y1
image.png
驗(yàn)證
直接畫圖看效果
這里只計(jì)算了 前舵機(jī)的和后舵機(jī)的角度
# 公式->numpy代碼
fx1=sy.lambdify((w,f,b),x1,"numpy")
fy1=sy.lambdify((w,f,b),y1,"numpy")
fx2=sy.lambdify((w,f,b),x2,"numpy")
fy2=sy.lambdify((w,f,b),y2,"numpy")
fx25=sy.lambdify((w,f,b),x25,"numpy")
fy25=sy.lambdify((w,f,b),y25,"numpy")
fx3=sy.lambdify((w,f,b),x3,"numpy")
fy3=sy.lambdify((w,f,b),y3,"numpy")
import matplotlib.pyplot as plt
import numpy as np
def show(als):
point_size = [math.pow(i/len(als),2)*10 for i in range(0,len(als))]
fig=plt.figure(figsize=(3,3),dpi=200)
ax=fig.add_subplot(111)
ax.set_xlim(-150,150)
ax.set_ylim(-150,150)
ax.scatter([-Linkage_S/2],[0],s=10,)
ax.scatter([Linkage_S/2],[0],s=10,)
x = [-fx1(0,x,y) for x,y in als]
y = [-fy1(0,x,y) for x,y in als]
ax.scatter(x,y,s=point_size)
x = [-fx2(0,x,y) for x,y in als]
y = [-fy2(0,x,y) for x,y in als]
ax.scatter(x,y,s=point_size)
x = [-fx3(0,x,y) for x,y in als]
y = [-fy3(0,x,y) for x,y in als]
ax.scatter(x,y,s=point_size)
x = [-fx25(0,x,y) for x,y in als]
y = [-fy25(0,x,y) for x,y in als]
ax.scatter(x,y,s=point_size)
plt.show()
als = [(x,x) for x in range(-20,20,1)]
show(als)
als = [(x/2,-x*x/90) for x in range(0,90,1)]
show(als)
ang = 150
als = [(x,25*x/ang) for x in range(0,ang,1)]
show(als)
image.png
image.png
image.png
image.png
image.png
