第一次做程序，，，两三个月之前做的摇头和扫描子程序，求指正

cool2005tzh

本来是做小车的 好久没动了。。。要知道我们学校十月份开始考试考到现在了 怨念啊！！！！

这个程序用来扫描最近距离的，检测到最近距离了之后迅速摇头转向最近的地方。。。。但是有时候会卡。。。不知道为什么。

其实我知道这样做不怎么好，等暑假了我把它修改一下，应该先把扫描到的东西存数组的 但那时候就没想到。。。前段时间想到了懒得加。。。求大神帮优化~~~~

cool2005tzh

、、、、、、、、、、、、、、、、、、、、、、、、、、、、、、、

cool2005tzh

1. #include <arduino.h>
   
2. 
   
3. int trig = 2;
   
4. int echo = 3;
   
5. int servo=4;
   
6. Servo frontservo;
   
7. float nearest=99999;
   
8. int angle=90;
   
9. float lonth=9999;
   
10. 
    
11. void scan(int pos);
    
12. 
    
13. //--------------------------------------------------------------
    
14. //---------------protect servo--------------------------------
    
15. //----------------------------------------------------------------  
    
16. void ahead()
    
17. {
    
18.   int npos=frontservo.read();
    
19.   if (npos>90)
    
20.   {
    
21.     for(int pos=npos;pos>90;pos-=1)
    
22.     {
    
23.       frontservo.write(pos);
    
24.       delay(5);
    
25.     }
    
26.   }
    
27.   
    
28.   if (npos<90)
    
29.   {
    
30.     for(int pos=npos;pos<90;pos+=1)
    
31.     {
    
32.       frontservo.write(pos);
    
33.       delay(5);
    
34.     }
    
35.   }
    
36.   
    
37. }
    
38. void research()
    
39. {
    
40. //--------------------------------------------------------------
    
41. //---------------protect tuoji--------------------------------
    
42. //----------------------------------------------------------------  
    
43.   int npos=frontservo.read();
    
44.   for(int pos=npos;pos>0;pos-=1)
    
45.   {
    
46.     frontservo.write(pos);
    
47.     delay(5);
    
48.   }
    
49. //--------------------------------------------------------------
    
50. //-------------------research 0--180--------------------------------
    
51. //----------------------------------------------------------------
    
52.   for(int pos=0;pos<180;pos+=5)
    
53.   {
    
54.     frontservo.write(pos);
    
55.     scan(pos);
    
56.   }
    
57. //--------------------------------------------------------------
    
58. //---------------research 180--0--------------------------------
    
59. //----------------------------------------------------------------
    
60.   for(int pos=180;pos>0;pos-=2)
    
61.   {
    
62.     frontservo.write(pos);
    
63.     scan(pos);
    
64.   }
    
65. //--------------------------------------------------------------
    
66. //---------------pot to the nearest eagle--------------------------------
    
67. //----------------------------------------------------------------  
    
68. for(int pos=0;pos<angle;pos+=2)
    
69. {
    
70.    frontservo.write(pos);
    
71.    delay(5);
    
72. }
    
73. //Serial.print("it is the nearest");
    
74. //delay(1000);
    
75. }
    
76. 
    
77. void scan(int pos)
    
78. {
    
79.     digitalWrite(trig,1);//sent signal
    
80.     delay(10);
    
81.     digitalWrite(trig,0);
    
82.     float time=pulseIn(echo,HIGH);//resive signal
    
83.     float lonth=((time)*340)/20000;
    
84.     nearest=min(lonth,nearest);//nearest or not
    
85.     if(nearest==lonth & lonth<15)
    
86.     angle=pos;
    
87.    
    
88.     /*Serial.print("time:");
    
89.     Serial.println(time);
    
90.     Serial.print("lonth:");
    
91.     Serial.print(lonth);
    
92.     Serial.print("       neareat:");
    
93.     Serial.println(nearest);
    
94.     Serial.print("angle:");
    
95.     Serial.print(pos);
    
96.     Serial.print("       nearest angle:");
    
97.     Serial.println(angle);*/
    
98. }
    

复制代码

cool2005tzh

这个是一点不起作用的主程序和小车运动的程序。。。。纠结 就是只会摇头

1. #include <Servo.h>
   
2. #include "action.h"
   
3. #include "research.h"
   
4. 
   
5. void setup()
   
6. {
   
7.   pinMode(trig,OUTPUT);
   
8.   pinMode(echo,INPUT);
   
9.   //Serial.begin(9600);
   
10.   frontservo.attach(servo);
    
11. }
    
12. 
    
13. void loop()
    
14. {
    
15.   ahead();
    
16.   research();
    
17.   while(1)
    
18.   {
    
19.     direction();
    
20.   }  
    
21. }
    
22. 
    
23. void direction()
    
24. {
    
25.   scan(angle);
    
26.   if(lonth>15)
    
27.   {   
    
28.     goahead();
    
29.   }
    
30.   else
    
31.   {
    
32.     research();
    
33.     if (angle>100)
    
34.     turnr();
    
35.     if (angle<80)
    
36.     turnl();
    
37.   }
    
38. 
    
39. }  
    

复制代码

前进后退什么的

1. #include <arduino.h>
   
2. int lmotor1=5;
   
3. int lmotor2=6;
   
4. int rmotor1=7;
   
5. int rmotor2=8;
   
6. 
   
7. void goahead()
   
8. {
   
9.   digitalWrite(lmotor1,HIGH);
   
10.   digitalWrite(lmotor2,LOW);
    
11.   digitalWrite(rmotor1,HIGH);
    
12.   digitalWrite(rmotor2,LOW);
    
13. }
    
14. 
    
15. void back()
    
16. {
    
17.   digitalWrite(lmotor1,LOW);
    
18.   digitalWrite(lmotor2,HIGH);
    
19.   digitalWrite(rmotor1,LOW);
    
20.   digitalWrite(rmotor2,HIGH);
    
21. }
    
22. 
    
23. void turnl()
    
24. {
    
25.   digitalWrite(lmotor1,LOW);
    
26.   digitalWrite(lmotor2,HIGH);
    
27.   digitalWrite(rmotor1,HIGH);
    
28.   digitalWrite(rmotor2,LOW);
    
29. }
    
30. 
    
31. void turnr()
    
32. {
    
33.   digitalWrite(lmotor1,HIGH);
    
34.   digitalWrite(lmotor2,LOW);
    
35.   digitalWrite(rmotor1,LOW);
    
36.   digitalWrite(rmotor2,HIGH);
    
37. }
    

复制代码

cool2005tzh

我现在又买了6dof还有蓝牙模块。。。到时候做出来玩玩~~~~