PID 循跡自走車範例

1. #include <Arduino.h>
   - e/ ^% o2 J. t
2. #include <Wire.h>
   1 a+ P9 w8 g; w0 c2 @- ?& Y
3. #include <Servo.h>
   8 J* c8 {( T) i+ }) T
4. 
   % L) I  K) [! J( b5 ^- Y3 s
5. #include "MePort.h"
   # v! _& N% }# X# T3 I) E& w
6. #include "MeUltrasonic.h"
   
7. #include "MeDCMotor.h"
   3 z5 s) `- \8 @0 \
8. 
   
9. //double Input, Output ;
   
10. float MaxSpeed = 255;
    
11. float MaxPower = 180;
    
12. float MinPower = 120;
    7 m) m1 y0 J- s" Z+ g: n2 q( c
13. float Error,ErrorAcc,ErrorDec;
    
14. 
    ) ^& }! R, d  a: c6 ^0 t& Q
15. float Kp=0.14;
    
16. float Kd=0.00020;//23;
    
17. float Ki=0.000201;
    
18. 
    5 W/ m# [+ ^3 b. }1 Z+ U
19. float nPower;
    - g5 ?1 g6 @) W# G. N
20. MePort lightsensor_6(6);
    5 B& w8 T, c1 H
21. MePort lightsensor_8(8);
    9 Z) H7 {* P) b4 u' W9 |; F- L
22. MeDCMotor motor_9(9);
    
23. MeDCMotor motor_10(10);
    
24. unsigned long previousMillis = 0;
    2 N" V6 w" l$ V* J* T
25. const long interval = 1;
    
26. 
    , Z% Z3 O  ~8 }, P
27. void setup(){
    
28.     lightsensor_6.dWrite1(1);
    
29.     nPower = 160;
    
30.     Error=0;
    ; k- |/ h; S* v$ i
31.     ErrorAcc=0;
    
32. }
    ( ]6 K; W: i/ U% b
33. 
    
34. void loop(){
    / }2 @- o- c3 X- K5 S) M: O
35.   unsigned long StartTime = millis();
    9 |& A$ F* \; w) K7 K  k
36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    
37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    ( [2 s3 u, g# p+ t) W
38.     ErrorAcc +=  nError*Kd ;
    + L7 R. V" [) F4 m# F- @! M
39.     ErrorDec -=  nError*Ki ;
    
40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    
41.     if(nError < 80 && nError > -80){
    5 ~( a9 f. E" E/ ^
42.       if(nPower < MaxPower){
    0 t# w$ F0 b% I) k8 v7 H
43.         nPower += 3;
    . B" R, u+ w! H  n1 _( e
44.       }
    
45.     } else{
    " N6 x! t" E9 ?$ O
46.       if(nPower > MinPower){
    
47.         nPower -= 2;
    
48.       }
    & @. E! d6 N- _, |2 ^" f8 @: k
49.     }
    . y' T) E2 A& b/ g  x* z% n( T$ L
50.     MotoL(nPower-Error);
    % p9 P, ^& s; I& R8 c1 R7 g3 a6 z9 i
51.     MotoR(nPower+Error);   
    - F" ?; G: j9 X1 H2 }% @
52.   }else{
    
53.     motor_9.run(0);
    ) @4 Z$ ^) C( H2 {" h5 i( a
54.     motor_10.run(0);
    # V% M2 ~) @; t6 L& N& L2 G% l, s
55.   }
    
56.   do{}while(millis() - StartTime < interval);
    
57. }
    9 l0 k  Y/ e. Q9 Q
58. 
    
59. void MotoL(int Power){
    / v2 y. }, Y) r$ J1 ^$ _# k
60.   if (Power > MaxSpeed){
    9 Q4 c' e/ _6 S$ [) P8 N( W
61.      Power = MaxSpeed;
    
62.   }
    
63.   if (Power < -MaxSpeed){
    
64.      Power = -MaxSpeed;
    
65.   }
    
66.   motor_9.run(Power);
    
67. }  
    ; F% ~2 v* b0 R8 }. c% ~
68. 
    3 }& r. i" F0 ?
69. void MotoR(int Power){
    % W' m8 r: C. X- x  D$ X
70.   if (Power > MaxSpeed){
    ( b+ }! B$ w  b$ A
71.      Power = MaxSpeed;
    0 g& h" k8 r+ P3 u
72.   }
    
73.   if (Power < -MaxSpeed){
    
74.      Power = -MaxSpeed;
    
75.   }
    ; n9 c7 }4 Y/ h+ n( v. y2 v
76.   motor_10.run(Power);
    
77. }  

複製代碼

) n' `) S% L# m' D

