超声波避障小车问题求教

xiaocai10000

我的小车主要问题就是停不下来，会直接撞上前面的障碍物，感觉超声波没在判断，程序如下，有高手帮我分析分析吗？

1. 
   
2. #include <Servo.h>
   
3. #define L1 7
   
4. #define L2 8
   
5. #define R1 12
   
6. #define R2 13
   
7. #define C0 5
   
8. #define C1 6
   
9. Servo myservo;
   
10. int rightDistance;
    
11. int leftDistance;
    
12. int middleDistance;
    
13. const int Trig=2;
    
14. const int Echo=3;
    
15. 
    
16. void forward()
    
17. {
    
18.       digitalWrite(L1,HIGH);
    
19.       digitalWrite(L2,LOW);
    
20.       analogWrite(C0,150);
    
21.       digitalWrite(R1,HIGH);
    
22.       digitalWrite(R2,LOW);
    
23.       analogWrite(C1,150);
    
24. }
    
25. 
    
26. void back()
    
27. {
    
28.       digitalWrite(L1,LOW);
    
29.       digitalWrite(L2,HIGH);
    
30.       analogWrite(C0,150);
    
31.       digitalWrite(R1,LOW);
    
32.       digitalWrite(R2,HIGH);
    
33.       analogWrite(C1,150);
    
34. }
    
35. 
    
36. void turnLeft()
    
37. {
    
38.       digitalWrite(L1,LOW);
    
39.       digitalWrite(L2,HIGH);
    
40.       analogWrite(C0,150);
    
41.       digitalWrite(R1,HIGH);
    
42.       digitalWrite(R2,LOW);
    
43.       analogWrite(C1,150);
    
44. }
    
45. 
    
46. void turnRight()
    
47. {
    
48.       digitalWrite(L1,HIGH);
    
49.       digitalWrite(L2,LOW);
    
50.       analogWrite(C0,150);
    
51.       digitalWrite(R1,LOW);
    
52.       digitalWrite(R2,HIGH);
    
53.       analogWrite(C1,150);
    
54. }
    
55. 
    
56. void gostop()
    
57. {
    
58.       digitalWrite(L1,LOW);
    
59.       digitalWrite(L2,LOW);
    
60.       analogWrite(C0,0);
    
61.       digitalWrite(R1,LOW);
    
62.       digitalWrite(R2,LOW);
    
63.       analogWrite(C1,0);
    
64. }
    
65. 
    
66. int Distance_test()
    
67. {
    
68.   digitalWrite(Trig,LOW);
    
69.   delayMicroseconds(2);
    
70.   digitalWrite(Trig, HIGH);
    
71.   delayMicroseconds(10);
    
72.   digitalWrite(Trig, LOW);
    
73.   float cm=pulseIn(Echo, HIGH);
    
74.   cm= cm/58.0;
    
75.   cm = (int(cm * 100.0)) / 100.0;
    
76.   return (int)cm;
    
77. }
    
78. 
    
79. void setup()
    
80. {
    
81.   myservo.attach(9);
    
82.   Serial.begin(9600);     // 初始化串口
    
83.   pinMode(Echo,INPUT);
    
84.   pinMode(Trig,OUTPUT);
    
85.   pinMode(L1,OUTPUT);
    
86.   pinMode(L2,OUTPUT);
    
87.   pinMode(R1,OUTPUT);
    
88.   pinMode(R2,OUTPUT);
    
89.   pinMode(C0,OUTPUT);
    
90.   pinMode(C1,OUTPUT);
    
91.   gostop();
    
92. }
    
93. void loop()
    
94. {
    
95. myservo.write(90);
    
96. delay(1000);
    
97. middleDistance=Distance_test();
    
98. Serial.print(middleDistance);
    
99. Serial.println("cm");
    
100. delay(1000);
     
101. if(middleDistance<=20)
     
102. {
     
103.   gostop();
     
104.   delay(500);
     
105.   myservo.write(30);
     
106.   delay(1000);
     
107.   rightDistance=Distance_test();
     
108.   delay(1000);
     
109.   myservo.write(90);
     
110.   delay(1000);
     
111.   myservo.write(150);
     
112.   delay(1000);
     
113.   leftDistance=Distance_test();
     
114.   delay(1000);
     
115.   myservo.write(90);
     
116.   delay(1000);
     
117.   middleDistance=Distance_test();
     
118.   delay(1000);
     
119.   if(rightDistance<=10&&leftDistance<=10)
     
120.   {
     
121.     back();
     
122.     delay(50);
     
123.   }
     
124.   else if(rightDistance>leftDistance)
     
125.   {
     
126.     turnRight();
     
127.     delay(50);
     
128.   }
     
129.   else if(rightDistance<leftDistance)
     
130.   {
     
131.     turnLeft();
     
132.     delay(50);
     
133.   }
     
134.   else
     
135.   {
     
136.     forward();
     
137.   }
     
138. }
     
139. else
     
140. {
     
141.   forward();
     
142. }
     
143. }
     
144. 
     

复制代码

1415926

myservo.write(90);
delay(1000);
middleDistance=Distance_test();
Serial.print(middleDistance);
Serial.println("cm");
delay(1000);
if(middleDistance<=20)


测距前后，你的程序各停了1秒钟，总计超过2秒，距离<20cm才转，是不是2秒已经跑的超过20cm了，
来不及停了？

564764406

要停下来啊，不要一直走啊，我做的是三个超声波的避障，就完全没问题啊，你的时间太长，没有必要，而且，最好看着串口显示的数据来调小车，不要一味的找程序。
串口的数据，对比才能发现到底是哪里错了。再去解决问题。
加油吧 。