PID 循跡自走車範例

1. #include <Arduino.h>
   8 v' T( L* }' b* T: O$ o# r- v& S4 K
2. #include <Wire.h>
   
3. #include <Servo.h>
   6 f' w" X; S1 {5 T+ O( \$ |# ]
4. 
   1 h( e" m2 L7 j% k6 N/ q
5. #include "MePort.h"
   
6. #include "MeUltrasonic.h"
   + G% D" S# E9 w% w  N* b
7. #include "MeDCMotor.h"
   
8. 
   8 P! Y6 [; S: M5 P0 m! U% R, K
9. //double Input, Output ;
   
10. float MaxSpeed = 255;
    0 g- ?1 O. s, U; s- |) C) k8 x
11. float MaxPower = 180;
    
12. float MinPower = 120;
    ) P& t6 `* N2 q3 F  K# R) `
13. float Error,ErrorAcc,ErrorDec;
    ! b* k% `1 K3 F' }$ t
14. 
    9 n9 t6 }* k6 b+ n( a
15. float Kp=0.14;
    3 U5 d5 y- F0 R) G
16. float Kd=0.00020;//23;
    ) _# d2 p7 V' d
17. float Ki=0.000201;
    
18. 
    7 j5 D5 u3 G( N, r
19. float nPower;
    * i4 E7 k6 K% N8 o% y* x5 h
20. MePort lightsensor_6(6);
    
21. MePort lightsensor_8(8);
    : Y' U7 A, i8 x
22. MeDCMotor motor_9(9);
    
23. MeDCMotor motor_10(10);
    2 w. N+ q: Z+ t+ ]# P2 ~
24. unsigned long previousMillis = 0;
    7 r% E% S$ ]- O+ h3 {6 u8 d
25. const long interval = 1;
    - `+ m' m' j, S2 K% S, B
26. 
    
27. void setup(){
    5 O$ v6 w! B3 Z
28.     lightsensor_6.dWrite1(1);
    
29.     nPower = 160;
    ' Y; _6 i: J) A. I
30.     Error=0;
    / w& E4 ^6 K4 e; S4 g7 y/ i0 j
31.     ErrorAcc=0;
    
32. }
    ( C" @% f& ^. {5 q+ p
33. 
    
34. void loop(){
    
35.   unsigned long StartTime = millis();
    ) P) ^2 P7 L3 b" X' y- m0 f
36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    
37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    ( e7 X1 P9 u3 ?" i; y
38.     ErrorAcc +=  nError*Kd ;
    
39.     ErrorDec -=  nError*Ki ;
    
40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    9 }$ I' O' b" ]; H+ s
41.     if(nError < 80 && nError > -80){
    
42.       if(nPower < MaxPower){
    ( a& B9 ~8 _3 l6 g$ G' Q+ T
43.         nPower += 3;
    
44.       }
    ! @! `# @3 [. x) H
45.     } else{
    - Q! r" H; A2 X, M2 D& ]$ G; h
46.       if(nPower > MinPower){
    
47.         nPower -= 2;
    9 j, x- A8 A$ h! t4 F. C+ e8 m9 V
48.       }
    
49.     }
    4 |2 V( f/ }5 m5 J4 X2 Y7 G, _, }
50.     MotoL(nPower-Error);
    1 ~; N; p1 b1 h' x/ q- B
51.     MotoR(nPower+Error);   
    
52.   }else{
    ' S( F7 B; h1 I+ v
53.     motor_9.run(0);
    
54.     motor_10.run(0);
    8 @' L4 [; h6 _1 u8 `. G& g7 O7 x
55.   }
    
56.   do{}while(millis() - StartTime < interval);
    5 Q* M: c* ^! B7 X* m9 I
57. }
    # v# V" C2 e7 Y1 L- R: u. |
58. 
    4 c1 m+ s' ^+ O/ |1 r
