找回密码
 立即注册

QQ登录

只需一步,快速开始

搜索
查看: 1133|回复: 1
打印 上一主题 下一主题
收起左侧

mega16狭窄足机器人程序

[复制链接]
跳转到指定楼层
楼主
ID:611223 发表于 2020-10-19 14:43 | 只看该作者 回帖奖励 |倒序浏览 |阅读模式
基于的单片机机器人狭窄足机器人实例
  1. #include "mega16.h"
  2. #include "delay.h"
  3. #include<math.h>
  4. float Now_s1=90,Now_s2=90,Now_s3=90,Now_s4=90,Now_s5=90,Now_s6=90,Now_F=0;
  5. #define uchar unsigned char
  6. #define uint unsigned int

  7. void Init(void)
  8. {
  9.     DDRA=0x00;
  10.     PORTA=0x00;
  11.     DDRB=0xff;
  12.     PORTB=0xff;
  13.     DDRC=0xff;
  14.     PORTC=0xff;
  15.     DDRD=0xff;
  16.     PORTD=0xff;
  17. }

  18. void Servo_Control(uint Angel,uchar Channel)
  19. {
  20.     if(Channel>=1&&Channel<=6)
  21.     {   
  22.         
  23.         
  24.         if(Angel<0)Angel=0;
  25.         if(Angel>180)Angel=180;                     
  26.         switch(Channel)
  27.         {
  28.             case 1: Angel=Angel*5-10; break; //40 200
  29.             case 2: Angel=Angel*5+8; break; //45 200
  30.             case 3: Angel=Angel*5+10; break; //40 203
  31.             case 4: Angel=Angel*5-2; break; //40 205
  32.             case 5: Angel=Angel*5+3; break; //40 205    //+6
  33.             case 6: Angel=Angel*4.85+30; break; //40 200   
  34.         }
  35.         Angel+=10;

  36.         if(Angel<0||Angel>980)Angel=0;
  37.         else if(Angel>950)Angel=950;
  38.          
  39.         PORTC=0x00;
  40.         PORTB=0x00;
  41.         PORTD.2=0;
  42.         delay_us(50);
  43.         Channel=7-Channel;
  44.         Channel=Channel-1;
  45.         PORTC=Channel*4+Angel/256;
  46.         PORTB=(uchar)Angel;
  47.    
  48.         delay_us(50);
  49.         PORTD.2=1;
  50.         
  51.         delay_us(100);
  52.         PORTC=0x00;
  53.         PORTB=0x00;
  54.         PORTD.2=0;
  55.     }
  56. }

  57. void ServoReset(void)
  58. {
  59.     uchar i=0;
  60.     for(i=1;i<=6;i++)
  61.     {                              
  62.         Servo_Control(90,i);
  63.     }
  64. }

  65. #define TIME_PER_STEP 5


  66. //type:0正常 1加速 2减速 3加速减速   4脚腕加速 其他减速 5脚腕减速 其他加速
  67. void Do(uchar s1,uchar s2,uchar s3,uchar s4,uchar s5,uchar s6,uint steps,int forward,uchar type)
  68. {
  69.     float Step_s1=0,Step_s2=0,Step_s3=0,Step_s4=0,Step_s5=0,Step_s6=0,Step_F=0;
  70.     uint i=0;

  71.     Step_s1=(s1-Now_s1)/steps;
  72.     Step_s2=(s2-Now_s2)/steps;
  73.     Step_s3=(s3-Now_s3)/steps;
  74.     Step_s4=(s4-Now_s4)/steps;
  75.     Step_s5=(s5-Now_s5)/steps;
  76.     Step_s6=(s6-Now_s6)/steps;
  77.     Step_F=(forward-Now_F)/steps;
  78.    
  79.     if(type==0)
  80.     {
  81.         for(i=0;i<steps;i++)
  82.         {   
  83.             Now_F+=Step_F;
  84.             Now_s1+=Step_s1;
  85.             Now_s2+=Step_s2;
  86.             Now_s3+=Step_s3;
  87.             Now_s4+=Step_s4;
  88.             Now_s5+=Step_s5;
  89.             Now_s6+=Step_s6;

  90.             Servo_Control((uchar)(Now_s1-Now_F),1);
  91.             Servo_Control((uchar)Now_s2,2);
  92.             Servo_Control((uchar)Now_s3,3);
  93.             Servo_Control((uchar)(Now_s4+Now_F),4);
  94.             Servo_Control((uchar)Now_s5,5);
  95.             Servo_Control((uchar)Now_s6,6);
  96.             delay_ms(TIME_PER_STEP);
  97.         }
  98.     }
  99.     else
  100.     if(type==1)
  101.     {
  102.         for(i=0;i<steps;i++)
  103.         {   
  104.             Step_F=((float)i/(float)steps)*((float)i/(float)steps)*(forward-Now_F)+Now_F;
  105.             Step_s1=((float)i/(float)steps)*((float)i/(float)steps)*(s1-Now_s1)+Now_s1;
  106.             Step_s2=((float)i/(float)steps)*((float)i/(float)steps)*(s2-Now_s2)+Now_s2;
  107.             Step_s3=((float)i/(float)steps)*((float)i/(float)steps)*(s3-Now_s3)+Now_s3;
  108.             Step_s4=((float)i/(float)steps)*((float)i/(float)steps)*(s4-Now_s4)+Now_s4;
  109.             Step_s5=((float)i/(float)steps)*((float)i/(float)steps)*(s5-Now_s5)+Now_s5;
  110.             Step_s6=((float)i/(float)steps)*((float)i/(float)steps)*(s6-Now_s6)+Now_s6;
  111.             
  112.             Servo_Control((uchar)Step_s1-Step_F,1);
  113.             Servo_Control((uchar)Step_s2,2);
  114.             Servo_Control((uchar)Step_s3,3);
  115.             Servo_Control((uchar)Step_s4+Step_F,4);
  116.             Servo_Control((uchar)Step_s5,5);
  117.             Servo_Control((uchar)Step_s6,6);
  118.             delay_ms(TIME_PER_STEP);
  119.         }   
  120.     }
  121.     else
  122.     if(type==2)
  123.     {
  124.         for(i=0;i<steps;i++)
  125.         {   
  126.             Step_F=(1.0-(1.0*((float)i/(float)steps-1.0)*((float)i/(float)steps-1.0)))*(forward-Now_F)+Now_F;
  127.             Step_s1=(1.0-(1.0*((float)i/(float)steps-1.0)*((float)i/(float)steps-1.0)))*(s1-Now_s1)+Now_s1;
  128.             Step_s2=(1.0-(1.0*((float)i/(float)steps-1.0)*((float)i/(float)steps-1.0)))*(s2-Now_s2)+Now_s2;
  129. ……………………

  130. …………限于本文篇幅 余下代码请从51黑下载附件…………
复制代码


狭窄足机器人.zip

114.58 KB, 下载次数: 2, 下载积分: 黑币 -5

分享到:  QQ好友和群QQ好友和群 QQ空间QQ空间 腾讯微博腾讯微博 腾讯朋友腾讯朋友
收藏收藏 分享淘帖 顶 踩
回复

使用道具 举报

沙发
ID:1 发表于 2020-10-19 20:53 | 只看该作者
本帖需要重新编辑补全电路原理图,源码,详细说明与图片即可获得100+黑币(帖子下方有编辑按钮)
回复

使用道具 举报

您需要登录后才可以回帖 登录 | 立即注册

本版积分规则

手机版|小黑屋|51黑电子论坛 |51黑电子论坛6群 QQ 管理员QQ:125739409;技术交流QQ群281945664

Powered by 单片机教程网

快速回复 返回顶部 返回列表