PID 循跡自走車範例

1. #include <Arduino.h>
   ' {7 ?* `. ~" l$ M/ @
2. #include <Wire.h>
   ! s- f3 R! L. p! B& p
3. #include <Servo.h>
   - Q) D8 d1 ]; C7 `$ z7 d+ K. k3 s
4. 
   
5. #include "MePort.h"
   
6. #include "MeUltrasonic.h"
   8 o1 y, a: k) f1 i$ m
7. #include "MeDCMotor.h"
   6 v8 T. T6 Q* k) I, u
8. 
   # }1 c* T5 z' ^. J/ u; L3 L
9. //double Input, Output ;
   
10. float MaxSpeed = 255;
    4 ]$ v& b) D' Z
11. float MaxPower = 180;
    
12. float MinPower = 120;
    
13. float Error,ErrorAcc,ErrorDec;
    3 F0 B$ A% [( Z7 l, K5 w
14. 
    
15. float Kp=0.14;
    
16. float Kd=0.00020;//23;
    
17. float Ki=0.000201;
    3 ~; i7 E% _& b6 Y$ q7 L. E
18. 
    
19. float nPower;
    
20. MePort lightsensor_6(6);
    
21. MePort lightsensor_8(8);
      u5 \' f5 e  N# I" c3 ^# {" I
22. MeDCMotor motor_9(9);
    ( @/ q- v! K% f1 @5 O" Q2 x1 M
23. MeDCMotor motor_10(10);
    
24. unsigned long previousMillis = 0;
    6 y! i7 V- L; N/ Y4 V, D- l7 h
25. const long interval = 1;
    * t2 c1 w: @7 n1 u
26. 
    
27. void setup(){
    8 O3 w. ~( k# A
28.     lightsensor_6.dWrite1(1);
    
29.     nPower = 160;
    6 ^' h7 @# `# m' G( A! j) r
30.     Error=0;
    
31.     ErrorAcc=0;
    * H3 C8 V, e! N9 z5 Q
32. }
    2 e2 q& C. k% _
33. 
    ) |* D4 |, e4 X' U/ F# X
34. void loop(){
    
35.   unsigned long StartTime = millis();
    " {. n6 l% D" B" U
36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    3 B4 ]8 Q6 |  o  `' `1 ~
37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    
38.     ErrorAcc +=  nError*Kd ;
    & f& u, Q! P+ V, M. Q3 T7 b! i
39.     ErrorDec -=  nError*Ki ;
    . J6 Z0 ]3 m- i# \$ f& ?
