程式問題請教

ypyp3399

您好
我使用Arduino DUE來驅動馬達，我採用5個感測器，有32種狀態，可是馬達運動卻不是我程式所設定的狀態，舉例case 0(感測器狀態皆為0)，馬達程式預設皆停止，實際運行時馬達卻全速運行。
想請問我的程式是否有問題。

1. const int IRPeople =10;   //紅外線感測器前方避障
   
2. const int SLeft_1 = 5;   //左邊方向紅外線二訊號輸入
   
3. const int SLeft = 6;      //左邊方向紅外線一訊號輸入
   
4. const int SMiddle = 7;    //中間方向紅外線訊號輸入
   
5. const int SRight = 11;     //右邊方向紅外線一訊號輸入
   
6. const int SRight_1 = 12;  //右邊方向紅外線二訊號輸入
   
7. 
   
8. const int PeopleLED = 13;  //紅外線感測前方避障狀態LED
   
9. const int Motor_Left = 9;
   
10. const int Motor_Right = 8;
    
11. const int Motor_M1 = DAC0;     //右邊
    
12. const int Motor_M2 = DAC1;     //左邊
    
13. 
    
14. byte byteSensorStatus = 0;
    
15. 
    
16. void setup(){
    
17.   Serial.begin(9600);
    
18. 
    
19.   pinMode(IRPeople,INPUT);  //紅外線感測器前方避障
    
20.   pinMode(SLeft_1,INPUT);
    
21.   pinMode(SLeft,INPUT);  
    
22.   pinMode(SMiddle,INPUT);
    
23.   pinMode(SRight,INPUT);
    
24.   pinMode(SRight_1,INPUT);
    
25.   
    
26.   pinMode(Motor_M1,OUTPUT);
    
27.   pinMode(Motor_M2,OUTPUT);
    
28.   pinMode(Motor_Left,OUTPUT);
    
29.   pinMode(Motor_Right,OUTPUT);
    
30.   pinMode(PeopleLED,OUTPUT);  //紅外線感測前方避障狀態LED
    
31.   
    
32.   delay(5000);
    
33. }
    
34. 
    
35. void loop(){
    
36.   int nIRStatus;
    
37.   
    
38.   byteSensorStatus = 0;
    
39.   nIRStatus = digitalRead(IRPeople);
    
40.   if(nIRStatus == 0){
    
41.   
    
42.     motorstop(0,0);
    
43.     delay(5000);
    
44.   }
    
45.   else{
    
46.   nIRStatus = digitalRead(SLeft_1);
    
47.   if(nIRStatus == 1)
    
48.   byteSensorStatus = (byteSensorStatus | (0x01 << 4));
    
49.   
    
50.   nIRStatus = digitalRead(SLeft);
    
51.   if(nIRStatus == 1)
    
52.   byteSensorStatus = (byteSensorStatus | (0x01 << 3));
    
53.   
    
54.   nIRStatus = digitalRead(SMiddle);
    
55.   if(nIRStatus == 1)
    
56.   byteSensorStatus = (byteSensorStatus | (0x01 << 2));
    
57.   
    
58.   nIRStatus = digitalRead(SRight);
    
59.   if(nIRStatus == 1)
    
60.   byteSensorStatus = (byteSensorStatus | (0x01 << 1));
    
61.   
    
62.   nIRStatus = digitalRead(SRight_1);
    
63.   if(nIRStatus == 1)
    
64.   byteSensorStatus = (byteSensorStatus | 0x01);
    
65.   
    
66.   drivemotor(byteSensorStatus);
    
67.   }
    
68. }
    
69. 
    
70. void drivemotor(byte nStatus)
    
