单片机教程网

电脑版
提示:原网页已由神马搜索转码, 内容由www.51hei.com提供.
查看:1797|回复: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. }

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

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


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

  100. void youzhijiaowan()
  101. {
  102.    analogWrite(m1,225);
  103.    analogWrite(m2,0);
  104.    analogWrite(m3,30);
  105.    analogWrite(m4,0);
  106. }
  107. void zuozhijiaowan()
  108. {
  109.    analogWrite(m1,0);
  110.    analogWrite(m2,0);
  111.    analogWrite(m3,250);
  112.    analogWrite(m4,0);
  113. }

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

  190. }

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


评分

黑币 +20
收起理由
+ 20
共享资料的黑币奖励！

查看全部评分

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

Powered by 单片机教程网