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

 找回密码
 立即注册

QQ登录

只需一步，快速开始

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

搜索
»论坛 › 嵌入式/单片机论坛 › 24小时必答区 › 循迹小车出弯道时出现左右摇摆的现象，该如何解决？附单 ...
返回列表 发新帖
查看: 3098|回复: 0
打印 上一主题 下一主题
收起左侧

循迹小车出弯道时出现左右摇摆的现象，该如何解决？附单片机程序

[复制链接]
跳转到指定楼层
楼主
ID:430606 发表于 2018-12-21 17:55 | 只看该作者 回帖奖励 |倒序浏览 |阅读模式
循迹小车出弯道时出现左右摇摆的现象，该如何解决？车子前后使用两个五路红外对管传感器
  1. //=======================================================
  2. //  975=转向轮向左转     车身左侧电机1
  3. //=======================================================
  4. #include <Servo.h>   
  5. #define SER1_BAUD 115200
  6. /*电机驱动引脚*/
  7. #define PWMB_IN1  11       //定义IN1引脚
  8. #define PWMB_IN2 6       //定义IN2引脚
  9. #define PWMA_IN1 5        //定义IN3引脚
  10. #define PWMA_IN2 3        //定义IN4引脚
  11. #define Servor_Pin    7
  12. #define  BEEP_PIN    12             //定义蜂鸣器引脚D6
  13. #define  NLED_PIN    13           //定义呼吸灯引脚D11
  14. #define IR1   A1
  15. #define IR2   A0
  16. #define IR3   A3
  17. #define IR4   A2
  18. #define IR5   10
  19. #define IH2   0
  20. #define IH3   A5
  21. #define IH4   A4
  22. #define  NLED_MS_BETWEEN        500
  23. #define  DUOJI_MS_BETWEEN       20.000
  24. int Out1,Out2,Out3,Out4,Out5,Out2H,Out3H,Out4H;
  25. int pwm_value=1435;
  26. int pwm_R=1140;
  27. int pwm_L=1860;
  28. int Echo =12;
  29. int Trig =13;
  30. int Distance = 50;
  31. double sv=130;         //直线速度
  32. double bv=130;        //24转速
  33. double cv=140;        //curve弯道速度
  34. int c1=0;         //矫正指示值
  35. int c5=0;
  36. int fore=0;       //前部判断
  37. int f=0;          //前部触发
  38. int dj=0;          //舵机
  39. int hou=0;        //尾部判断
  40. int h=0;          //尾部光电
  41. int z=0;          //障碍位置设定L==0,R==1
  42. double v=1;       //累计车速
  43. char val=0;
  44. double num1=1;
  45. double num2=1;
  46. double num3=1;
  47. double num4=1;
  48. double num5=1;
  49. double n1=1;
  50. double n2=1;
  51. double n4=1;
  52. double n5=1;
  53. double st=1;    //stop定时
  54. unsigned long time1;
  55. unsigned long timel=0;
  56. unsigned long timer=0;
  57. unsigned long dif=0;
  58. Servo myservo;           //创建舵机伺服对象数组
  59. unsigned char handle_ms_between( unsigned long *time_ms, unsigned int ms_between);
  60. //void handle_nled();
  61. void Motor_Forward(char motor,char pwm);
  62. void Motor_Back(char motor,char pwm); //电机反转
  63. void Motor_Right();
  64. void Motor_Left();
  65. void Distance_test();
  66. void dida(u8 times, u8 frequency);
  67. void rear_judge();
  68. void fore_judge();
  69. void correct();
  70. void R_count();
  71. void L_count();
  72. void dis();
  73. void detect();
  74. void count();
  75. void setup()
  76. {
  77.    pinMode(BEEP_PIN, OUTPUT);
  78. //  pinMode(NLED_PIN, OUTPUT);
  79.    pinMode(IR1, INPUT);
  80.    pinMode(IR2, INPUT);
  81.    pinMode(IR3, INPUT);
  82.    pinMode(IR4, INPUT);
  83.    pinMode(IR5, INPUT);
  84.    pinMode(IH2, INPUT);
  85.    pinMode(IH3, INPUT);
  86.    pinMode(IH4, INPUT);
  87.    pinMode(Echo, INPUT);   
  88.    pinMode(Trig, OUTPUT);  
  89.    Serial.begin(SER1_BAUD);
  90.    myservo.attach(Servor_Pin);
  91.    myservo.writeMicroseconds(pwm_value);
  92.    dida(1, 1000);
  93.    delay(1000);
  94. }

  96. void loop()
  97. {
  98.     while(1)
  99.     {
  100.         Distance_test();
  101.         time1=millis();
  102.         Serial.print("time:");
  103.         detect();
  104.         fore_judge();                       //前轮位置判断
  105.         rear_judge();                       //后轮位置判断
  106.         count();                            //计时  
  107.       //  dif=abs(timel-timer);
  108.        //  Serial.print("timer:");
  109.        // Serial.println(timer);
  110.        // Serial.print("timel:");
  111.        // Serial.println(timel);
  112.        //if(dif<=1600 && dif>=10)
  113.      //  {
  114.      //  correct();
  115.        // Motor_Forward(1,255);
  116.        // Motor_Forward(2,255);
  117.        // delay(5000);
  118.         // Serial.print("dif:");
  119.        //    Serial.println(dif);
  120.     //   }
  121.     //   else
  122.      //  {;}
  123.      //   Serial.println(c1);
  124.          
  125. //=======================================================
  126. //  主程序
  127. //=======================================================   
  128.         if(Out1==1 && Out2==1 && Out3==0 && Out4==1 && Out5==1)   //直行
  129.         {
  130.            v=num3;
  131.            if(hou==3)
  132.            {
  133.            Motor_Forward(1,sv-v);
  134.            Motor_Forward(2,sv-v);
  135.            myservo.writeMicroseconds(pwm_value);
  136.            }
  137.             else
  138.            {
  139.             v=num3;
  140.             v=v*0.9;
  141.            Motor_Forward(1,sv-v);
  142.            Motor_Forward(2,sv-v);
  143.            }
  144.         }
  145.         
  146.          else if(Out1==0 && Out2==1 && Out3==1 && Out4==1 && Out5==1)  //1左转
  147.         {
  148.           while( Out4==1 )
  149.         {
  150.            Motor_Left();
  151.            timel=millis();
  152.          }
  153.           num3=0;
  154.          }
  155.         
  156.         else if(Out1==1 && Out2==1 && Out3==1 && Out4==0 && Out5==1)  //4右转
  157.         {
  158.             v=num3;
  159.             v=v*0.7;
  160.             dj=1;
  161.           if(hou==3)
  162.           {
  163.            Motor_Forward(1,bv-v);                           //bv=160,v=110,num=20
  164.            Motor_Forward(2,bv-20-v-num4);
  165.            myservo.writeMicroseconds(pwm_value-80);
  166.            }
  167.            else if(hou==4 && h==0)
  168.            {
  169.            Motor_Forward(1,bv-v);
  170.            Motor_Forward(2,bv-20-v-num4);
  171.            myservo.writeMicroseconds(pwm_value-40);
  172.            }
  173.             else if(hou==2 && h==0)
  174.            {
  175.               Motor_Forward(1,bv-v);
  176.               Motor_Forward(2,bv-20-v-num4);
  177.               myservo.writeMicroseconds(pwm_value-130);
  178.            }
  179.            else if(h==1 && hou==4)
  180.            {
  181.               Motor_Forward(1,bv-v);
  182.               Motor_Forward(2,bv-20-v-num4);
  183.               myservo.writeMicroseconds(pwm_value);
  184.             }
  185.              else if(h==1 && hou==2)//-
  186.            {
  187.               Motor_Forward(1,bv-20-v-num4);
  188.               Motor_Forward(2,bv-v);
  189.               myservo.writeMicroseconds(pwm_value-180);
  190.             }
  191.             else
  192.            {
  193.             v=num3;
  194.             Motor_Forward(1,bv-v);
  195.             Motor_Forward(2,bv-v);
  196.            }
  197.         }
  198.         
  199.         else if(Out1==1 && Out2==1 && Out3==1 && Out4==1 && Out5==0)  //5右转
  200.         {
  201.           while( Out2==1)
  202.         {
  203.           Motor_Right();
  204.           timer=millis();
  205.         }   
  206.         num3=0;
  207.         }
  208.         
  209.         else if(Out1==1 && Out2==0 && Out3==1 && Out4==1 && Out5==1)  //2左转
  210.         {
  211.           v=num3;
  212.           v=v*0.7;
  213.           dj=1;
  214.            if(hou==3)             //bv=160,v=110,num=20
  215.           {
  216.            Motor_Forward(1,bv-20-v-num2);
  217.            Motor_Forward(2,bv-v);
  218.            myservo.writeMicroseconds(pwm_value+80);
  219.            }
  220.            else if(hou==2 && h==0)
  221.            {
  222.            Motor_Forward(1,bv-20-v-num2);
  223.            Motor_Forward(2,bv-v);
  224.            myservo.writeMicroseconds(pwm_value+40);
  225.            }
  226.             else if(hou==4 && h==0)
  227.            {
  228.               Motor_Forward(1,bv-v);
  229.               Motor_Forward(2,bv-30-v);
  230.               myservo.writeMicroseconds(pwm_value+130);
  231.            }
  232.            else if(h==1 && hou==2)
  233.            {
  234.               Motor_Forward(1,bv-20-v-num2);
  235.               Motor_Forward(2,bv-v);
  236.               myservo.writeMicroseconds(pwm_value);
  237.             }
  238.              else if(h==1 && hou==4)
  239.            {
  240.               Motor_Forward(1,bv-v);
  241.               Motor_Forward(2,bv-20-v-num2);
  242.               myservo.writeMicroseconds(pwm_value+180);
  243.             }
  244.             else
  245.             {
  246.             Motor_Forward(1,bv-v);
  247.             Motor_Forward(2,bv-v);
  248.             }
  249.         }
  250.        else if(Out1==0 && Out2==0 && Out3==0 && Out4==0 && Out5==0)    //stop计数
  251.         {
  252.           if(st>=100)
  253.           {
  254.             Motor_Forward(1,255);
  255.             Motor_Forward(2,255);
  256.            break;//还是用return
  257.             }
  258.             else
  259.             {;}
  260.         }
  261.        else
  262.        {
  263.          v=num3;
  264.          Motor_Forward(1,sv-v);
  265.          Motor_Forward(2,sv-v);
  266.         }
  267.         dis();
  268.     }
  269. }


  272. void Motor_Right()                      //5右转调用程序   wandaozhuanjiaodizeng
  273. {
  274.          detect();
  275.          fore_judge();
  276.          rear_judge();
  277.          R_count();
  278.          if(Out1==1 && Out2==1 && Out3==1 && Out4==1 && Out5==0)
  279.          {
  280.           if(dj==1 && h==0)
  281.           {
  282.            Motor_Forward(1,170);
  283.            Motor_Forward(2,80);
  284.            myservo.writeMicroseconds(pwm_value-360);     
  285.           }
  286.           else if(dj==1 && h==1 && hou==3)
  287.           {
  288.            Motor_Forward(1,170);
  289.            Motor_Forward(2,80);     
  290.            myservo.writeMicroseconds(pwm_value-360);
  291.           }
  292.           else if(dj==1 && h==1 && hou==2)    //-
  293.           {
  294.            Motor_Forward(1,170);
  295.            Motor_Forward(2,80);
  296.            myservo.writeMicroseconds(pwm_value-360);
  297.           }
  298.           else if(dj==1 && h==1 && hou==4)
  299.           {
  300.            Motor_Forward(1,170);
  301.            Motor_Forward(2,80);
  302.            myservo.writeMicroseconds(pwm_value-360);
  303.             }
  304.            else          //*******
  305.           {
  306.            Motor_Forward(1,160);
  307.            Motor_Forward(2,80);
  308.            myservo.writeMicroseconds(pwm_value-360);
  309.             }
  310.            //Serial.print("55");
  311.         }
  312.          else if(Out1==1 && Out2==1 && Out3==1 && Out4==0 && Out5==1)
  313.         {   
  314.            dj=0;
  315.            Motor_Forward(1,cv-n4);                 //cv=150
  316.            Motor_Forward(2,cv-60-n4);
  317.            myservo.writeMicroseconds(pwm_value-310);
  318.          //  Serial.print("54");
  319.         }
  320.          else if(Out1==1 && Out2==1 && Out3==0 && Out4==1 && Out5==1)
  321.         {
  322.            Motor_Forward(1,cv);
  323.            Motor_Forward(2,cv-30);
  324.            myservo.writeMicroseconds(pwm_value-130);
  325.          //  Serial.print("53");
  326.         }
  327.          else if(Out1==1 && Out2==1 && Out3==1 && Out4==1 && Out5==1 && f==1 && fore==0)
  328.         {
  329.            dj=0;
  330.           if(h==0 )
  331.           {
  332.            Motor_Forward(1,170);
  333.            Motor_Forward(2,100);
  334.            myservo.writeMicroseconds(pwm_value-450);   
  335.          //  Serial.print("zou");
  336.           }
  337.           else if(h==1 && hou==3)
  338.           {
  339.            Motor_Forward(1,170);
  340.            Motor_Forward(2,100);
  341.            myservo.writeMicroseconds(pwm_value-450);   
  342.          //  Serial.print("zou");
  343.           }
  344.            else if(h==1 && hou!=3)   //-
  345.           {
  346.            Motor_Forward(1,170);
  347.            Motor_Forward(2,100);
  348.            myservo.writeMicroseconds(pwm_value-450);   
  349.          //  Serial.print("zou");
  350.           }
  351.            else
  352.            {
  353.            Motor_Forward(1,cv);
  354.            Motor_Forward(2,cv);
  355.            }
  356.         }   
  357.           else
  358.        {
  359.          Motor_Forward(1,cv);
  360.          Motor_Forward(2,cv);
  361.         }
  362. }
  363. void Motor_Left()                    //1左转调用程序
  364. {
  365.         detect();
  366.         fore_judge();
  367.         rear_judge();
  368.         L_count();
  369.        if(Out1==0 && Out2==1 && Out3==1 && Out4==1 && Out5==1)
  370.         {
  371.           if(dj==1 && h==0)
  372.           {
  373.            myservo.writeMicroseconds(pwm_value+360);
  374.            Motor_Forward(1,80);
  375.            Motor_Forward(2,170);
  376.           }
  377.           else if(dj==1 && h==1 && hou==3)
  378.           {
  379.            myservo.writeMicroseconds(pwm_value+360);
  380.            Motor_Forward(1,80);
  381.            Motor_Forward(2,170);
  382.           }
  383.           else if(dj==1 && h==1 && hou==2)
  384.           {
  385.            Motor_Forward(1,80);
  386.            Motor_Forward(2,170);
  387.            myservo.writeMicroseconds(pwm_value+360);
  388.           }
  389.           else if(dj==1 && h==1 && hou==4)
  390.           {
  391.            Motor_Forward(1,80);
  392.            Motor_Forward(2,170);
  393.            myservo.writeMicroseconds(pwm_value+360);
  394.            }
  395.           else
  396.           {
  397.            Motor_Forward(1,120);
  398.            Motor_Forward(2,160);
  399.            myservo.writeMicroseconds(pwm_value+360);
  400.             }
  401.         }   
  402.            else if(Out1==1 && Out2==0 && Out3==1 && Out4==1 && Out5==1)
  403.         {
  404.           dj=0;
  405.            Motor_Forward(1,cv-60-n2);//-
  406.            Motor_Forward(2,cv-n2);
  407.            myservo.writeMicroseconds(pwm_value+310);
  408.          //  Serial.print("12");  
  409.         }
  410.         else if(Out1==1 && Out2==1 && Out3==0 && Out4==1 && Out5==1)
  411.         {
  412.            Motor_Forward(1,cv-30);//-
  413.            Motor_Forward(2,cv);
  414.            myservo.writeMicroseconds(pwm_value+130);
  415.         //   Serial.print("13");  
  416.         }
  417.          else if(Out1==1 && Out2==1 && Out3==1 && Out4==1 && Out5==1 && f==1 && fore==0)//-
  418.         {
  419.            dj=0;
  420.           if(h==0)
  421.           {
  422.            Motor_Forward(1,100);
  423.            Motor_Forward(2,170);
  424.            myservo.writeMicroseconds(pwm_value+450);   
  425.          //  Serial.print("you");
  426.            }
  427.            else if(h==1 && hou==3)
  428.             {
  429.            Motor_Forward(1,100);
  430.            Motor_Forward(2,170);
  431.            myservo.writeMicroseconds(pwm_value+450);   
  432.          //  Serial.print("you");
  433.            }
  434.            else if(h==1 && hou!=3)//-
  435.            {
  436.            Motor_Forward(1,100);
  437.            Motor_Forward(2,170);
  438.            myservo.writeMicroseconds(pwm_value+450);
  439.             }
  440.              else
  441.            {
  442.             Motor_Forward(1,160);
  443.             Motor_Forward(2,160);
  444.         }
  445.            }
  446.              else
  447.            {
  448.             Motor_Forward(1,cv);
  449.             Motor_Forward(2,cv);
  450.         }
  451. }
  452. void correct()
  453. {      
  454.   detect();
  455.   if(c1==1)
  456.   {
  457.     while(Out3==0)
  458.     {
  459.       Out3 = digitalRead(IR3);
  460.       myservo.writeMicroseconds(pwm_value+100);
  461.       Motor_Forward(1,170);
  462.       Motor_Forward(2,230);
  463.     }
  464.   }
  465.    else if(c5==1)
  466.   {
  467.     while(Out3==0)
  468.     {
  469.       Out3 = digitalRead(IR3);
  470.       myservo.writeMicroseconds(pwm_value-100);
  471.       Motor_Forward(1,230);
  472.       Motor_Forward(2,170);
  473.     }
  474.   }
  475.   else
  476.   {;}
  477.   }
  478. void rear_judge()
  479. {
  480.    if(Out2H==0 && Out3H==1 && Out4H==1)
  481.    {  hou=2;h=0;  }
  482.    else if(Out2H==1 && Out3H==0 && Out4H==1)
  483.    {  hou=3; h=0; }
  484.    else if(Out2H==1 && Out3H==1 && Out4H==0)
  485.    {  hou=4; h=0;  }
  486.     else
  487.     {h=1;}
  488.   }
  489.   void fore_judge()
  490.   {
  491.    if (Out2==0 || Out3==0 || Out4==0)
  492.    {
  493.     fore=1;
  494.     f=0;
  495.     }
  496.     else if(Out1==0 && Out2==1 && Out3==1 && Out4==1 && Out5==1)
  497.     {
  498.       fore=0;f=0;
  499.       c1=1;
  500.       c5=0;
  501.       }
  502.     else if(Out1==1 && Out2==1 && Out3==1 && Out4==1 && Out5==0)
  503.     {
  504.       fore=0;f=0;
  505.       c5=1;
  506.       c1=0;
  507.       }
  508.     else
  509.     {
  510.       f=1;
  511.       }
  512.     }
  513. void dis()
  514. {
  515.   if(Distance < 35)  //避障
  516.   {
  517.    while(Distance<35)//再次判断是否有障碍物，若有则转动方向后，继续判断
  518.   {   
  519.      Distance_test();//测量前方距离
  520.     if(z==1)
  521.     {
  522.      Motor_Forward(1,200);
  523.      Motor_Forward(2,150);
  524.      myservo.writeMicroseconds(1160);//        右转 400ms
  525.      }
  526.     else
  527.     {
  528.      Motor_Forward(1,200);
  529.      Motor_Forward(2,150);
  530.      myservo.writeMicroseconds(1700);//        右转 400ms
  531.     }
  532.    }
  533.     if(z==0)
  534.     {
  535.      Motor_Forward(1,180);
  536.      Motor_Forward(2,180);
  537.      myservo.writeMicroseconds(1430);//        右转 400ms
  538.      delay(1000);
  539.      }
  540.     else
  541.     {
  542.      Motor_Forward(1,180);
  543.      Motor_Forward(2,180);
  544.      myservo.writeMicroseconds(1430);//        右转 400ms
  545.      delay(1000);
  546.     }
  547.     if(z==0)
  548.     {
  549.      Out5 = digitalRead(IR5);
  550.      while(Out5==0)
  551.      {
  552.      Out5 = digitalRead(IR5);
  553.      Motor_Forward(1,200);
  554.      Motor_Forward(2,150);
  555.      myservo.writeMicroseconds(1160);//        右转 400ms
  556.      }
  557.     }
  558.     else
  559.     {
  560.      Out1 = digitalRead(IR1);
  561.      while(Out1==0)
  562.      {
  563.      Out1 = digitalRead(IR1);
  564.      Motor_Forward(1,200);
  565.      Motor_Forward(2,150);
  566.      myservo.writeMicroseconds(1700);//        右转 400ms
  567.     }
  568.     }
  569.    }  
  570. }
  571. void R_count()
  572. {
  573.     if(Out1==1 && Out2==1 && Out3==1 && Out4==0 && Out5==1)    //4右转计数
  574.         {
  575.             n4=n4+0.1;
  576.             if(n4>=50)
  577.            {    n4=50;            }
  578.           //Serial.print("num4:");      //输出num3
  579.           //Serial.println(num4);       //显示次数
  580.          }
  581.          else  { n4=1;  }
  582.      if(Out1==1 && Out2==1 && Out3==1 && Out4==1 && Out5==0)    //4右转计数
  583.      {
  584.             n5=n5+0.1;
  585.             if(n5>=50)
  586.            {    n5=50;            }
  587.           //Serial.print("num4:");      //输出num3
  588.           //Serial.println(num4);       //显示次数
  589.          }
  590.          else  { n5=1;  }
  591.   }
  592. void L_count()
  593. {
  594.     if(Out1==1 && Out2==0 && Out3==1 && Out4==1 && Out5==1)    //4右转计数
  595.         {
  596.             n2=n2+0.1;
  597.             if(n2>=30)
  598.            {    n2=30;            }
  599.           //Serial.print("num4:");      //输出num3
  600.           //Serial.println(num4);       //显示次数
  601.          }
  602.          else  { n2=1;  }
  603.      if(Out1==0 && Out2==1 && Out3==1 && Out4==1 && Out5==1)    //4右转计数
  604.      {
  605.             n1=n1+0.1;
  606.             if(n1>=30)
  607.            {    n1=30;            }
  608.           //Serial.print("num4:");      //输出num3
  609.           //Serial.println(num4);       //显示次数
  610.          }
  611.          else  { n1=1;  }
  612.   }
  613. void Distance_test()   // 量出前方距离
  614. {
  615.   digitalWrite(Trig, LOW);   // 给触发脚低电平2μs
  616.   delayMicroseconds(2);
  617.   digitalWrite(Trig, HIGH);  // 给触发脚高电平10μs，这里至少是10μs
  618.   delayMicroseconds(10);
  619.   digitalWrite(Trig, LOW);    // 持续给触发脚低电
  620.   float Fdistance = pulseIn(Echo, HIGH);  // 读取高电平时间(单位：微秒)
  621.   Fdistance= Fdistance/58;       //为什么除以58等于厘米，  Y米=（X秒*344）/2
  622.   Distance = Fdistance;
  623.   if(Distance>=60)
  624.   {
  625.     Distance=60;
  626.   }
  627.   else if(Distance<=8)
  628.   {
  629.     Distance=60;
  630.   }
  631. // Serial.println(Distance);         //显示距离
  632. }
  633. void handle_nled()
  634. {
  635.     static unsigned long systick_ms_bak = 0;
  636.     if(!handle_ms_between(&systick_ms_bak, NLED_MS_BETWEEN))return;  
  637.     digitalWrite(NLED_PIN, val);
  638.     val = ~val;
  639. }

  641. void dida(u8 times, u8 frequency)
  642. {
  643.     for(byte i = 0; i < times; i++ )
  644.     {
  645.         digitalWrite(BEEP_PIN, LOW);
  646.         delay(frequency);
  647.         delay(frequency);
  648.         digitalWrite(BEEP_PIN, HIGH );
  649.         delay(frequency);
  650.         delay(frequency);  
  651.     }
  652. }
  653. unsigned char handle_ms_between( unsigned long *time_ms, unsigned int ms_between)
  654. {
  655.     if(millis() - *time_ms < ms_between)
  656.     {
  657.         return 0;  
  658.     }
  659.     else
  660.     {
  661.          *time_ms = millis();
  662.          return 1;
  663.     }
  664. }
  665. void detect()
  666. {
  667.    Out1 = digitalRead(IR1);
  668.    Out2 = digitalRead(IR2);
  669.    Out3 = digitalRead(IR3);
  670.    Out4 = digitalRead(IR4);
  671.    Out5 = digitalRead(IR5);   
  672.    Out2H = digitalRead(IH2);
  673.    Out3H = digitalRead(IH3);
  674.    Out4H = digitalRead(IH4);   
  675. }
  676. void Motor_Forward(char motor,char pwm)    //电机正转
  677. {
  678.   if(motor==1)
  679.   {
  680.      analogWrite(PWMA_IN1,pwm);
  681.      analogWrite(PWMA_IN2,255);
  682.   }
  683.   else if(motor==2)
  684.   {
  685.      analogWrite(PWMB_IN1,pwm);
  686.      analogWrite(PWMB_IN2,255);
  687.   }
  688. }

  690. void Motor_Back(char motor,char pwm)      //电机反转
  691. {
  692.   if(motor==1)
  693.   {
  694.      analogWrite(PWMA_IN1,255);
  695.      analogWrite(PWMA_IN2,pwm);
  696.   }
  697.   else if(motor==2)
  698.   {
  699.      analogWrite(PWMB_IN1,255);
  700.      analogWrite(PWMB_IN2,pwm);
  701.   }
  702. }
  703. void count()
  704. {
  705.    if(Out1==1 && Out2==1 && Out3==0 && Out4==1 && Out5==1)    //3直行计数
  706.         {
  707.             v=v+0.5;
  708.             if(v>=sv-10)
  709.            {    v=sv-10;            }
  710.            num3=v;
  711.         }
  712.         else {;}
  713.         
  714.          if(Out1==1 && Out2==1 && Out3==1 && Out4==0 && Out5==1)    //4右转计数
  715.         {
  716.             num4=num4+0.2;
  717.             if(num4>=20)
  718.            {    num4=20;            }
  719.          }
  720.          else  { num4=1;  }
  721.          
  722.          if(Out1==1 && Out2==0 && Out3==1 && Out4==1 && Out5==1)    //2左转计数
  723.         {
  724.             num2=num2+0.2;
  725.             if(num2>=20)
  726.            {     num2=20;            }
  727.          }
  728.          else  { num2=1;  }

  730.           if(Out1==0 && Out2==0 && Out3==0 && Out4==0 && Out5==0)    //stop计数
  731.         {
  732.             st=st+0.2;
  733.          }
  734.          else  { st=1;  }
  735.   }
复制代码


CX1.docx

21.78 KB, 下载次数: 6

寻迹程序
分享到:  QQ好友和群QQ好友和群 QQ空间QQ空间 腾讯微博腾讯微博 腾讯朋友腾讯朋友
收藏收藏 分享淘帖 顶顶 踩踩
回复

使用道具 举报

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

本版积分规则

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

Powered by 单片机教程网

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