您好，不知是否能向您請教。
目前和宋修賢老師在處理Ardui Car
雖然已使用較繁雜的方式處理了跑出黑線外的狀況
; E+ D( m; u6 w! o# W0 y  m
但基於想追求更精簡的程式所以還是想請問一下
; b4 ^3 E4 u' {& }% P5 L就是如果我只以單純寫PID控制時，常遇到CNY70跑出黑線外後便往前衝
不知道您是否願意教我可以如何處理


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

1. int MotoSpeed = 250;
   % }; E7 G  [' S3 I3 {0 E! d. {& F
2. double CNY70Val = 1000;
   * Y/ ?& I: T! L' b
3. int interror = 0;
   
4. int olderror = 0;
   
5. double values;
   
6. 
   
7. 
   
8. void CNY70()
   2 _% w0 q0 _- N1 q6 ~- K) W
9. {
   
10.   valuesRR = analogRead(RR)
    $ J4 `$ h$ M9 ~" _& J( F# X
11.   valuesMR = analogRead(MR);
    ( k! G8 ~0 k; B
12.   valuesMM = analogRead(MM);
    " b: P$ \6 @, d2 ]0 S7 ~- j1 x
13.   valuesML = analogRead(ML);
    
14.   valuesLL = analogRead(LL);
    
15. 
    4 x& Z0 W6 h7 S; n2 c
16.   if (valuesRR > CNY70Val)
    # Y; x8 t% R& w' }/ Y2 B; t
17.     valuesRR = CNY70Val;
    ( l" u6 g1 o3 }) L
18.   if (valuesMR > CNY70Val)
    
19.     valuesMR = CNY70Val;
    ' ^' y& }! d1 j- _) ~
20.   if (valuesMM > CNY70Val)
    9 \5 c. r9 C+ u% o
21.     valuesMM = CNY70Val;
    
22.   if (valuesML > CNY70Val)
    % P9 ]/ r  r" o0 x& Q' ^% Q
23.     valuesML = CNY70Val;
    2 o  v9 \0 \7 ?0 W* `6 C
24.   if (valuesLL > CNY70Val)
    ! ]: f5 X- _8 r: h6 A/ ]9 Q
25.     valuesLL = CNY70Val;
    4 v6 y6 ?! q" P1 g
26. 
    
27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    " Z- C2 n9 h  d0 i0 i
28. }
    
29. 
    & V8 k- F0 @6 o. q
30. void Car()
      g3 l; g" G+ ~2 N2 {% a, s
31. {
    
32.   while (1) {
    9 _& w+ A. G8 @+ O3 D
33.     CNY70();
    
34. 
    : C8 X* _7 I$ q6 L( I
35.     int error = ((int)values);
    8 v- d8 O; S1 P
36.     interror += error;
    + O/ a9 G9 e; Z9 U; P
37.     int lasterror = error - olderror;
    
38.     olderror = error;
    ; y: }* N1 E* M$ Y0 ?
39.     int power = error / 5 + interror / 10000 + lasterror;
    9 m. y3 T2 W6 p8 I
40. 
    & H7 W+ }/ E" ]1 a
41.     if (power > MotoSpeed)
    ; U4 `* x8 M  ~' x
42.       power = MotoSpeed;
    
43.     if (power < -MotoSpeed)
    
44.       power = -MotoSpeed;
    
45. 
    . E3 m3 v/ ?8 ^$ o1 `5 y
46.     if (power > 0)
    
47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作，正數正轉 負數反轉。
    
48.     else
    
49.       Speed(MotoSpeed + power, MotoSpeed);
    ' f; P' o  f8 T* q" x6 c) p" B
50.   }
    
51. }

複製代碼

# A$ y5 N9 ]8 ?, m
0 l: s" j' o' h# W

感謝分享

感謝分享