产品详情-价格2400-淘宝开源玩具wavego

单足坐标解算

各个组件的长度(手机测量的不准，估值）

# 长度 单位mm
Linkage_WH = 18;     # wave舵机的支点高度 
Linkage_WX = 12;     # wave 舵机的摆动距离
Linkage_S = 13;    # 前舵机的和后舵机的距离
Linkage_L = 40.0;    # 舵机臂
Linkage_D = 30;      # 舵机下半部分的长度

建立坐标系

坐标方向如图所示
XYZ坐标&旋转方向
W:WAVE->绕X轴旋转舵机
F:FRONT->前舵机
B:BACK->后舵机
坐标中心的为：FB中点的X轴位置 和 W舵机的旋转轴的 作为原点

坐标计算

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

#计算出tlen1
# tlen1 = sy.sqrt(y2**2+(x2+Linkage_S)**2)

#计算出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 结果是定值

edgeX = x2-x1
edgeY = y2-y1
tailLen = math.sqrt((Linkage_L+Linkage_D)**2+Linkage_D**2)

# 获得夹角
angTar = ang2+ang3

# 获得结果夹角
x3 = (sy.cos(-angTar)*edgeX-sy.sin(-angTar)*edgeY)*(tailLen)/tlen2 + x1
y3 = (sy.sin(-angTar)*edgeX+sy.cos(-angTar)*edgeY)*(tailLen)/tlen2 + y1


#计算 y 轴时不要 计算 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

验证

直接画图看效果
这里只计算了 前舵机的和后舵机的角度

# 公式->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