机器蛇运动算法模拟蛇的运动,根据蛇的种类不同,会有很多种运动方法,此处只研究最常见的四种:蜿蜒运动、蠕动运动、翻滚运动、侧向运动。
STM32做主控,数字舵机做驱动。
本文主要参照催春的硕士论文《仿生蛇的设计及其运动仿真》。
1、先在VS中写好算法,蜿蜒其实就可看作是一个三角函数
/*
a–关节i与关节i-1之间的转角 (度)
b–e为0时,关节转交的幅值(度)
c–关节转角频率(PI/S)
d–相位差(度)
e–整个关节系统的偏转角(度)
f–拟合速度
i–关节数
*/
struct Snake
{
//蜿蜒运动
//a=b*sin(c*t+i*d)+e
//a=b*(1-E^(-f*t))*sin(c*t+i*d)+e加入无限趋近于零的函数
//float a;
float b ;
float c;
float d;
float e;
//float f;//拟合时间,先不管
//float i;
float t;
};
float get_ang(Snake snake, int i, float t)
{
return snake.b * sin(snake.c*t + i*snake.d) + snake.e;//三角函数
}
void print_ang(float *buf)
{
for (size_t i = 0; i < 12; i++)
{
cout << “ang” << i << ” = “ << *(buf + i) << “\t”;
}
cout << “” << endl;
}
int main()
{
Snake snake;
snake.b = 120;
snake.c = 1.5;
snake.d = 1.0;
snake.e = 0;
snake.t = 0.1;
float ang[12] = { 0,0,0,0,0,0,0,0,0,0,0 };
for (float t = 0; t < 10;)
{
for (size_t i = 0; i < 12; i++)
{
ang[i] = get_ang(snake, i + 1, t);
}
t += snake.t;
print_ang(ang);
}
return 0;
}
2、查看输出没有错误,现移植到STM32
typedef struct
{
//a=b*sin(c*t+i*d)+e
//a=b*(1-E^(-f*t))*sin(c*t+i*d)+e加入无限趋近于零的函数
//float a;
float b ;
float c;
float d;
float e;
//float f;//拟合时间,先不管
//float i;
float t;
}Snake;
/*
a–关节i与关节i-1之间的转角 (度)
b–e为0时,关节转交的幅值(度)
c–关节转角频率(PI/S)
d–相位差(度)
e–整个关节系统的偏转角(度)
f–拟合速度
i–关节数
*/
float get_ang_2(Snake snake, int i, float t)
{
return snake.b * sin(snake.c*t + i*snake.d) + snake.e;
}
void snake_go(u8 times, u8 wait_time)
{
Snake snake;
float t;
u16 Speed[17] = {0,0, 0,0, 0,0, 0,0, 0,0, 0,0,0,0,0,0,0};
//float aa[6] = {0.4,0.6,0.8,1.0,1.2,1.4};
u8 i;
s16 ang[17];
clear_buf(ang,17);
snake.b = 120;
snake.c = 1.5;
snake.d = 1.0;
snake.e = 0;
snake.t = 0.1;
for ( t = 0; t < times;)
{
for(i=0;i<6;i++)
{
ang[11 – 2*i] = get_ang_2(snake, i, t);//aa[5-i]*get_ang_2(snake, i, t);
}
t += snake.t;
RobotAction(MotorZeroAngle,ang,Speed);//参数传给舵机,最后给舵机的角度是MotorZeroAngle+ang
delay_ms(wait_time);
}
}
主程序中直接调用snake_go(100,30);