40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    ! f8 \0 A" V( D; e7 f4 _1 u) a& W
41.     if(nError < 80 && nError > -80){
    ! [( J" f$ R: o3 r
42.       if(nPower < MaxPower){
    9 g9 x& f* x) h* J' [
43.         nPower += 3;
    - L$ N+ B" g  K8 B7 C& B
44.       }
    
45.     } else{
    
46.       if(nPower > MinPower){
    $ G0 J0 P; u# c; @- L
47.         nPower -= 2;
    6 _: y' I5 ?5 y1 h
48.       }
    ! ?! ^% ?- f/ b& _" h( V5 w9 }( b# ~
49.     }
    
50.     MotoL(nPower-Error);
    # X) w( |. l2 Q' b0 z, S' W) ^
51.     MotoR(nPower+Error);   
    
52.   }else{
    5 {7 X1 Y# ~3 r* S: Y8 q
53.     motor_9.run(0);
    0 ^. d% r5 T6 ~( C7 f" M3 a
54.     motor_10.run(0);
    
55.   }
    : @7 C$ Q2 z9 D, j- k- S* Y
56.   do{}while(millis() - StartTime < interval);
    ; }( V% }1 j& s# i- d
57. }
    2 m9 Y( b6 o7 b4 m! A. k6 y
58. 
    ' [6 }# H" g2 T' N
59. void MotoL(int Power){
    - h9 L+ X$ [3 r2 b; n0 l' R
60.   if (Power > MaxSpeed){
    + q+ g( Z, `; H8 @, @9 @% y: J
61.      Power = MaxSpeed;
    & |. b3 O" \' `& u8 @4 b: A( J0 H
62.   }
    
63.   if (Power < -MaxSpeed){
    
64.      Power = -MaxSpeed;
    8 m# u1 @# M* ^
65.   }
    
66.   motor_9.run(Power);
    4 C$ G, c# q" u0 \( G3 o4 I
67. }  
    # t" D3 E) a% n% S7 f# h6 j5 ?0 i+ S
68. 
    
69. void MotoR(int Power){
    4 @: T; k4 B$ k' w! i1 i
70.   if (Power > MaxSpeed){
    & l% Y& ^7 d( ~
71.      Power = MaxSpeed;
    
72.   }
    
73.   if (Power < -MaxSpeed){
    , `. K- H$ j5 q; z& o
74.      Power = -MaxSpeed;
    7 k- q; m* _, G
75.   }
    
76.   motor_10.run(Power);
    
77. }  

複製代碼

% V, L  ]/ H8 L
# a- r9 d6 a: R- j您好，不知是否能向您請教。
# u5 {! _# f$ `5 d+ P目前和宋修賢老師在處理Ardui Car
2 d4 V) @6 [4 F7 h; Y( B( B雖然已使用較繁雜的方式處理了跑出黑線外的狀況

但基於想追求更精簡的程式所以還是想請問一下
; `1 v: G  g* Z2 |* J就是如果我只以單純寫PID控制時，常遇到CNY70跑出黑線外後便往前衝
不知道您是否願意教我可以如何處理

  n$ t9 m& f; K
以下是我挑出我一般寫純PID控制的Code：

1. int MotoSpeed = 250;
   5 `1 R, O: x+ ^: c
2. double CNY70Val = 1000;
   / `- K. _- z6 z0 U
3. int interror = 0;
   
4. int olderror = 0;
   6 q! W# l0 X* `- r) j" O' u( F3 I
5. double values;
   
6. 
   ' y) k1 A  M  o0 b2 C: u4 j1 t% O% ]
7. 
   ( R( |/ U! V' X5 |" t  [
8. void CNY70()
   
9. {
   
10.   valuesRR = analogRead(RR)
    + G4 O4 n; Z! e1 j- ~5 R2 O) @
11.   valuesMR = analogRead(MR);
    
12.   valuesMM = analogRead(MM);
    5 g  _' S" N+ @- w
13.   valuesML = analogRead(ML);
    
14.   valuesLL = analogRead(LL);
    8 H5 }8 \% x0 e1 ~
15. 
    
16.   if (valuesRR > CNY70Val)
    
17.     valuesRR = CNY70Val;
    
18.   if (valuesMR > CNY70Val)
    
19.     valuesMR = CNY70Val;
    
20.   if (valuesMM > CNY70Val)
    8 s) o$ A# w5 x/ w5 F
21.     valuesMM = CNY70Val;
    
22.   if (valuesML > CNY70Val)
    
23.     valuesML = CNY70Val;
    
24.   if (valuesLL > CNY70Val)
    
25.     valuesLL = CNY70Val;
    6 M* L% I& h- |# Y4 a
26. 
    
27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    8 z7 ]+ X+ O" m- I
28. }
    ) l$ b1 g. f4 Y/ R3 ?5 }; b. N. P7 ~
29. 
    
30. void Car()
    ( |6 w7 W, _  B5 s/ q, p
31. {
    ! S' |8 c. `* W  b( y9 o
32.   while (1) {
    
33.     CNY70();
    
34. 
    7 |( E/ _* p* m( e
35.     int error = ((int)values);
    + K3 i- O" y$ \  R. G3 z1 T
36.     interror += error;
    9 ^) S; j" \6 L$ A9 D2 Q, Q
37.     int lasterror = error - olderror;
    . w- F+ J; F% i% \* g' K
38.     olderror = error;
    / e' k+ h" t' N/ O
39.     int power = error / 5 + interror / 10000 + lasterror;
    
40. 
    
41.     if (power > MotoSpeed)
    ' I& O% X/ q: V" J9 i" K
42.       power = MotoSpeed;
      N) J$ m4 P/ A5 t6 P
43.     if (power < -MotoSpeed)
    
44.       power = -MotoSpeed;
    : k( E3 \( w; W* B) F9 L( @' N0 d, H
45. 
    * V5 V. i) {7 E4 p
46.     if (power > 0)
    : ^' N7 o5 D" ~5 q5 w6 X
47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作，正數正轉 負數反轉。
    . \2 |& a, f$ h+ S/ U
48.     else
    - _# I% N5 V/ q# E
49.       Speed(MotoSpeed + power, MotoSpeed);
    
50.   }
    1 V2 e" s/ }5 k9 Z1 k5 }5 q; {
51. }

複製代碼

# W# m# u- g+ t3 U0 `/ l
2 Q2 \" z. l/ F- d

感謝分享

感謝分享