PID 循跡自走車範例

1. #include <Arduino.h>
   ( i/ M, F. ]; E4 Y3 R2 V8 ?
2. #include <Wire.h>
   1 D8 c( `4 a* n1 B6 n
3. #include <Servo.h>
   
4. 
   
5. #include "MePort.h"
   7 z1 Q! q, c+ M( h" v
6. #include "MeUltrasonic.h"
   
7. #include "MeDCMotor.h"
   0 S: ^6 I8 ]9 W1 i
8. 
   
9. //double Input, Output ;
   $ z0 ]/ j' \1 ]5 n/ k
10. float MaxSpeed = 255;
    + ]' f3 t- V( _/ r
11. float MaxPower = 180;
    - h" h6 T' Y. ^( T' H! t' D
12. float MinPower = 120;
    : X& a0 ?1 }0 e+ q; J
13. float Error,ErrorAcc,ErrorDec;
    ' Q1 y- V# ~- x' S6 M& D
14. 
    
15. float Kp=0.14;
    
16. float Kd=0.00020;//23;
    * T1 B3 X$ o! x; |4 Y4 L1 Q1 F
17. float Ki=0.000201;
    
18. 
    # P8 S* p: @/ a% W
19. float nPower;
    
20. MePort lightsensor_6(6);
    1 a- f+ R" j" @, V! J& g5 E/ o
21. MePort lightsensor_8(8);
    
22. MeDCMotor motor_9(9);
    ! y! X# W/ n% n  O5 T7 ~
23. MeDCMotor motor_10(10);
    - `' m5 P/ `2 c. l
24. unsigned long previousMillis = 0;
    
25. const long interval = 1;
    
26. 
    $ [9 r  ]# J  Z& o8 O
27. void setup(){
    
28.     lightsensor_6.dWrite1(1);
    
29.     nPower = 160;
    
30.     Error=0;
    & Q% h% @- u- }$ t9 i, M+ t1 X
31.     ErrorAcc=0;
    5 v3 p) `6 f3 k& ~; V' k1 Y; n
32. }
    
33. 
    
34. void loop(){
    7 S6 V- E3 P9 x$ c) |# T
35.   unsigned long StartTime = millis();
    % h' N: l7 h; Y* I8 T% T- F9 b
36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    
37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    % ?1 Q, R0 S: i/ L0 l
38.     ErrorAcc +=  nError*Kd ;
    
39.     ErrorDec -=  nError*Ki ;
    5 U5 R+ D' F0 m8 T! P, M
40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    
41.     if(nError < 80 && nError > -80){
    
42.       if(nPower < MaxPower){
    8 q" w+ L$ C8 P4 e6 d. t1 F* C! I
43.         nPower += 3;
    / |3 D$ t- o/ @  b* k) u7 ]" b
44.       }
      k- k! b1 h1 U( k
45.     } else{
    
46.       if(nPower > MinPower){
    
47.         nPower -= 2;
    
48.       }
    ; A8 Q3 d( r7 o. V
49.     }
    
50.     MotoL(nPower-Error);
    
51.     MotoR(nPower+Error);   
    5 ]. D& b8 |% n+ p% c+ Y% r
52.   }else{
    
53.     motor_9.run(0);
    8 u' E, w- x  ?+ n7 c0 H8 R* g
54.     motor_10.run(0);
    
55.   }
    
56.   do{}while(millis() - StartTime < interval);
    ) K! d6 Z1 E. T; p& O  f) W
57. }
    8 T5 L/ f+ u8 I; N" }. S9 c
58. 
    3 n  @% C9 m- W8 @' n% b9 s8 S
59. void MotoL(int Power){
    
60.   if (Power > MaxSpeed){
    ( y# b* M' L) l) N& R0 U
61.      Power = MaxSpeed;
    
62.   }
    9 w. `$ ~6 N$ F4 h; w3 ]
63.   if (Power < -MaxSpeed){
    
64.      Power = -MaxSpeed;
    9 B$ ]: e" H1 T7 k! K
65.   }
    6 A) U0 ~# I6 y: N
66.   motor_9.run(Power);
    9 ~3 k' O7 j6 l- X. H: U
67. }  
    
68. 
    
