机器蛇运动算法（一）–蜿蜒运动

机器蛇运动算法模拟蛇的运动，根据蛇的种类不同，会有很多种运动方法，此处只研究最常见的四种：蜿蜒运动、蠕动运动、翻滚运动、侧向运动。

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);