71. {
    
72.   switch(nStatus)
    
73.   {
    
74.     case 31:// SL1:1 SL:1 SM:1 SR:1 SR1:1//　黑黑黑黑黑
    
75.     motorstop(0,0);
    
76.     break;
    
77.     case 30:// SL1:1 SL:1 SM:1 SR:1 SR1:0//　黑黑黑黑白
    
78.     motorstop(0,0);
    
79.     break;
    
80.     case 29:// SL1:1 SL:1 SM:1 SR:0 SR1:1//　黑黑黑白黑
    
81.     motorstop(0,0);
    
82.     break;
    
83.     case 28:// SL1:1 SL:1 SM:1 SR:0 SR1:0//　黑黑黑白白
    
84.     right(0,0);
    
85.     break;
    
86.     case 27:// SL1:1 SL:1 SM:0 SR:1 SR1:1//　黑黑白黑黑
    
87.     motorstop(0,0);
    
88.     break;
    
89.     case 26:// SL1:1 SL:1 SM:0 SR:1 SR1:1//　黑黑白黑白
    
90.     motorstop(0,0);
    
91.     break;
    
92.     case 25:// SL1:1 SL:1 SM:0 SR:0 SR1:1//　黑黑白白黑
    
93.     motorstop(0,0);
    
94.     break;
    
95.     case 24:// SL1:1 SL:1 SM:0 SR:0 SR1:0//　黑黑白白白
    
96.     slowright_1(0,0);
    
97.     break;
    
98.     case 23:// SL1:1 SL:0 SM:1 SR:1 SR1:1//　黑白黑黑黑
    
99.     motorstop(0,0);
    
100.     break;
     
101.     case 22:// SL1:1 SL:0 SM:1 SR:1 SR1:0//　黑白黑黑白
     
102.     motorstop(0,0);
     
103.     break;
     
104.     case 21:// SL1:1 SL:0 SM:1 SR:0 SR1:1//　黑白黑白黑
     
105.     motorstop(0,0);
     
106.     break;
     
107.     case 20:// SL1:1 SL:0 SM:1 SR:0 SR1:0//　黑白黑白白
     
108.     motorstop(0,0);
     
109.     break;
     
110.     case 19:// SL1:1 SL:0 SM:0 SR:1 SR1:1//　黑白白黑黑
     
111.     motorstop(0,0);
     
112.     break;
     
113.     case 18:// SL1:1 SL:0 SM:0 SR:1 SR1:0//　黑白白黑白
     
114.     motorstop(0,0);
     
115.     break;
     
116.     case 17:// SL1:1 SL:0 SM:0 SR:0 SR1:1//　黑白白白黑
     
117.     motorstop(0,0);
     
118.     break;
     
119.     case 16:// SL1:1 SL:0 SM:0 SR:0 SR1:0//　黑白白白白
     
120.     slowright_1(0,0);
     
121.     break;
     
122.     case 15:// SL1:0 SL:1 SM:1 SR:1 SR1:1//　白黑黑黑黑
     
123.     motorstop(0,0);
     
124.     break;
     
125.     case 14:// SL1:0 SL:1 SM:1 SR:1 SR1:0//　白黑黑黑白
     
126.     forward(0,130);
     
127.     break;
     
128.     case 13:// SL1:0 SL:1 SM:1 SR:0 SR1:1//　白黑黑白黑
     
129.     motorstop(0,0);
     
130.     break;
     
131.     case 12:// SL1:0 SL:1 SM:1 SR:0 SR1:0//　白黑黑白白
     
132.     slowright(0,0);
     
133.     break;
     
134.     case 11:// SL1:0 SL:1 SM:0 SR:1 SR1:1//　白黑白黑黑
     
135.     motorstop(0,0);
     
136.     break;
     
137.     case 10:// SL1:0 SL:1 SM:0 SR:1 SR1:0//　白黑白黑白
     
138.     motorstop(0,0);
     
139.     break;
     
140.     case 9:// SL1:0 SL:1 SM:0 SR:0 SR1:1//　白黑白白黑
     
141.     motorstop(0,0);
     
142.     break;
     
143.     case 8:// SL1:0 SL:1 SM:0 SR:0 SR1:0//　白黑白白白
     
144.     slowright(0,0);
     
145.     break;
     
146.     case 7:// SL1:0 SL:0 SM:1 SR:1 SR1:1//　白白黑黑黑
     
147.     left(0,0);
     
148.     break;
     
149.     case 6:// SL1:0 SL:0 SM:1 SR:1 SR1:0//　白白黑黑白
     
150.     slowleft(0,0);
     
151.     break;
     
152.     case 5:// SL1:0 SL:0 SM:1 SR:0 SR1:1//　白白黑白黑
     
153.     motorstop(0,0);
     
154.     break;
     
155.     case 4:// SL1:0 SL:0 SM:1 SR:0 SR1:0//　白白黑白白
     
156.     forward(0,130);
     
157.     break;
     
158.     case 3:// SL1:0 SL:0 SM:0 SR:1 SR1:1//　白白白黑黑
     
159.     slowleft_1(0,0);
     
160.     break;
     
161.     case 2:// SL1:0 SL:0 SM:0 SR:1 SR1:0//　白白白黑白
     
162.     slowleft(0,0);
     
163.     break;
     
164.     case 1:// SL1:0 SL:0 SM:0 SR:0 SR1:1//　白白白白黑
     
165.     slowleft_1(0,0);
     
166.     break;
     
167.     case 0:// SL1:0 SL:0 SM:0 SR:0 SR1:0//　白白白白白
     
168.     motorstop(0,0);
     
169.     break;
     
170.   }
     
171. }
     