59. void MotoL(int Power){
    
60.   if (Power > MaxSpeed){
    , |: e4 h7 M( p4 r% f, j/ J3 J' t0 O" s
61.      Power = MaxSpeed;
    
62.   }
    
63.   if (Power < -MaxSpeed){
    
64.      Power = -MaxSpeed;
    ! X4 G8 A3 p; f, h* E
65.   }
      G! U' x8 O& ?" o3 @
66.   motor_9.run(Power);
    
67. }  
    $ @, e$ |" H. T3 y' `* c
68. 
    
69. void MotoR(int Power){
    7 U; L! b5 w7 o% C
70.   if (Power > MaxSpeed){
    
71.      Power = MaxSpeed;
    ! [. v( G0 c0 h) N
72.   }
    * V6 o. y/ W+ m8 `! Y
73.   if (Power < -MaxSpeed){
    ! V1 n; p" G- `. r5 M  Y, V8 h5 ~
74.      Power = -MaxSpeed;
    
75.   }
    ( f( c0 C- W* o" x9 ~/ @+ [
76.   motor_10.run(Power);
    9 Q& B  O: c6 R6 H( x! o. w
77. }  

複製代碼

- L& K+ ]5 z; `- Q( C

, V5 W9 p1 `$ m% t6 l您好，不知是否能向您請教。
+ u5 R# s9 n2 @/ p- G目前和宋修賢老師在處理Ardui Car
雖然已使用較繁雜的方式處理了跑出黑線外的狀況
6 M- A6 h) F0 w, i6 R6 a
但基於想追求更精簡的程式所以還是想請問一下
, }  S0 K7 F0 D4 r" x$ f/ @就是如果我只以單純寫PID控制時，常遇到CNY70跑出黑線外後便往前衝
不知道您是否願意教我可以如何處理
6 E1 j- X  e% T. t, M  m( x- F

& H. F; C4 q% J4 x以下是我挑出我一般寫純PID控制的Code：

1. int MotoSpeed = 250;
   8 H6 y- P! J! ]# ~) C0 M; G
2. double CNY70Val = 1000;
   
3. int interror = 0;
   5 X4 |0 C0 q4 e9 ]1 l
4. int olderror = 0;
   
5. double values;
   0 l% f) R; P/ m( a/ y. _
6. 
   6 c! M6 A% m, B/ t1 l  Z5 l
7. 
   
8. void CNY70()
   
9. {
   
10.   valuesRR = analogRead(RR)
    
11.   valuesMR = analogRead(MR);
    
12.   valuesMM = analogRead(MM);
      s5 z  Q. v$ ~8 c' d% T4 }0 A1 P
13.   valuesML = analogRead(ML);
    9 f; H0 R7 J( P* w; B, n
14.   valuesLL = analogRead(LL);
    
15. 
    ( }1 G: \3 [7 W$ x8 K+ I1 q4 ^
16.   if (valuesRR > CNY70Val)
    + _" d  \8 ]1 n" {7 D6 R  M+ f. k
17.     valuesRR = CNY70Val;
    4 G2 w% i& F& U7 w( z
18.   if (valuesMR > CNY70Val)
    3 |; j8 ]$ H) m0 y. f+ S
19.     valuesMR = CNY70Val;
    " A3 d* i3 L! D; K; y  }2 J
20.   if (valuesMM > CNY70Val)
    5 X$ o) V7 x* f  p& r+ O" P; W
21.     valuesMM = CNY70Val;
    , L3 `# \5 Y5 Z1 U
22.   if (valuesML > CNY70Val)
    " w* ~% d5 ]) ], K8 c9 i
23.     valuesML = CNY70Val;
    9 }% V" j  E0 s+ _+ E
24.   if (valuesLL > CNY70Val)
    
25.     valuesLL = CNY70Val;
    4 N( y  @7 @+ Q6 o
26. 
    8 I* j# ~+ ?/ ~+ S
27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    
28. }
    * Z3 B# @4 o9 R; K
29. 
    $ r0 a& d4 P- N
30. void Car()
    
31. {
    
32.   while (1) {
    # G' P6 j) P  Z
33.     CNY70();
    
34. 
    
35.     int error = ((int)values);
    ' v& n8 n  G0 d! Z
36.     interror += error;
    
37.     int lasterror = error - olderror;
      J  o+ ~- r5 k1 r  V; A
38.     olderror = error;
    
39.     int power = error / 5 + interror / 10000 + lasterror;
    
40. 
    
41.     if (power > MotoSpeed)
    0 D* C1 C8 Y, K
42.       power = MotoSpeed;
    
43.     if (power < -MotoSpeed)
    
44.       power = -MotoSpeed;
    " c8 k4 |0 A6 f% n# p* ^) g& b- L, Q
45. 
    
46.     if (power > 0)
    9 W7 d: w+ u6 P  p) Z
47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作，正數正轉 負數反轉。
    2 j+ }9 L$ q8 a, D2 M
48.     else
    
49.       Speed(MotoSpeed + power, MotoSpeed);
    ; y2 k7 P% K% f3 w( a9 Y7 T- H4 d0 Y
50.   }
    ' [$ V  \$ R# E- Y( Q# C9 N
51. }

複製代碼

感謝分享

感謝分享