找回密码
 立即注册

QQ登录

只需一步,快速开始

搜索
查看: 1479|回复: 0
收起左侧

Arduino寻迹避障小车程序

[复制链接]
ID:882006 发表于 2021-8-25 21:43 | 显示全部楼层 |阅读模式
  1. int m1=5;
  2. int m2=6;
  3. int m3=9;
  4. int m4=3;
  5. #define TRIG 7
  6. #define ECHO 11
  7. float cm;
  8. float tmp;
  9. float juli;
  10. int out1=A0;
  11. int out2=A1;
  12. int out3=A2;
  13. int out4=A3;
  14. int out5=A4;
  15. int out[5]={0,0,0,0,0,};
  16. int i;
  17. int hx;
  18. int b=0;
  19. int bx;
  20. void setup()
  21. {
  22.    pinMode(m1,OUTPUT);
  23.    pinMode(m2,OUTPUT);
  24.    pinMode(m3,OUTPUT);
  25.    pinMode(m4,OUTPUT);
  26.    Serial.begin(9600);
  27.    pinMode(TRIG,OUTPUT);
  28.    pinMode(ECHO,INPUT);
  29.    pinMode(out1,INPUT);
  30.    pinMode(out2,INPUT);
  31.    pinMode(out3,INPUT);
  32.    pinMode(out4,INPUT);
  33.    pinMode(out5,INPUT);
  34.    i=0;
  35. }
  36. void distance()//定义距离
  37. {
  38.     digitalWrite(TRIG,LOW);
  39.     delayMicroseconds(2);
  40.     digitalWrite(TRIG,HIGH);
  41.     delayMicroseconds(10);
  42.     digitalWrite(TRIG,LOW);
  43.     tmp=float(pulseIn(ECHO,HIGH));
  44.     cm=(tmp*17)/1000;
  45.     Serial.print("Echo=");
  46.     Serial.print(tmp);
  47.     Serial.print(",距离=");
  48.     Serial.print(cm);
  49.     Serial.println("cm");
  50.     juli=cm;
  51. }
  52. void pao()//前进
  53. {
  54.      analogWrite(m1,140);
  55.      analogWrite(m2,0);
  56.      analogWrite(m3,140 );
  57.      analogWrite(m4,0);
  58. }

  59. void zuozhuan()
  60. {
  61.   analogWrite(m1,140);
  62.   analogWrite(m2,0);         
  63.   analogWrite(m3,30);
  64.   analogWrite(m4,0);        
  65. }

  66. void youzhuan()
  67. {
  68.   analogWrite(m1,30);
  69.   analogWrite(m2,0);         
  70.   analogWrite(m3,160);
  71.   analogWrite(m4,0);      
  72. }


  73. void youzhuanj()
  74. {
  75.      analogWrite(m1,0);
  76.      analogWrite(m2,0);
  77.      analogWrite(m3,180);
  78.      analogWrite(m4,0);
  79. }
  80. void zuozhuanj()
  81. {
  82.      analogWrite(m1,150);
  83.      analogWrite(m2,0);
  84.      analogWrite(m3,0);
  85.      analogWrite(m4,0);
  86. }
  87. void shache()//定义刹车
  88. {
  89.     analogWrite(m1,0);
  90.     analogWrite(m2,0);
  91.     analogWrite(m3,0);
  92.     analogWrite(m4,0);
  93.    
  94. }

  95. void youzhijiaowan()
  96. {
  97.     analogWrite(m1,225);
  98.     analogWrite(m2,0);
  99.     analogWrite(m3,30);
  100.     analogWrite(m4,0);
  101. }
  102. void zuozhijiaowan()
  103. {
  104.    analogWrite(m1,0);
  105.     analogWrite(m2,0);
  106.     analogWrite(m3,250);
  107.     analogWrite(m4,0);
  108. }

  109. void xunji()
  110. {
  111.        out[0] = digitalRead(out1);
  112.        out[1] = digitalRead(out2);
  113.        out[2] = digitalRead(out3);
  114.        out[3] = digitalRead(out4);
  115.        out[4] = digitalRead(out5);
  116.        if ( ((out[0] == 0) && (out[1] == 0)&&(out[2] == 0) && (out[3] == 0) && (out[4] == 0)) )
  117.        {
  118.         i=i++;
  119.         if(i==2)
  120.          {
  121.           shache();
  122.           delay(200);
  123.           zuozhijiaowan();
  124.           delay(200);
  125.          }
  126.          else  if(i>=3)
  127.          {
  128.            pao();
  129.            delay(20);
  130.            shache();
  131.          
  132.          }
  133.          else
  134.          pao();
  135.          
  136.         
  137.        }
  138.       else if ((out[0] == 1) && (out[1] == 1) && (out[2] == 1) && (out[3] == 1) && (out[4] == 1))
  139.       
  140.       {
  141.         
  142.         if(bx==0)
  143.         {
  144.           delay(200);
  145.           pao();
  146.         }
  147.       
  148.       }
  149.      
  150.      else if ((out[0] == 1) && (out[1] == 1) && (out[2] == 0) && (out[3] == 1) && (out[4] == 1))
  151.      {
  152.         pao();
  153.      }
  154.     else if ((out[0] == 1) && (out[1] == 0) && (out[2] == 1) && (out[3] == 1) && (out[4] == 1))
  155.     {
  156.      
  157.        youzhuan();
  158.       
  159.     }
  160.     else if ((out[0] == 0) && (out[1] == 1) && (out[2] == 1) && (out[3] == 1) && (out[4] == 1))
  161.     {
  162.       youzhuan();  
  163.      if ((out[0] == 1) && (out[1] == 1) && (out[2] == 1) && (out[3] == 1) && (out[4] == 1))
  164.     {
  165.      zuozhuan();
  166.    
  167.     }
  168.       }
  169.     else if ((out[0] == 1) && (out[1] == 1) && (out[2] == 1) && (out[3] == 0) && (out[4] == 1))
  170.     {
  171.     zuozhuan();
  172.     }
  173.    else if ((out[0] == 1) && (out[1] == 1) && (out[2] == 1) && (out[3] == 1) && (out[4] == 0))
  174.     {
  175.     zuozhuanj();
  176.       if ((out[0] == 1) && (out[1] == 1) && (out[2] == 1) && (out[3] == 1) && (out[4] == 1))
  177.     {
  178.      zuozhuan();
  179.    
  180.     }
  181.     }
  182.    

  183. }

  184. void duankou()
  185. {
  186.      out[0] = digitalRead(out1);
  187.      out[1] = digitalRead(out2);
  188.      out[2] = digitalRead(out3);
  189.      out[3] = digitalRead(out4);
  190.      out[4] = digitalRead(out5);
  191. }
  192. void bizhang()
  193. {
  194.    while(1)
  195.    {
  196.     distance();
  197.     if(juli<=60&&juli>10)
  198.     {
  199.       if(juli<35&&juli>10)
  200.       {
  201.        shache();
  202.        youzhijiaowan();
  203.        delay(70);
  204.         distance();
  205.         if(juli>60)
  206.       
  207.         {
  208.           while(1)
  209.           {
  210.             pao();
  211.             duankou();
  212.             if ((out[0] == 0) || (out[1] == 0) || (out[2] == 0))  
  213.                  {
  214.                       shache();
  215.                       delay(200);
  216.                       zuozhijiaowan();
  217.                       delay(150);
  218.                       break;
  219.                   }
  220.             
  221.           }
  222.                
  223.         }
  224.         
  225.       }
  226.       else
  227.       xunji();
  228.      }
  229.      else
  230.     xunji();
  231.    }
  232. }
  233. void loop()
  234.   {
  235.    bizhang();
  236.   }
复制代码


评分

参与人数 1黑币 +20 收起 理由
admin + 20 共享资料的黑币奖励!

查看全部评分

回复

使用道具 举报

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

本版积分规则

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

Powered by 单片机教程网

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