172. 
     
173. void motorstop(byte flag, byte motorspeed)
     
174. {
     
175.   
     
176.   analogWrite(Motor_M1,motorspeed);
     
177.   analogWrite(Motor_M2,motorspeed);
     
178.   
     
179. }
     
180. 
     
181. void forward(byte flag, byte motorspeed)
     
182. {
     
183.   
     
184.   digitalWrite(Motor_Left,LOW);
     
185.   digitalWrite(Motor_Right,LOW);
     
186.   analogWrite(Motor_M1,motorspeed);
     
187.   analogWrite(Motor_M2,motorspeed);
     
188. }
     
189. 
     
190. void right(byte flag, byte motorspeed)
     
191. {
     
192.   digitalWrite(Motor_Left,LOW);
     
193.   digitalWrite(Motor_Right,LOW);
     
194.   analogWrite(Motor_M1,motorspeed);
     
195.   analogWrite(Motor_M2,70);
     
196. }
     
197. 
     
198. void slowright(byte flag, byte motorspeed)
     
199. {
     
200.   digitalWrite(Motor_Left,LOW);
     
201.   digitalWrite(Motor_Right,LOW);
     
202.   analogWrite(Motor_M1,motorspeed);
     
203.   analogWrite(Motor_M2,70);
     
204. }
     
205. 
     
206. void slowright_1(byte flag, byte motorspeed)
     
207. {
     
208.   digitalWrite(Motor_Left,LOW);
     
209.   digitalWrite(Motor_Right,LOW);
     
210.   analogWrite(Motor_M1,motorspeed);
     
211.   analogWrite(Motor_M2,80);
     
212. }
     
213. 
     
214. void left(byte flag, byte motorspeed)
     
215. {
     
216.   digitalWrite(Motor_Left,LOW);
     
217.   digitalWrite(Motor_Right,LOW);
     
218.   analogWrite(Motor_M1,70);
     
219.   analogWrite(Motor_M2,motorspeed);
     
220. }
     
221. 
     
222. void slowleft(byte flag, byte motorspeed)
     
223. {
     
224.   digitalWrite(Motor_Left,LOW);
     
225.   digitalWrite(Motor_Right,LOW);
     
226.   analogWrite(Motor_M1,70);
     
227.   analogWrite(Motor_M2,motorspeed);
     
228. }
     
229. 
     
230. void slowleft_1(byte flag, byte motorspeed)
     
231. {
     
232.   digitalWrite(Motor_Left,LOW);
     
233.   digitalWrite(Motor_Right,LOW);
     
234.   analogWrite(Motor_M1,80);
     
235.   analogWrite(Motor_M2,motorspeed);
     
236. }

複製代碼

vegewell

回復 1# ypyp3399

是的,你這個程式有些問題,
如,三方紅外線訊號輸入同時是一,
那你的byteSensorStatus,應該選擇哪個?得到最好的動作,
你應該先測試一下 motorstop(0,0);
另外寫個簡單的測試程式,沒接紅外線感測器的,
據我瞭解,有些馬達驅動晶片,即使enable設定為0,也一樣照轉不誤.