机器蛇运动算法（二）–蠕动运动

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

STM32做主控，数字舵机做驱动。

本文主要参照催春的硕士论文《仿生蛇的设计及其运动仿真》。

1、蠕动根据图理解含义，一目了然

 

2、每次移动距离为s，然后s和两个关节组成三角形，求反三角函数，先在VS中实现反三角函数

#define PI 3.1415926  
   

struct Triangle  
{  
    float a;  
    float b;  
    float c;  
    float ang_a;  
    float ang_b;  
    float ang_c;  
};  
   

void get_ang(Triangle *triangle)  
{  
    if ((triangle->a + triangle->b) > triangle->c  
        && (triangle->a + triangle->b) > triangle->c  
        && (triangle->a + triangle->b) > triangle->c)  
    {  
        triangle->ang_a = acos((triangle->b*triangle->b + triangle->c*triangle->c – triangle->a * triangle->a) / (2 * triangle->b*triangle->c));  
        triangle->ang_b = acos((triangle->a*triangle->a + triangle->c*triangle->c – triangle->b * triangle->b) / (2 * triangle->a*triangle->c));  
        triangle->ang_c = acos((triangle->a*triangle->a + triangle->b*triangle->b – triangle->c * triangle->c) / (2 * triangle->a*triangle->b));  
    }  
}  
void print_ang(Triangle *triangle)  
{  
    cout << “a=” << triangle->a << endl;  
    cout << “b=” << triangle->b << endl;  
    cout << “c=” << triangle->c << endl;  
    //cout << “ang_a=” << (triangle->ang_a/PI)*180.0*(1024/300.0)<< endl;  
    cout << “ang_a=” << triangle->ang_a << endl;  
    cout << “ang_b=” << triangle->ang_b << endl;  
    cout << “ang_c=” << triangle->ang_c << endl;  
}  
   

int main()  
{  
    Triangle triangle;  
    triangle.a = 156.0;  
    triangle.b = 79.0;  
    triangle.c = 131.0;  
    get_ang(&triangle);  
    print_ang(&triangle);  
    return 0;  
}  

3、测出所有可垂直方向运动的关节长度，然后反三角函数求出角度，为了可以平滑的运动，写一个微分，从角度一到角度二慢慢变化，方法如下：

