未登录

开通VIP,畅享免费电子书等14项超值服

开通VIP
Arduino智能循迹小车程序 8路循迹 左前右超声避障

2018.09.15

关注
  1. //===============================================================
  2. //雷达消零减为2,加入findspot函,加入checkforward函数,已加入AT==0时回避对方配送站算,已加入角落修正点(AT双向)
  3. //#include

  4. int right1=30;//in1
  5. int right2=31;//in2
  6. int rightpwm=4;//En1


  7. int left1=33;//后片in3
  8. int left2=34;//后片in4
  9. int leftpwm=3;//后片EnB

  10. int EMP1=36;
  11. int EMP2=37;

  12. int LED1=25;
  13. int LED2=22;
  14. int LED3=23;
  15. int LED4=24;

  16. int LEDL=28;
  17. int LEDF=27;
  18. int LEDR=26;
  19. int Color = 29;

  20. int TrigF = A2;
  21. int EchoF = A3;
  22. int TrigL = A4;
  23. int EchoL = A5;
  24. int TrigR = A6;
  25. int EchoR = A7;

  26. int key = A8;
  27. int beep = A9;

  28. const int Sensor1 = 15;
  29. const int Sensor2 = 16;            
  30. const int Sensor3 = 17;
  31. const int Sensor4 = 18;
  32. const int Sensor5 = 19;
  33. const int Sensor6 = 20;
  34. const int SensorF1 = 14;
  35. const int SensorF2 = 21;



  36. int FD = 0;
  37. int RD = 0;
  38. int LD = 0;
  39. int S1;  
  40. int S2;   
  41. int S3;
  42. int S4;
  43. int S5;
  44. int S6;
  45. int SF1;
  46. int SF2;
  47. int C;
  48. int X = 0;
  49. int Y = 0;
  50. int TR = 0;
  51. int TL = 0;
  52. int Z = 0;
  53. int P = 0; //记录拐点数
  54. int AT =0;//AT=0进攻状态,AT=1撤退状态


  55. void setup()
  56. {
  57.   Serial.begin(9600);

  58.   pinMode(left1,OUTPUT);
  59.   pinMode(left2,OUTPUT);
  60.   pinMode(leftpwm,OUTPUT);
  61.   
  62.   pinMode(right1,OUTPUT);
  63.   pinMode(right2,OUTPUT);
  64.   pinMode(rightpwm,OUTPUT);  
  65.   
  66.   pinMode(EMP1,OUTPUT);
  67.   pinMode(EMP2,OUTPUT);
  68.    
  69.   pinMode(LED1,OUTPUT);
  70.   pinMode(LED2,OUTPUT);
  71.   pinMode(LED3,OUTPUT);
  72.   pinMode(LED4,OUTPUT);
  73.   pinMode(LEDL,OUTPUT);
  74.   pinMode(LEDF,OUTPUT);
  75.   pinMode(LEDR,OUTPUT);
  76.   pinMode(Color, INPUT);
  77.   
  78.   pinMode(key, INPUT);
  79.   pinMode(beep, OUTPUT);
  80.   
  81.   pinMode(Sensor1, INPUT);
  82.   pinMode(Sensor2, INPUT);
  83.   pinMode(Sensor3, INPUT);
  84.   pinMode(Sensor4, INPUT);
  85.   pinMode(Sensor5, INPUT);
  86.   pinMode(Sensor6, INPUT);
  87.   pinMode(SensorF1, INPUT);
  88.   pinMode(SensorF2, INPUT);

  89.   pinMode(EchoF, INPUT);
  90.   pinMode(EchoR, INPUT);
  91.   pinMode(EchoL, INPUT);
  92.   pinMode(TrigF, OUTPUT);
  93.   pinMode(TrigR, OUTPUT);
  94.   pinMode(TrigL, OUTPUT);
  95. }

  96. void run(int time)     
  97. {
  98.   digitalWrite(left1,HIGH);
  99.   digitalWrite(left2,LOW);
  100.   digitalWrite(leftpwm,HIGH);
  101.   analogWrite(leftpwm,240);
  102.   
  103.   digitalWrite(right1,HIGH);
  104.   digitalWrite(right2,LOW);
  105.   digitalWrite(rightpwm,HIGH);
  106.   analogWrite(rightpwm,240);

  107.   delay(time * 1);
  108. }

  109. void left(int time)     
  110. {

  111.   digitalWrite(leftpwm,LOW);

  112.   digitalWrite(right1,HIGH);
  113.   digitalWrite(right2,LOW);
  114.   digitalWrite(rightpwm,HIGH);
  115.   analogWrite(rightpwm,240);
  116.   delay(time * 1);
  117. }
  118. void right(int time)     
  119. {
  120.   digitalWrite(left1,HIGH);
  121.   digitalWrite(left2,LOW);
  122.   digitalWrite(leftpwm,HIGH);
  123.   analogWrite(leftpwm,240);
  124.   
  125.   digitalWrite(rightpwm,LOW);

  126.   delay(time * 1);
  127. }


  128. void brake(int time)     
  129. { digitalWrite(left2,HIGH);
  130.   digitalWrite(left1,LOW);
  131.   digitalWrite(leftpwm,HIGH);
  132.   analogWrite(leftpwm,30);
  133.   
  134.   digitalWrite(right2,HIGH);
  135.   digitalWrite(right1,LOW);
  136.   digitalWrite(rightpwm,HIGH);
  137.   analogWrite(rightpwm,30);

  138.   delay(time * 1);
  139. }
  140. //==========================================================
  141. void back(int time)     
  142. {
  143.   digitalWrite(left2,HIGH);
  144.   digitalWrite(left1,LOW);
  145.   digitalWrite(leftpwm,HIGH);
  146.   analogWrite(leftpwm,240);
  147.   
  148.   digitalWrite(right2,HIGH);
  149.   digitalWrite(right1,LOW);
  150.   digitalWrite(rightpwm,HIGH);
  151.   analogWrite(rightpwm,240);
  152.   delay(time * 1);
  153. }

  154. void brakeback(int time)     
  155. {
  156.   digitalWrite(left1,HIGH);
  157.   digitalWrite(left2,LOW);
  158.   digitalWrite(leftpwm,HIGH);
  159.   analogWrite(leftpwm,30);
  160.   
  161.   digitalWrite(right1,HIGH);
  162.   digitalWrite(right2,LOW);
  163.   digitalWrite(rightpwm,HIGH);
  164.   analogWrite(rightpwm,30);

  165.   delay(time * 1);
  166. }

  167. void spinleft(int time)     
  168. {
  169.   digitalWrite(left2,HIGH);
  170.   digitalWrite(left1,LOW);
  171.   digitalWrite(leftpwm,HIGH);
  172.   analogWrite(leftpwm,240);
  173.   
  174.   digitalWrite(right1,HIGH);
  175.   digitalWrite(right2,LOW);
  176.   digitalWrite(rightpwm,HIGH);
  177.   analogWrite(rightpwm,240);
  178.   delay(time * 1);
  179. }

  180. void brakespinleft(int time)     
  181. {
  182. digitalWrite(left1,HIGH);
  183.   digitalWrite(left2,LOW);
  184.   digitalWrite(leftpwm,HIGH);
  185.   analogWrite(leftpwm,30);
  186.   
  187.   digitalWrite(right2,HIGH);
  188.   digitalWrite(right1,LOW);
  189.   digitalWrite(rightpwm,HIGH);
  190.   analogWrite(rightpwm,30);
  191.   delay(time * 1);
  192. }


  193. void spinright(int time)     
  194. {
  195.   digitalWrite(left1,HIGH);
  196.   digitalWrite(left2,LOW);
  197.   digitalWrite(leftpwm,HIGH);
  198.   analogWrite(leftpwm,240);
  199.   
  200.   digitalWrite(right2,HIGH);
  201.   digitalWrite(right1,LOW);
  202.   digitalWrite(rightpwm,HIGH);
  203.   analogWrite(rightpwm,240);
  204.   delay(time * 1);
  205. }

  206. void brakespinright(int time)     
  207. {
  208.   digitalWrite(left2,HIGH);
  209.   digitalWrite(left1,LOW);
  210.   digitalWrite(leftpwm,HIGH);
  211.   analogWrite(leftpwm,240);
  212.   
  213.   digitalWrite(right1,HIGH);
  214.   digitalWrite(right2,LOW);
  215.   digitalWrite(rightpwm,HIGH);
  216.   analogWrite(rightpwm,240);
  217.   delay(time * 1);
  218. }

  219. void stop(int time)   
  220. {
  221.   digitalWrite(leftpwm,LOW);


  222.   digitalWrite(rightpwm,LOW);

  223.   delay(time * 1);   
  224. }


  225. //==========================================================

  226. void keysacn()//按键扫描
  227. {
  228.   int val;
  229.   val = digitalRead(key); //读取电平值赋给val
  230.   while (!digitalRead(key)) //当按键没被按下时,一直循环
  231.   {
  232.     val = digitalRead(key); //此句可省略,可让循环跑空
  233.   }
  234.   while (digitalRead(key)) //当按键被按下时
  235.   {
  236.     delay(10);        //延时10ms
  237.     val = digitalRead(key); //读取电平值赋给val
  238.     if (val == HIGH) //第二次判断按键是否被按下
  239.     {
  240.      // digitalWrite(beep, HIGH);                //蜂鸣器响
  241.      // while (!digitalRead(key))        //判断按键是否被松开
  242.         digitalWrite(beep, LOW);                //蜂鸣器停止
  243.     }
  244.     else
  245.       digitalWrite(beep, LOW); //蜂鸣器停止
  246.   }
  247. }

  248. //==========================================================
  249. void gostraight()
  250. {run(120);}

  251. void turnleft()
  252. {//run(360);
  253.   spinleft(500);
  254. do {spinleft(1);SF1 =digitalRead(SensorF1);SF2 =digitalRead(SensorF2);}
  255. while(SF1==LOW || SF2==LOW);
  256. brakespinleft(50);
  257. stop(10);
  258. }

  259. void turnright()
  260. { //run(360);
  261.   spinright(500);
  262.   do {spinright(1);SF1 =digitalRead(SensorF1);SF2 =digitalRead(SensorF2);}
  263.   while(SF1==LOW || SF2==LOW);
  264.   brakespinright(50);
  265.   stop(10);
  266. }
  267.   
  268. void rightback()
  269. { //run(360);
  270.   spinright(500);
  271.   do {spinright(1);SF1 =digitalRead(SensorF1);SF2 =digitalRead(SensorF2);}
  272.   while(SF1==LOW || SF2==LOW);
  273.   brakespinright(50);
  274.   stop(50);
  275. spinright(500);
  276.   do {spinright(1);SF1 =digitalRead(SensorF1);SF2 =digitalRead(SensorF2);}
  277.   while(SF1==LOW || SF2==LOW);
  278.   brakespinright(50);
  279.   stop(10); }

  280. void leftback()
  281. {//run(360);
  282.   spinleft(500);
  283. do {spinleft(1);SF1 =digitalRead(SensorF1);SF2 =digitalRead(SensorF2);}
  284. while(SF1==LOW || SF2==LOW);
  285. brakespinleft(50);
  286. stop(50);
  287. spinleft(500);
  288. do {spinleft(1);SF1 =digitalRead(SensorF1);SF2 =digitalRead(SensorF2);}
  289. while(SF1==LOW || SF2==LOW);
  290. brakespinleft(50);
  291. stop(10);}

  292. // void findspot()
  293. //{ run(360);}
  294.   
  295.    void checkforward()
  296. {
  297.   
  298.          S1 = digitalRead(Sensor1);
  299.          S2 = digitalRead(Sensor2);
  300.          S3 = digitalRead(Sensor3);
  301.          S4 = digitalRead(Sensor4);
  302.          S5 = digitalRead(Sensor5);
  303.          S6 = digitalRead(Sensor6);
  304.      //   SF1 = digitalRead(SensorF1);
  305.      //   SF2 = digitalRead(SensorF2);
  306.   
  307.       if (S1 == HIGH & S2 == LOW & S3 == LOW & S4 == LOW & S5 == LOW & S6 == LOW)
  308.            { run(100);
  309.             spinright(100);}
  310. else if (S1 == HIGH & S2 == HIGH & S3 == LOW & S4 == LOW & S5 == LOW & S6 == LOW)
  311.             {run(80);
  312.             spinright(80);}  
  313. else if (S1 == LOW & S2 == HIGH & S3 == LOW & S4 == LOW & S5 == LOW & S6 == LOW)
  314.             {run(60);
  315.             spinright(60);}
  316. else if (S1 == LOW & S2 == HIGH & S3 == HIGH & S4 == LOW & S5 == LOW & S6 == LOW)
  317.             {run(40);
  318.             spinright(40);}
  319. else if (S1 == LOW & S2 == LOW & S3 == HIGH & S4 == LOW & S5 == LOW & S6 == LOW)
  320.             {run(20);
  321.             spinright(20);}     

  322. else if (S1 == LOW & S2 == LOW & S3 == LOW & S4 == HIGH & S5 == LOW & S6 == LOW)
  323.             {run(20);
  324.             spinleft(20);}
  325. else if (S1 == LOW & S2 == LOW & S3 == LOW & S4 == HIGH & S5 == HIGH & S6 == LOW)
  326.             {run(40);
  327.             spinleft(40);}
  328. else if (S1 == LOW & S2 == LOW & S3 == LOW & S4 == LOW & S5 == HIGH & S6 == LOW)
  329.             {run(60);
  330.             spinleft(60);}
  331. else if (S1 == LOW & S2 == LOW & S3 == LOW & S4 == LOW & S5 == HIGH & S6 == HIGH)
  332.             {run(80);
  333.             spinleft(80);}
  334. else if (S1 == LOW & S2 == LOW & S3 == LOW & S4 == LOW & S5 == LOW & S6 == HIGH)
  335.             {run(100);
  336.             spinleft(100);}
  337.   else
  338.            stop(1);   
  339.         }



  340. void Distance_testF()   // 量出前方距离
  341. {
  342.   digitalWrite(LEDF, HIGH);
  343.   digitalWrite(TrigF, LOW);   // 给触发脚低电平2μs
  344.   delayMicroseconds(2);
  345.   digitalWrite(TrigF, HIGH);  // 给触发脚高电平10μs,这里至少是10μs
  346.   delayMicroseconds(10);
  347.   digitalWrite(TrigF, LOW);    // 持续给触发脚低电
  348.   float Fdistance = pulseIn(EchoF, HIGH);  // 读取高电平时间(单位:微秒)
  349.   Fdistance= Fdistance/58;
  350.   int FDT = Fdistance;
  351.   FD = FDT<2?100:FDT;
  352.   digitalWrite(LEDF, LOW);

  353. Serial.print('FDistance:');     
  354. Serial.println(FD);

  355. }  

  356. void Distance_testR()   // 量出前方距离
  357. {
  358.   digitalWrite(LEDR, HIGH);
  359.   digitalWrite(TrigR, LOW);   // 给触发脚低电平2μs
  360.   delayMicroseconds(2);
  361.   digitalWrite(TrigR, HIGH);  // 给触发脚高电平10μs,这里至少是10μs
  362.   delayMicroseconds(10);
  363.   digitalWrite(TrigR, LOW);    // 持续给触发脚低电
  364.   float Rdistance = pulseIn(EchoR, HIGH);  // 读取高电平时间(单位:微秒)
  365.   Rdistance= Rdistance/58;
  366.   int RDT = Rdistance;
  367.   RD=RDT<2?100:RDT;
  368.   digitalWrite(LEDR, LOW);

  369.   Serial.print('RDistance:');     
  370.   Serial.println(RD);
  371. }  

  372. void Distance_testL()   // 量出前方距离
  373. { digitalWrite(LEDL, HIGH);
  374.   digitalWrite(TrigL, LOW);   // 给触发脚低电平2μs
  375.   delayMicroseconds(2);
  376.   digitalWrite(TrigL, HIGH);  // 给触发脚高电平10μs,这里至少是10μs
  377.   delayMicroseconds(10);
  378.   digitalWrite(TrigL, LOW);    // 持续给触发脚低电
  379.   float Ldistance = pulseIn(EchoL, HIGH);  // 读取高电平时间(单位:微秒)
  380.   Ldistance= Ldistance/58;
  381.   int LDT = Ldistance;
  382.   LD = LDT<2?100:LDT;
  383.   digitalWrite(LEDL, LOW);

  384.   Serial.print('LDistance:');     
  385.   Serial.println(LD);  
  386. }


  387. //==========================================================

  388. void loop()
  389. {
  390. keysacn();
  391. delay(100);

  392.   TL = 0;
  393.   TR = 0;
  394.   X = 0;
  395.   Y = 0;
  396.   P = 0;
  397.   Z = 0;
  398.   AT = 0;                    
  399.                      
  400.                        

  401. while (1)
  402.   {
  403.     switch ((TR - TL) % 4)//4灯顺序按逆时针1234排布
  404.                           { case 0: digitalWrite(LED1, HIGH);
  405.                                     digitalWrite(LED2, LOW);
  406.                                     digitalWrite(LED3, LOW);
  407.                                     digitalWrite(LED4, LOW);
  408.                                     break;
  409.                             case 1: digitalWrite(LED1, LOW);
  410.                                     digitalWrite(LED2, HIGH);
  411.                                     digitalWrite(LED3, LOW);
  412.                                     digitalWrite(LED4, LOW);
  413.                                     break;
  414.                             case 2: digitalWrite(LED1, LOW);
  415.                                     digitalWrite(LED2, LOW);
  416.                                     digitalWrite(LED3, HIGH);
  417.                                     digitalWrite(LED4, LOW);
  418.                                     break;
  419.                             case 3: digitalWrite(LED1, LOW);
  420.                                     digitalWrite(LED2, LOW);
  421.                                     digitalWrite(LED3, LOW);
  422.                                     digitalWrite(LED4, HIGH);
  423.                                     break;
  424.                             case -1:digitalWrite(LED1, LOW);
  425.                                     digitalWrite(LED2, LOW);
  426.                                     digitalWrite(LED3, LOW);
  427.                                     digitalWrite(LED4, HIGH);
  428.                                     break;
  429.                             case -2:digitalWrite(LED1, LOW);
  430.                                     digitalWrite(LED2, LOW);
  431.                                     digitalWrite(LED3, HIGH);
  432.                                     digitalWrite(LED4, LOW);
  433.                                     break;
  434.                             case -3:digitalWrite(LED1, LOW);
  435.                                     digitalWrite(LED2, HIGH);
  436.                                     digitalWrite(LED3, LOW);
  437.                                     digitalWrite(LED4, LOW);
  438.                                     break;}
  439.     S1 = digitalRead(Sensor1);
  440.     S2 = digitalRead(Sensor2);
  441.     S3 = digitalRead(Sensor3);
  442.     S4 = digitalRead(Sensor4);
  443.     S5 = digitalRead(Sensor5);
  444.     S6 = digitalRead(Sensor6);
  445.     SF1 = digitalRead(SensorF1);
  446.     SF2 = digitalRead(SensorF2);
  447.     C = digitalRead(Color);
  448.                   
  449.    
  450.    // SB1 = digitalRead(SensorB1);
  451.    // SB2 = digitalRead(SensorB2);
  452. //*******************************直路行驶开始***********************************
  453.     if (S1 == LOW & S2 == LOW & S3 == HIGH & S4 == HIGH & S5 == LOW & S6 == LOW)
  454.             run(1);
  455.    else if (S1 == LOW & S2 == LOW & S3 == HIGH & S4 == LOW & S5 == LOW & S6 == LOW) //4、LLHLLL,直路修偏
  456.              spinleft(1);
  457.    else if (S1 == LOW & S2 == LOW & S3 == LOW & S4 == HIGH & S5 == LOW & S6 == LOW) //5、LLLHLL,直路修偏
  458.              spinright(1);
  459. //*******************************直路行驶结束***********************************  
  460. //*******************************进入右丁字路口***********************************
  461.    else if (S1 == LOW & S2 == LOW &(S3 == HIGH || S4 == HIGH) & S5 == HIGH & S6 == HIGH)
  462.       {            
  463.             turnright(); }
  464.                
  465.            
  466. //********************右丁字路口结束************************
  467. //********************左丁字路口开始************************
  468. else if (S1 == HIGH & S2 == HIGH & (S3 == HIGH || S4 == HIGH) & S5 == LOW & S6 == LOW)
  469.      {  turnleft();}
  470.   
  471. //********************左丁字路口结束************************
  472. //********************进入十字路口************************   
  473.     else if (S1 == HIGH & S2 == HIGH &( S3 == HIGH || S4 == HIGH) & S5 == HIGH & S6 == HIGH)
  474.                    {
  475.                      gostraight();   }
  476.   //********************十字路口结束************************   
  477.   //********************进入前丁字路口************************  
  478.    else if (S1 == HIGH & S2 == HIGH & S3 == LOW & S4 == LOW & S5 == HIGH & S6 == HIGH)
  479.        {              
  480.                rightback();   ; }
  481.                               
  482. //********************前丁字路口结束************************        
  483. //********************进入右拐角************************  
  484.    else if (S1 == LOW  & S3 == LOW & S4 == LOW & S5 == HIGH & S6 == HIGH)
  485.     {               
  486.                    turnright() ; }
  487. //********************右拐角结束************************
  488. //********************进入左拐角************************      
  489.          else if (S1 == HIGH & S2 == HIGH & S3 == LOW & S4 == LOW & S6 == LOW)
  490.                {      
  491.                     turnleft();    }
  492. //********************左拐角结束************************   
  493.    
  494.       else if (S1 == LOW & S2 == HIGH & S3 == HIGH & S4 == LOW & S5 == LOW & S6 == LOW) //11、LHHLLL:直道右偏较多
  495.         left(1);
  496.       else if (S1 == LOW & S2 == LOW & S3 == LOW & S4 == HIGH & S5 == HIGH & S6 == LOW) //12、LLLHHL:直道左偏较多
  497.         right(1);
  498.       
  499.       else if (S1 == LOW & S2 == HIGH & S3 == LOW & S4 == LOW & S5 == LOW & S6 == LOW) //13、LHLLLL:直道右偏太多
  500.         spinleft(1);
  501.       else if (S1 == LOW & S2 == LOW & S3 == LOW & S4 == LOW & S5 == HIGH & S6 == LOW) //14、LLLLHL:直道左偏太多
  502.         spinright(1);
  503.     //  else if (S1 == LOW & S2 == LOW & S3 == HIGH & S4 == HIGH & S5 == HIGH & S6 == LOW) //LLHHHL:刚前出右丁路口时
  504.     //    left(1);
  505.     // else if (S1 == LOW & S2 == HIGH & S3 == HIGH & S4 == HIGH & S5 == LOW & S6 == LOW)  //LHHHLL:刚前出左丁路口时
  506.     //    right(1);
  507.       else if (S1 == HIGH & S2 == LOW & S3 == LOW & S4 == LOW & S5 == LOW & S6 == LOW)//17、HLLLLL:行车出现巨大偏差,加这一句用来保险
  508.         left(1);
  509.       else if (S1 == LOW & S2 == LOW & S3 == LOW & S4 == LOW & S5 == LOW & S6 == HIGH)//18、LLLLLH:行车出现巨大偏差,加这一句用来保险
  510.         right(1);
  511.       else if (S1 == LOW & S2 == LOW & S3 == LOW & S4 == LOW & S5 == LOW & S6 == LOW) //20、LLLLLL:脱轨
  512.         back(1);

  513.       else
  514.         run(1);
  515.    

  516. }
  517. }




本站仅提供存储服务,所有内容均由用户发布,如发现有害或侵权内容,请点击举报
打开APP阅读全文并永久保存 更多类似文章
猜你喜欢
类似文章
Arduino小车
微型直流电机控制基本方法 L298N模块
【教程】手把手教你,让你的模型动起来
[转载]Arduino实现315M无线模块控制双路继电器
图形化编程娱乐于教,Kittenblock 实例,arduino点亮和关闭小灯
基础教程2 Arduino 让多个LED 炫酷地闪
更多类似文章 >>
生活服务
绑定账号成功
后续可登录账号畅享VIP特权!
如果VIP功能使用有故障,
可点击这里联系客服!