标题: Arduino寻迹避障小车程序 [打印本页]
作者: 爱慕春妮    时间: 2021-8-25 21:43
标题: Arduino寻迹避障小车程序
  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.   }
复制代码






欢迎光临 (http://www.51hei.com/bbs/) Powered by Discuz! X3.1