#define PI 3.1415926  
//  
typedef struct   
{  
    float a;  
    float b;  
    float c;  
    float ang_a;  
    float ang_b;  
    float ang_c;  
}Triangle;  
void get_ang(Triangle *triangle)  
{  
    if ((triangle->a + triangle->b) > triangle->c  
        && (triangle->a + triangle->b) > triangle->c  
        && (triangle->a + triangle->b) > triangle->c)  
    {  
        triangle->ang_a = acos((triangle->b*triangle->b + triangle->c*triangle->c – triangle->a * triangle->a) / (2 * triangle->b*triangle->c));  
        triangle->ang_b = acos((triangle->a*triangle->a + triangle->c*triangle->c – triangle->b * triangle->b) / (2 * triangle->a*triangle->c));  
        triangle->ang_c = acos((triangle->a*triangle->a + triangle->b*triangle->b – triangle->c * triangle->c) / (2 * triangle->a*triangle->b));  
    }  
}  
void gradual_change_for_buf_send_bot(s16* buf_1, s16* buf_2, u8 times, u8 wait_time)  
{  
    float gradual_buf[17],temp_buf[17];  
    s16 send_buf[17];  
    u8 i=0,j=0;  
    u16 Speed[17] = {0,0,        0,0, 0,0,  0,0,    0,0,        0,0,0,0,0,0,0};  
    //获取gradual，起始步骤  
    for(i=0;i<17;i++)  
    {  
        gradual_buf[i]=((float)(*(buf_2+i) – *(buf_1+i)))/times;  
        temp_buf[i] = (float)*(buf_1+i);  
    }  
    for(i=0;i<times;i++)  
    {  
        //计算当前发送角度  
        for(j=0;j<17;j++)  
        {  
            temp_buf[j] += gradual_buf[j];  
            send_buf[j] = (s16)temp_buf[j];  
        }  
        RobotAction(MotorZeroAngle,send_buf,Speed);  
        delay_ms(wait_time);  
    }  
}  
void clear_buf(s16 *buf,u8 len)  
{  
    u8 i;  
    for(i=0;i<len;i++)  
        *(buf+i) = 0;  
}  
void copy_buf(s16 *buf_1,s16 *buf_2,u8 len)  
{  
    u8 i;  
    for(i=0;i<len;i++)  
        *(buf_2+i) = *(buf_1+i);  
}  
void xxx(u16 mm,u8 times, u8 wait_time)//80  30  20  
{  
    u8 i=0;  
    s16 buf_1[17],buf_2[17];  
    //关节长度  
    float joint[6] = {78.9,131.0,131.0,131.0,131.0,54.6};  
    Triangle triangle;  
      

    //第一个  
    triangle.a = joint[0]+ joint[1] – mm ;  
    triangle.b = joint[0];  
    triangle.c = joint[1];  
    get_ang(&triangle);  
    clear_buf(buf_1,17);  
    clear_buf(buf_2,17);  
    buf_2[6] = -(s16)((triangle.ang_b/PI)*180.0*(1024/300.0));  
    buf_2[11] = (s16)((1 – triangle.ang_a/PI)*180.0*(1024/300.0));  
    gradual_change_for_buf_send_bot(buf_1,buf_2,times,wait_time);  
      

    //第二个  
    triangle.a = joint[1]+ joint[2] – mm ;  
    triangle.b = joint[1];  
    triangle.c = joint[2];  
    get_ang(&triangle);  
    copy_buf(buf_2,buf_1,17);  
    clear_buf(buf_2,17);  
    buf_2[11] = -(s16)((triangle.ang_c/PI)*180.0*(1024/300.0));  
    buf_2[6] = (s16)((1 – triangle.ang_a/PI)*180.0*(1024/300.0));  
    buf_2[4] = -(s16)((triangle.ang_b/PI)*180.0*(1024/300.0));  
    gradual_change_for_buf_send_bot(buf_1,buf_2,times,wait_time);  
      

    //第三个  
    triangle.a = joint[2]+ joint[3] – mm ;  
    triangle.b = joint[2];  
    triangle.c = joint[3];  
    get_ang(&triangle);  
    copy_buf(buf_2,buf_1,17);  
    clear_buf(buf_2,17);  
    buf_2[6] = -(s16)((triangle.ang_c/PI)*180.0*(1024/300.0));  
    buf_2[4] = (s16)((1 – triangle.ang_a/PI)*180.0*(1024/300.0));  
    buf_2[2] = -(s16)((triangle.ang_b/PI)*180.0*(1024/300.0));  
    gradual_change_for_buf_send_bot(buf_1,buf_2,times,wait_time);  
      

    //第四个  
    triangle.a = joint[3]+ joint[4] – mm ;  
    triangle.b = joint[3];  
    triangle.c = joint[4];  
    get_ang(&triangle);  
    copy_buf(buf_2,buf_1,17);  
    clear_buf(buf_2,17);  
    buf_2[4] = -(s16)((triangle.ang_c/PI)*180.0*(1024/300.0));  
    buf_2[2] = (s16)((1 – triangle.ang_a/PI)*180.0*(1024/300.0));  
    buf_2[0] = -(s16)((triangle.ang_b/PI)*180.0*(1024/300.0));  
    gradual_change_for_buf_send_bot(buf_1,buf_2,times,wait_time);  
      

    //第五个  
    triangle.a = joint[4]+ joint[5] – mm ;  
    triangle.b = joint[4];  
    triangle.c = joint[5];  
    get_ang(&triangle);  
    copy_buf(buf_2,buf_1,17);  
    clear_buf(buf_2,17);  
    buf_2[2] = -(s16)((triangle.ang_c/PI)*180.0*(1024/300.0));  
    gradual_change_for_buf_send_bot(buf_1,buf_2,times,wait_time);  
      

    //落  
    copy_buf(buf_2,buf_1,17);  
    clear_buf(buf_2,17);  
    gradual_change_for_buf_send_bot(buf_1,buf_2,times,wait_time);  
}  

mm就是每次走的长度