【四足大创】1.2单腿足尖的曲线控制

【四足大创】1.2单腿足尖的曲线控制

思路

已知足尖的运动轨迹，每隔一小段时间取一个点，通过该点足尖的坐标，倒算出舵机此刻的角度，再让舵机转动到解算出的角度。

Tip：设置几个参数（如足尖运动方程，周期长度，取点间隔等），程序开始运行时，批量计算特定时刻足尖位置，以及位置坐标对应的舵机角度，然后存入数组，之后调用数组数据即可。

足尖曲线

如图所示，曲线分为两部分，上部是一摆线，下部是一条直线，足尖在曲线上周期循环。假设一个周期为2秒，以胯关节为原点，向右为x轴正方向，向下为y轴正方向，则曲线方程可以为（具体参数乱编的）：

unsigned long long time;
double t,x,y;
time=millis();
time=time/2000;
t=12.56*time/1000;
if (t<6.28){
	x=t-sin(t)-3.14;
	y=-1+cos(t)+10;
} else {
	x=-t+9.42
	y=10;
}

运动学逆解算

令足尖C点的坐标为(x₀，y₀)，顺时针为x轴正方向，上肢长L₁，下肢长L₂，C点距原点L₀，胯关节舵机应转动至angle_1，膝关节应转动至angle_2，则如图所示：

L₀=sqrt(x₀²+y₀²);
θ=-arctan(x₀/y₀)；
α=arccos((L₀²+L₁²-L₂²)/(2 * L₀ * L₁));
β=arccos((L₁²+L₂²-L₀²)/(2 * L₁ * L₂));
angle_1=init_1+α+θ；
angle_2=init_2+β-90；