PID 循跡自走車範例

1. #include <Arduino.h>
   
2. #include <Wire.h>
   5 M" p  F( o: ]: @
3. #include <Servo.h>
   . h1 R- m" X% V- T6 @# v- U5 O
4. 
   
5. #include "MePort.h"
   
6. #include "MeUltrasonic.h"
   
7. #include "MeDCMotor.h"
   2 t4 J& U9 [/ L
8. 
   0 b! F1 y1 g4 z+ t0 o
9. //double Input, Output ;
   
10. float MaxSpeed = 255;
    4 H. f! q1 M7 _& x& p
11. float MaxPower = 180;
    6 O4 f1 X6 h+ m$ L6 I8 l6 f
12. float MinPower = 120;
    & }' i& h2 S4 R* M% t0 z
13. float Error,ErrorAcc,ErrorDec;
    * ^  \) |) d$ @* n8 M/ K
14. 
    - J5 k. F: k6 K
15. float Kp=0.14;
    
16. float Kd=0.00020;//23;
    
17. float Ki=0.000201;
    
18. 
    
19. float nPower;
    6 R/ N# P+ S# {
20. MePort lightsensor_6(6);
    
21. MePort lightsensor_8(8);
    ' J; h3 w0 {% ^6 g
22. MeDCMotor motor_9(9);
    
23. MeDCMotor motor_10(10);
      |- m/ X# p; Q' r7 F
24. unsigned long previousMillis = 0;
    
25. const long interval = 1;
    - ~( s, |; |$ X3 J6 }6 @& e; \
26. 
    
27. void setup(){
    1 u0 ^6 A7 a* F5 u
28.     lightsensor_6.dWrite1(1);
    
29.     nPower = 160;
    5 r5 {: F/ }" i
30.     Error=0;
    8 v, Y. T, J8 \) d" W. ]3 B
31.     ErrorAcc=0;
    1 o" l2 e; m. X6 J6 ^! M# D2 D6 g
32. }
    
33. 
    
34. void loop(){
    + D, S4 P8 C7 E% R3 q, D: w
35.   unsigned long StartTime = millis();
    
36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    ) {* g* U5 r  M- ^$ d' ^) {
37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    : m; r5 A# \3 B. Y% v
38.     ErrorAcc +=  nError*Kd ;
    
39.     ErrorDec -=  nError*Ki ;
    
40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
      e  v- w& ~7 B4 g  S
41.     if(nError < 80 && nError > -80){
    
42.       if(nPower < MaxPower){
    
43.         nPower += 3;
    / ]' D3 f* i$ Z* R5 T! j" a
44.       }
    
45.     } else{
    5 U9 Y" ]' r& ]) {6 k
46.       if(nPower > MinPower){
    $ d( L8 R1 p4 F* Q
47.         nPower -= 2;
    
48.       }
    
49.     }
    + q2 Q/ K+ p( t. s
50.     MotoL(nPower-Error);
    7 \  b. d; S  \3 k2 u4 v9 b( M/ ~
51.     MotoR(nPower+Error);   
    
52.   }else{
    
53.     motor_9.run(0);
    # d- v7 ?* g: u7 ]2 y
54.     motor_10.run(0);
    
55.   }
    % Z% a1 _1 ~$ Y
56.   do{}while(millis() - StartTime < interval);
    ; N7 }! h7 C  E) d+ w7 }
57. }
    7 o5 e/ L2 }- [+ i+ x- S, \
58. 
    3 x- A0 v6 N" M) G3 Q2 ]
59. void MotoL(int Power){
    
60.   if (Power > MaxSpeed){
    
61.      Power = MaxSpeed;
    7 I: V/ D7 Z7 O7 j1 ?4 B/ C
62.   }
    + _  o0 N! B; [
63.   if (Power < -MaxSpeed){
    
64.      Power = -MaxSpeed;
    
65.   }
    
66.   motor_9.run(Power);
    & k/ G) t; o$ R6 v/ D8 V, f" x
67. }  
    . ~" L4 ~& V9 _/ a( x% t
68. 
    
69. void MotoR(int Power){
    
70.   if (Power > MaxSpeed){
    
71.      Power = MaxSpeed;
    $ f3 G2 |; \% U+ r! }
72.   }
    9 d) }6 G$ |" `% \) }