69. void MotoR(int Power){
    
70.   if (Power > MaxSpeed){
    
71.      Power = MaxSpeed;
    
72.   }
      f) D% U, R4 K  x/ Z9 f$ s- w
73.   if (Power < -MaxSpeed){
      F. t9 f$ c: F' i
74.      Power = -MaxSpeed;
    
75.   }
    + H: h  j, T  O7 d$ l
76.   motor_10.run(Power);
    : i2 A# d" B( R
77. }  

複製代碼

- g1 `2 N" T/ Z8 `+ y

) c; F0 g6 m. c8 W您好，不知是否能向您請教。
" a* ^$ N+ \( U, e3 {$ i目前和宋修賢老師在處理Ardui Car
雖然已使用較繁雜的方式處理了跑出黑線外的狀況

" h; G. ]+ \* D* e, q( y& Z但基於想追求更精簡的程式所以還是想請問一下
' ?% _+ K% a$ e就是如果我只以單純寫PID控制時，常遇到CNY70跑出黑線外後便往前衝
4 \% w& d8 N7 Y! x* a. z不知道您是否願意教我可以如何處理


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

1. int MotoSpeed = 250;
   * c/ D! ^. [$ Z7 r
2. double CNY70Val = 1000;
   
3. int interror = 0;
     P5 {" L4 N2 }0 b
4. int olderror = 0;
   # r5 v) o' z. m' i. r
5. double values;
   / ?+ p+ S  O) M3 E+ z6 [: E% N2 q
6. 
   
7. 
   7 |; _/ V0 _+ [0 a& G2 D0 l5 X
8. void CNY70()
   ) h0 ^! d( \6 t5 ~9 O# t# G4 d
9. {
   
10.   valuesRR = analogRead(RR)
    
11.   valuesMR = analogRead(MR);
    
12.   valuesMM = analogRead(MM);
    
13.   valuesML = analogRead(ML);
    
14.   valuesLL = analogRead(LL);
    & u. n* |. A8 s: P' ^0 u; W
15. 
    % p+ b* x0 _. F. c" X4 N% H- K
16.   if (valuesRR > CNY70Val)
    
17.     valuesRR = CNY70Val;
    - n- a5 S" C$ h& M" }0 a
18.   if (valuesMR > CNY70Val)
    8 [: N6 U" M# {% |5 v, d
19.     valuesMR = CNY70Val;
    
20.   if (valuesMM > CNY70Val)
    
21.     valuesMM = CNY70Val;
    
22.   if (valuesML > CNY70Val)
    
23.     valuesML = CNY70Val;
    
24.   if (valuesLL > CNY70Val)
    ! \9 |$ L; y6 i
25.     valuesLL = CNY70Val;
    & e4 Y1 N/ P4 S( f( Y3 l
26. 
    0 I' C5 p: i2 l/ l* B
27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    $ D; M8 G6 @- m$ v( x& p
28. }
    
29. 
    6 K- G% N7 D" [0 {1 f: x1 t1 s
30. void Car()
    
31. {
    9 u' K5 b; `7 R% Q8 c2 M
32.   while (1) {
    
33.     CNY70();
    
34. 
    
35.     int error = ((int)values);
    
36.     interror += error;
    
37.     int lasterror = error - olderror;
    
38.     olderror = error;
    
39.     int power = error / 5 + interror / 10000 + lasterror;
    
40. 
    9 N) L% S' S6 Y; q1 G4 H# ~" \. ^
41.     if (power > MotoSpeed)
    , M  c. ^) }- d8 F2 n. e/ m
42.       power = MotoSpeed;
    
43.     if (power < -MotoSpeed)
    8 d1 O/ d- ]2 `4 h1 r8 k5 d
44.       power = -MotoSpeed;
    
45. 
    
46.     if (power > 0)
    % v) y& p1 c  y3 g' v9 B9 n
47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作，正數正轉 負數反轉。
    
48.     else
    7 I* ~- U8 [/ n3 P) }% o# s
49.       Speed(MotoSpeed + power, MotoSpeed);
    
50.   }
    ( V3 X; ^, c, c) T, V3 _0 l  A
51. }

複製代碼

, x7 h: z* K% q6 B6 v0 Q

感謝分享

感謝分享