机器蛇运动算法(一)–蜿蜒运动

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

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

  • 0
    点赞
  • 28
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
非常感谢您的提问!以下是一个简单的贪吃算法的C语言实现: #include <stdio.h> #include <stdlib.h> #include <conio.h> #include <time.h> #define ROW 20 #define COL 40 int snake[ROW * COL][2]; // 的坐标 int food[2]; // 食物的坐标 int len = 3; // 的长度 int score = 0; // 得分 void init(); // 初始化 void draw(); // 绘制 void move(); // 移动 void generate_food(); // 生成食物 int is_eat_food(); // 判断是否吃到食物 int is_game_over(); // 判断游戏是否结束 int main() { init(); while (1) { draw(); move(); if (is_eat_food()) { generate_food(); score++; len++; } if (is_game_over()) { printf("Game over! Your score is %d.\n", score); break; } Sleep(100); } return 0; } void init() { srand(time(NULL)); // 初始化的位置 snake[0][0] = ROW / 2; snake[0][1] = COL / 2; snake[1][0] = ROW / 2 + 1; snake[1][1] = COL / 2; snake[2][0] = ROW / 2 + 2; snake[2][1] = COL / 2; // 初始化食物的位置 generate_food(); } void draw() { system("cls"); for (int i = 0; i < ROW; i++) { for (int j = 0; j < COL; j++) { if (i == food[0] && j == food[1]) { printf("*"); } else { int flag = 0; for (int k = 0; k < len; k++) { if (i == snake[k][0] && j == snake[k][1]) { printf("O"); flag = 1; break; } } if (!flag) { printf(" "); } } } printf("\n"); } printf("Score: %d\n", score); } void move() { int direction = getch(); int new_head[2]; new_head[0] = snake[len - 1][0]; new_head[1] = snake[len - 1][1]; switch (direction) { case 'w': new_head[0]--; break; case 's': new_head[0]++; break; case 'a': new_head[1]--; break; case 'd': new_head[1]++; break; } for (int i = 0; i < len - 1; i++) { snake[i][0] = snake[i + 1][0]; snake[i][1] = snake[i + 1][1]; } snake[len - 1][0] = new_head[0]; snake[len - 1][1] = new_head[1]; } void generate_food() { int x, y; do { x = rand() % ROW; y = rand() % COL; } while (x == snake[len - 1][0] && y == snake[len - 1][1]); food[0] = x; food[1] = y; } int is_eat_food() { if (snake[len - 1][0] == food[0] && snake[len - 1][1] == food[1]) { return 1; } else { return 0; } } int is_game_over() { if (snake[len - 1][0] < 0 || snake[len - 1][0] >= ROW || snake[len - 1][1] < 0 || snake[len - 1][1] >= COL) { return 1; } for (int i = 0; i < len - 1; i++) { if (snake[i][0] == snake[len - 1][0] && snake[i][1] == snake[len - 1][1]) { return 1; } } return 0; }

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值