设为首页收藏本站
开启辅助访问 切换到宽版

 找回密码
 立即注册

QQ登录

只需一步，快速开始

热门： 51单片机 | 24小时必答区 | 单片机教程 | 单片机DIY制作 | STM32 | Cortex M3 | 模数电子 | 电子DIY制作 | 音响/功放 | 拆机乐园 | Arduino | 嵌入式OS | 程序设计

搜索
»论坛 › 电子技术分类讨论区 › 智能小车/机器人 › 简单机器人0上台防掉台小程序
返回列表 发新帖
查看: 3154|回复: 0
打印 上一主题 下一主题
收起左侧

简单机器人0上台防掉台小程序

[复制链接]
跳转到指定楼层
楼主
ID:221364 发表于 2017-7-21 15:07 | 只看该作者 |只看大图 回帖奖励 |倒序浏览 |阅读模式
机器人大赛，小白参赛

所有资料51hei提供下载:
Debug.zip (1.85 KB, 下载次数: 7)


0上台防掉台源程序如下:
  1. #include  "Apps/SystemTask.h"
  2. uint8 SERVO_MAPPING[5] = {1,2,3,4,5};
  3. int main()
  4. {
  5.     int  ad[6] = {0};
  6.     int  io[12] = {0};
  7.     int  ave = 0;
  8.     MFInit();
  9.     MFInitServoMapping(&SERVO_MAPPING[0],5);
  10.     MFSetPortDirect(0x00000000);
  11.     MFSetServoMode(1,1);
  12.     MFSetServoMode(2,1);
  13.     MFSetServoMode(3,0);
  14.     MFSetServoMode(4,0);
  15.     MFSetServoMode(5,0);
  16.     //初始化状态
  17.     MFSetServoRotaSpd(1,0);
  18.     MFSetServoRotaSpd(2,0);
  19.     MFSetServoPos(3,617,512);
  20.     MFSetServoPos(4,358,512);
  21.     MFSetServoPos(5,581,512);
  22.     MFServoAction();
  23.     //io1
  24.     io[1] = MFGetDigiInput(1);
  25.     while (1)
  26.     {
  27.         if (io[1]==0)
  28.         {
  29.             break ;
  30.         }
  31.         //io1
  32.         io[1] = MFGetDigiInput(1);
  33.     }
  34.     //立爪
  35.     MFSetServoRotaSpd(1,0);
  36.     MFSetServoRotaSpd(2,0);
  37.     MFSetServoPos(3,280,512);
  38.     MFSetServoPos(4,825,512);
  39.     MFSetServoPos(5,116,512);
  40.     MFServoAction();
  41.     DelayMS(1000);
  42.     //立爪前进
  43.     MFSetServoRotaSpd(1,400);
  44.     MFSetServoRotaSpd(2,-400);
  45.     MFSetServoPos(3,280,512);
  46.     MFSetServoPos(4,825,512);
  47.     MFSetServoPos(5,116,512);
  48.     MFServoAction();
  49.     DelayMS(1000);
  50.     //放前爪
  51.     MFSetServoRotaSpd(1,400);
  52.     MFSetServoRotaSpd(2,-400);
  53.     MFSetServoPos(3,721,512);
  54.     MFSetServoPos(4,825,512);
  55.     MFSetServoPos(5,116,512);
  56.     MFServoAction();
  57.     DelayMS(1000);
  58.     //放后爪
  59.     MFSetServoRotaSpd(1,400);
  60.     MFSetServoRotaSpd(2,-400);
  61.     MFSetServoPos(3,280,512);
  62.     MFSetServoPos(4,300,512);
  63.     MFSetServoPos(5,649,512);
  64.     MFServoAction();
  65.     DelayMS(1000);
  66.     //台上状态
  67.     MFSetServoRotaSpd(1,400);
  68.     MFSetServoRotaSpd(2,-400);
  69.     MFSetServoPos(3,617,512);
  70.     MFSetServoPos(4,358,512);
  71.     MFSetServoPos(5,581,512);
  72.     MFServoAction();
  73.     DelayMS(1000);
  74.     while (1)
  75.     {
  76.         //io3
  77.         io[3] = MFGetDigiInput(3);
  78.         //io4
  79.         io[4] = MFGetDigiInput(4);
  80.         //io5
  81.         io[5] = MFGetDigiInput(5);
  82.         //io6
  83.         io[6] = MFGetDigiInput(6);
  84.         //台上状态
  85.         MFSetServoRotaSpd(1,600);
  86.         MFSetServoRotaSpd(2,-600);
  87.         MFSetServoPos(3,617,512);
  88.         MFSetServoPos(4,358,512);
  89.         MFSetServoPos(5,581,512);
  90.         MFServoAction();
  91.         //io3
  92.         io[3] = MFGetDigiInput(3);
  93.         if (io[3]==1)
  94.         {
  95.             //后退
  96.             MFSetServoRotaSpd(1,-400);
  97.             MFSetServoRotaSpd(2,400);
  98.             MFSetServoPos(3,671,512);
  99.             MFSetServoPos(4,358,512);
  100.             MFSetServoPos(5,581,512);
  101.             MFServoAction();
  102.             DelayMS(400);
  103.             //右转
  104.             MFSetServoRotaSpd(1,700);
  105.             MFSetServoRotaSpd(2,700);
  106.             MFSetServoPos(3,671,512);
  107.             MFSetServoPos(4,358,512);
  108.             MFSetServoPos(5,581,512);
  109.             MFServoAction();
  110.             DelayMS(400);
  111.             //台上状态
  112.             MFSetServoRotaSpd(1,400);
  113.             MFSetServoRotaSpd(2,-400);
  114.             MFSetServoPos(3,617,512);
  115.             MFSetServoPos(4,358,512);
  116.             MFSetServoPos(5,581,512);
  117.             MFServoAction();
  118.         }
  119.         else
  120.         {
  121.             //io4
  122.             io[4] = MFGetDigiInput(4);
  123.             if (io[4]==1)
  124.             {
  125.                 //后退
  126.                 MFSetServoRotaSpd(1,-400);
  127.                 MFSetServoRotaSpd(2,400);
  128.                 MFSetServoPos(3,617,512);
  129.                 MFSetServoPos(4,358,512);
  130.                 MFSetServoPos(5,581,512);
  131.                 MFServoAction();
  132.                 DelayMS(400);
  133.                 //左转
  134.                 MFSetServoRotaSpd(1,-700);
  135.                 MFSetServoRotaSpd(2,-700);
  136.                 MFSetServoPos(3,617,512);
  137.                 MFSetServoPos(4,358,512);
  138.                 MFSetServoPos(5,581,512);
  139.                 MFServoAction();
  140.                 DelayMS(400);
  141.                 //台上状态
  142.                 MFSetServoRotaSpd(1,400);
  143.                 MFSetServoRotaSpd(2,-400);
  144.                 MFSetServoPos(3,617,512);
  145.                 MFSetServoPos(4,358,512);
  146.                 MFSetServoPos(5,581,512);
  147.                 MFServoAction();
  148.             }
  149.             else
  150.             {
  151.                 //io5
  152.                 io[5] = MFGetDigiInput(5);
  153.                 if (io[5]==1)
  154.                 {
  155.                     //前进
  156.                     MFSetServoRotaSpd(1,400);
  157.                     MFSetServoRotaSpd(2,-400);
  158.                     MFSetServoPos(3,617,512);
  159.                     MFSetServoPos(4,358,512);
  160.                     MFSetServoPos(5,581,512);
  161.                     MFServoAction();
  162.                     DelayMS(400);
  163.                     //右转
  164.                     MFSetServoRotaSpd(1,700);
  165.                     MFSetServoRotaSpd(2,700);
  166.                     MFSetServoPos(3,617,512);
  167.                     MFSetServoPos(4,358,512);
  168.                     MFSetServoPos(5,581,512);
  169.                     MFServoAction();
  170.                     DelayMS(200);
  171.                     //台上状态
  172.                     MFSetServoRotaSpd(1,400);
  173.                     MFSetServoRotaSpd(2,-400);
  174.                     MFSetServoPos(3,617,512);
  175.                     MFSetServoPos(4,358,512);
  176.                     MFSetServoPos(5,581,512);
  177.                     MFServoAction();
  178.                 }
  179.                 else
  180.                 {
  181.                     //io6
  182.                     io[6] = MFGetDigiInput(6);
  183.                     if (io[6]==1)
  184.                     {
  185.                         //前进
  186.                         MFSetServoRotaSpd(1,400);
  187.                         MFSetServoRotaSpd(2,-400);
  188.                         MFSetServoPos(3,617,512);
  189.                         MFSetServoPos(4,358,512);
  190.                         MFSetServoPos(5,581,512);
  191.                         MFServoAction();
  192.                         DelayMS(500);
  193.                         //左转
  194.                         MFSetServoRotaSpd(1,-700);
  195.                         MFSetServoRotaSpd(2,-700);
  196.                         MFSetServoPos(3,617,512);
  197.                         MFSetServoPos(4,358,512);
  198.                         MFSetServoPos(5,581,512);
  199.                         MFServoAction();
  200.                         DelayMS(200);
  201.                         //台上状态
  202.                         MFSetServoRotaSpd(1,400);
  203.                         MFSetServoRotaSpd(2,-400);
  204.                         MFSetServoPos(3,617,512);
  205.                         MFSetServoPos(4,358,512);
  206.                         MFSetServoPos(5,581,512);
  207.                         MFServoAction();
  208.                     }
  209.                     else
  210.                     {
  211.                         //io3
  212.                         io[3] = MFGetDigiInput(3);
  213.                         //io4
  214.                         io[4] = MFGetDigiInput(4);
  215.                         if ((io[3]&&io[4]==1))
  216.                         {
  217.                             //后退
  218.                             MFSetServoRotaSpd(1,-400);
  219.                             MFSetServoRotaSpd(2,400);
  220.                             MFSetServoPos(3,671,512);
  221.                             MFSetServoPos(4,358,512);
  222.                             MFSetServoPos(5,581,512);
  223.                             MFServoAction();
  224.                             DelayMS(1000);
  225.                             //右转
  226.                             MFSetServoRotaSpd(1,700);
  227.                             MFSetServoRotaSpd(2,700);
  228.                             MFSetServoPos(3,617,512);
  229.                             MFSetServoPos(4,358,512);
  230.                             MFSetServoPos(5,581,512);
  231.                             MFServoAction();
  232.                             DelayMS(200);
  233.                             //台上状态
  234.                             MFSetServoRotaSpd(1,400);
  235.                             MFSetServoRotaSpd(2,-400);
  236.                             MFSetServoPos(3,617,512);
  237.                             MFSetServoPos(4,358,512);
  238.                             MFSetServoPos(5,581,512);
  239.                             MFServoAction();
  240.                         }
  241.                         else
  242.                         {
  243.                             //io5
  244.                             io[5] = MFGetDigiInput(5);
  245.                             //io6
  246.                             io[6] = MFGetDigiInput(6);
  247.                             if ((io[5]&&io[6]==1))
  248.                             {
  249.                                 //后退
  250.                                 MFSetServoRotaSpd(1,-400);
  251.                                 MFSetServoRotaSpd(2,400);
  252.                                 MFSetServoPos(3,671,512);
  253.                                 MFSetServoPos(4,358,512);
  254.                                 MFSetServoPos(5,581,512);
  255.                                 MFServoAction();
  256.                                 DelayMS(500);
  257.                                 //右转
  258.                                 MFSetServoRotaSpd(1,700);
  259.                                 MFSetServoRotaSpd(2,700);
  260.                                 MFSetServoPos(3,617,512);
  261.                                 MFSetServoPos(4,358,512);
  262.                                 MFSetServoPos(5,581,512);
  263.                                 MFServoAction();
  264.                                 DelayMS(200);
  265.                                 //台上状态
  266.                                 MFSetServoRotaSpd(1,400);
  267.                                 MFSetServoRotaSpd(2,-400);
  268.                                 MFSetServoPos(3,617,512);
  269.                                 MFSetServoPos(4,358,512);
  270.                                 MFSetServoPos(5,581,512);
  271.                                 MFServoAction();
  272.                             }
  273.                             else
  274.                             {
  275.                                 //io3
  276.                                 io[3] = MFGetDigiInput(3);
  277.                                 //io5
  278.                                 io[5] = MFGetDigiInput(5);
  279.                                 if ((io[3]&&io[5]==1))
  280.                                 {
  281.                                     //后退
  282.                                     MFSetServoRotaSpd(1,-400);
  283.                                     MFSetServoRotaSpd(2,400);
  284.                                     MFSetServoPos(3,671,512);
  285.                                     MFSetServoPos(4,358,512);
  286.                                     MFSetServoPos(5,581,512);
  287.                                     MFServoAction();
  288.                                     DelayMS(200);
  289.                                     //左转
  290.                                     MFSetServoRotaSpd(1,-700);
  291.                                     MFSetServoRotaSpd(2,-700);
  292.                                     MFSetServoPos(3,617,512);
  293.                                     MFSetServoPos(4,358,512);
  294.                                     MFSetServoPos(5,581,512);
  295.                                     MFServoAction();
  296.                                     DelayMS(200);
  297.                                     //台上状态
  298.                                     MFSetServoRotaSpd(1,400);
  299.                                     MFSetServoRotaSpd(2,-400);
  300.                                     MFSetServoPos(3,617,512);
  301.                                     MFSetServoPos(4,358,512);
  302.                                     MFSetServoPos(5,581,512);
  303.                                     MFServoAction();
  304.                                 }
  305.                                 else
  306.                                 {
  307.                                     //io4
  308.                                     io[4] = MFGetDigiInput(4);
  309.                                     //io6
  310.                                     io[6] = MFGetDigiInput(6);
  311.                                     if ((io[4]&&io[6]==1))
  312.                                     {
  313.                                         //前进
  314.                                         MFSetServoRotaSpd(1,400);
  315.                                         MFSetServoRotaSpd(2,-400);
  316.                                         MFSetServoPos(3,617,512);
  317.                                         MFSetServoPos(4,358,512);
  318.                                         MFSetServoPos(5,581,512);
  319.                                         MFServoAction();
  320.                                         DelayMS(500);
  321.                                         //台上状态
  322.                                         MFSetServoRotaSpd(1,400);
  323.                                         MFSetServoRotaSpd(2,-400);
  324.                                         MFSetServoPos(3,617,512);
  325.                                         MFSetServoPos(4,358,512);
  326.                                         MFSetServoPos(5,581,512);
  327.                                         MFServoAction();
  328.                                     }
  329.                                     else
  330.                                     {
  331.                                         //io3
  332.                                         io[3] = MFGetDigiInput(3);
  333.                                         //io4
  334.                                         io[4] = MFGetDigiInput(4);
  335.                                         //io5
  336.                                         io[5] = MFGetDigiInput(5);
  337.                                         if ((io[3]&&io[4]&&io[5]==1))
  338.                                         {
  339.                                             //右转
  340.                                             MFSetServoRotaSpd(1,700);
  341.                                             MFSetServoRotaSpd(2,700);
  342.                                             MFSetServoPos(3,617,512);
  343.                                             MFSetServoPos(4,358,512);
  344.                                             MFSetServoPos(5,581,512);
  345.                                             MFServoAction();
  346.                                             DelayMS(200);
  347.                                             //台上状态
  348.                                             MFSetServoRotaSpd(1,400);
  349.                                             MFSetServoRotaSpd(2,-400);
  350.                                             MFSetServoPos(3,617,512);
  351.                                             MFSetServoPos(4,358,512);
  352.                                             MFSetServoPos(5,581,512);
  353.                                             MFServoAction();
  354.                                         }
  355.                                         else
  356.                                         {
  357.                                             //io4
  358.                                             io[4] = MFGetDigiInput(4);
  359.                                             //io5
  360.                                             io[5] = MFGetDigiInput(5);
  361.                                             //io6
  362.                                             io[6] = MFGetDigiInput(6);
  363.                                             if ((io[4]&&io[5]&&io[6]==1))
  364.                                             {
  365.                                                 //左转
  366.                                                 MFSetServoRotaSpd(1,-700);
  367.                                                 MFSetServoRotaSpd(2,-700);
  368.                                                 MFSetServoPos(3,617,512);
  369.                                                 MFSetServoPos(4,358,512);
  370.                                                 MFSetServoPos(5,581,512);
  371.                                                 MFServoAction();
  372.                                                 DelayMS(200);
  373.                                                 //台上状态
  374.                                                 MFSetServoRotaSpd(1,400);
  375.                                                 MFSetServoRotaSpd(2,-400);
  376.                                                 MFSetServoPos(3,617,512);
  377.                                                 MFSetServoPos(4,358,512);
  378.                                                 MFSetServoPos(5,581,512);
  379.                                                 MFServoAction();
  380.                                             }
  381.                                             else
  382.                                             {
  383.                                                 //io1
  384.                                                 io[1] = MFGetDigiInput(1);
  385.                                                 if (io[1]==0)
  386.                                                 {
  387.                                                     DelayMS(200);
  388.                                                     //撞击
  389.                                                     MFSetServoRotaSpd(1,900);
  390.                                                     MFSetServoRotaSpd(2,-900);
  391.                                                     MFSetServoPos(3,617,512);
  392.                                                     MFSetServoPos(4,358,512);
  393.                                                     MFSetServoPos(5,581,512);
  394.                                                     MFServoAction();
  395. ……………………

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


微信图片_20170721150432.png (32.4 KB, 下载次数: 102)

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

使用道具 举报

返回列表 发新帖
高级模式
B Color Image Link Quote Code Smilies
您需要登录后才可以回帖 登录 | 立即注册

本版积分规则

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

Powered by 单片机教程网

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