73.   if (Power < -MaxSpeed){
    . s- N; ^6 G; Z# o
74.      Power = -MaxSpeed;
    + ^/ f9 A% u9 e) f/ {; b
75.   }
    
76.   motor_10.run(Power);
    
77. }  

複製代碼

5 o2 A0 s$ u& i6 `+ V7 ]

) q6 L$ o1 }0 Z$ n! l0 d
您好，不知是否能向您請教。
$ _2 N% [% m" N目前和宋修賢老師在處理Ardui Car
' s: O3 [3 q& ^2 l( A雖然已使用較繁雜的方式處理了跑出黑線外的狀況
' H+ b% Q6 A+ v4 a7 @
但基於想追求更精簡的程式所以還是想請問一下
# w- j2 w) [; W& `! T就是如果我只以單純寫PID控制時，常遇到CNY70跑出黑線外後便往前衝
不知道您是否願意教我可以如何處理
8 S: h! p. d  h

以下是我挑出我一般寫純PID控制的Code：

1. int MotoSpeed = 250;
   
2. double CNY70Val = 1000;
   : j5 c9 y3 f0 y
3. int interror = 0;
   
4. int olderror = 0;
   
5. double values;
   + W" L# {  D( z
6. 
   
7. 
   
8. void CNY70()
   
9. {
   
10.   valuesRR = analogRead(RR)
    
11.   valuesMR = analogRead(MR);
    
12.   valuesMM = analogRead(MM);
    * t* `: V; n' ~0 k$ C
13.   valuesML = analogRead(ML);
    5 N% n- T2 Y6 d2 s
14.   valuesLL = analogRead(LL);
    " S2 B% S, _9 Y. R2 Q" B- b# J
15. 
    
16.   if (valuesRR > CNY70Val)
    ; e  e  h* }7 {( m$ `
17.     valuesRR = CNY70Val;
    . w8 [* C4 p6 _0 \. Y* Q; `
18.   if (valuesMR > CNY70Val)
    & _% N' ^, F" w3 v6 }5 t4 I" R* \6 r+ p
19.     valuesMR = CNY70Val;
    
20.   if (valuesMM > CNY70Val)
    8 o9 n8 R0 @2 E) J8 B
21.     valuesMM = CNY70Val;
    
22.   if (valuesML > CNY70Val)
    
23.     valuesML = CNY70Val;
    - ]8 |  {& h' `4 P" W1 Q5 p
24.   if (valuesLL > CNY70Val)
    
25.     valuesLL = CNY70Val;
    # X: D& l, I/ `! B
26. 
    
27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    ! r0 u+ T6 n; Y* r
28. }
    
29. 
    
30. void Car()
    
31. {
    " S/ d2 W% E5 I: |- A; I2 o2 x
32.   while (1) {
    
33.     CNY70();
    / X+ `; T- T/ e1 k# {; ?+ o0 _& D7 D* ?
34. 
    3 V% c$ F7 A2 T: Q7 f: a( a2 M
35.     int error = ((int)values);
    : \0 h9 J3 ?8 Z
36.     interror += error;
    
37.     int lasterror = error - olderror;
    ) ?9 ?, a) l: |- f8 z. Q
38.     olderror = error;
    
39.     int power = error / 5 + interror / 10000 + lasterror;
    ! D3 u) U  `0 }- |' |" A; Q
40. 
    
41.     if (power > MotoSpeed)
    
42.       power = MotoSpeed;
    0 B- h7 h) E/ v3 f: o
43.     if (power < -MotoSpeed)
    3 n/ r) @3 }. M- t
44.       power = -MotoSpeed;
    
45. 
    
46.     if (power > 0)
    
47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作，正數正轉 負數反轉。
    
48.     else
    
49.       Speed(MotoSpeed + power, MotoSpeed);
    . P7 X8 h2 l: B8 ~: h+ N
50.   }
    
51. }

複製代碼

& i! M% W4 s( [% z# C# P3 F) h* S" a
+ @+ v4 |2 u/ b' i$ c

感謝分享

感謝分享