PID 循跡自走車範例

1. #include <Arduino.h>
   
2. #include <Wire.h>
     S- ?' t7 v% r5 V
3. #include <Servo.h>
   & J* o0 c' n9 o0 D* `
4. 
   ; X9 l; {' t/ i* _. E/ t' {+ v/ J
5. #include "MePort.h"
   2 L- {/ r2 m- Y; U" e6 X: M5 v
6. #include "MeUltrasonic.h"
   
7. #include "MeDCMotor.h"
   
8. 
   
9. //double Input, Output ;
   0 X+ F6 M  @/ v; a
10. float MaxSpeed = 255;
    
11. float MaxPower = 180;
    ( p$ c2 [, H3 D- t# r- D
12. float MinPower = 120;
    
13. float Error,ErrorAcc,ErrorDec;
    & m; M" T* }6 K# A
14. 
    7 P% N0 n+ g' G
15. float Kp=0.14;
    
16. float Kd=0.00020;//23;
    
17. float Ki=0.000201;
    
18. 
    3 `' o  k/ m  R& Q7 a! t1 H
19. float nPower;
    
20. MePort lightsensor_6(6);
    
21. MePort lightsensor_8(8);
    ; b9 u& h: x* [! \
22. MeDCMotor motor_9(9);
    
23. MeDCMotor motor_10(10);
    
24. unsigned long previousMillis = 0;
    6 |, n0 ^1 G- u8 g8 z+ x, X
25. const long interval = 1;
    6 ~, j! o; |+ V0 p+ j
26. 
    2 ~5 @: q) ~% i7 L$ H) \1 f
27. void setup(){
    1 U* _( P0 ?+ W, K3 G
28.     lightsensor_6.dWrite1(1);
    6 h& N% T% F% i; }
29.     nPower = 160;
    
30.     Error=0;
    # g! h+ x# F" \8 V! z0 y& \
31.     ErrorAcc=0;
    4 n' R3 ]8 I6 R3 ]6 {0 U
32. }
    
33. 
    
34. void loop(){
    $ T% x6 g' G, F
35.   unsigned long StartTime = millis();
    4 [5 [" u( I. A9 a- R4 V
36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    ! ~, k9 i- }& j3 P
37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    ) I. {5 n$ F. h5 w: x0 {7 A
38.     ErrorAcc +=  nError*Kd ;
    ; M' C! s" _  T: J) ~$ |" A
39.     ErrorDec -=  nError*Ki ;
    6 ]7 A( s$ _, p8 J% y0 b; C
40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    & a. M" b: L9 }( ]9 o0 A! M
41.     if(nError < 80 && nError > -80){
    
42.       if(nPower < MaxPower){
    
43.         nPower += 3;
    
44.       }
    
45.     } else{
    
46.       if(nPower > MinPower){
    " y5 ?7 R8 ]- @1 ^( R. T/ z& j, w
47.         nPower -= 2;
    1 L1 a. D/ @6 E* p* j& H: P
48.       }
    
49.     }
    
50.     MotoL(nPower-Error);
    / @# l1 u9 F7 a( T, q  A
51.     MotoR(nPower+Error);   
    
52.   }else{
    9 _9 [3 P6 d4 K; s
53.     motor_9.run(0);
    
54.     motor_10.run(0);
    ; `6 o# j2 t: P2 M7 X
55.   }
    
56.   do{}while(millis() - StartTime < interval);
    : I7 E# w' C6 Y; [5 a& x; ?" o
57. }
    1 s  @3 o) ?) U6 A; M
58. 
    
59. void MotoL(int Power){
    
60.   if (Power > MaxSpeed){
    + t: p' `/ J: {. k
61.      Power = MaxSpeed;
    
62.   }
    
63.   if (Power < -MaxSpeed){
    
64.      Power = -MaxSpeed;
    
65.   }
    
66.   motor_9.run(Power);
    & Y5 Z6 \$ D; W' O8 t( ?
67. }  
    
68. 
    4 c$ d, j- U& h2 l0 P0 ?5 z
69. void MotoR(int Power){
    
70.   if (Power > MaxSpeed){
    ' }/ e* j& p1 W7 X# d" F. v
71.      Power = MaxSpeed;
    
72.   }
    , M& p  [: l+ \3 M/ }
73.   if (Power < -MaxSpeed){
    ! z; R7 {7 a8 E7 L% r- w( `. m
74.      Power = -MaxSpeed;
    . G+ d, `. `# A" p0 F6 }. _, Z
75.   }
    
76.   motor_10.run(Power);
    , ^/ P, ^& d9 x- Z: u+ Y
77. }  

複製代碼

0 F- ]9 _1 ]# U1 c' U, a8 S
0 v+ F! F: h0 ]9 z$ |/ G您好，不知是否能向您請教。
% J2 \+ ^5 R; Q$ ]目前和宋修賢老師在處理Ardui Car
雖然已使用較繁雜的方式處理了跑出黑線外的狀況

3 h8 j5 d! v% c" W! i但基於想追求更精簡的程式所以還是想請問一下
, c! l, \4 _- `5 r就是如果我只以單純寫PID控制時，常遇到CNY70跑出黑線外後便往前衝
9 k# \: p% O- [. x% z1 f0 v3 L( p不知道您是否願意教我可以如何處理
9 O: X7 C; u9 a" e! {

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

1. int MotoSpeed = 250;
   + S: b. n& w# T2 H
2. double CNY70Val = 1000;
   
3. int interror = 0;
   ; E! D+ i9 i& z! D) ?
4. int olderror = 0;
   
5. double values;
   8 B+ j7 u9 p9 ]$ o2 A; s% H
6. 
   
7. 
   
8. void CNY70()
   
9. {
   
10.   valuesRR = analogRead(RR)
    ) w0 S4 U4 f- @3 g2 @
11.   valuesMR = analogRead(MR);
    
12.   valuesMM = analogRead(MM);
    
13.   valuesML = analogRead(ML);
    ) V8 A; o$ H9 s" c" L
14.   valuesLL = analogRead(LL);
    ; D4 E" t/ m& {7 e; P4 Z4 Y
15. 
    
16.   if (valuesRR > CNY70Val)
    
17.     valuesRR = CNY70Val;
    3 ?" n7 P) ^- x# Z, O! ~% h9 [8 T: Y
18.   if (valuesMR > CNY70Val)
    
19.     valuesMR = CNY70Val;
    
20.   if (valuesMM > CNY70Val)
    9 f9 e; [( C2 q
21.     valuesMM = CNY70Val;
    
22.   if (valuesML > CNY70Val)
    
23.     valuesML = CNY70Val;
    
24.   if (valuesLL > CNY70Val)
    * l6 ~3 S  V) E& V1 K* e
25.     valuesLL = CNY70Val;
    
26. 
    $ j- Q' L) d, r/ i8 k, u: N
27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
      c. l' m1 \2 h% e! X
28. }
    
29. 
    7 z. |2 w* F7 y1 M1 A' ~* J( d1 V  P
30. void Car()
    
31. {
    
32.   while (1) {
    
33.     CNY70();
    
34. 
    
35.     int error = ((int)values);
    2 c/ S3 R! h# ]- P0 T- Q
36.     interror += error;
    
37.     int lasterror = error - olderror;
    
38.     olderror = error;
    
39.     int power = error / 5 + interror / 10000 + lasterror;
    
40. 
    $ w9 E, K+ ^& n8 n8 ~$ \9 @
41.     if (power > MotoSpeed)
    
42.       power = MotoSpeed;
    $ B% U! x) W& _+ N' G2 {! B+ |
43.     if (power < -MotoSpeed)
    
44.       power = -MotoSpeed;
    
45. 
    : O4 Z" u: P( f! h1 n: D
46.     if (power > 0)
    
47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作，正數正轉 負數反轉。
    9 Q4 S  x- c2 B# y
48.     else
    ' ^  r' x6 C: h  [0 ~( k3 d- K& X
49.       Speed(MotoSpeed + power, MotoSpeed);
    
50.   }
    % N3 S4 h' b5 w3 j* s) H9 ]
51. }

複製代碼

1 p& m% j' O* L3 \) E, X

感謝分享