PID 循跡自走車範例

1. #include <Arduino.h>
   8 W2 q9 F* B3 N( A1 L
2. #include <Wire.h>
   5 q* u" N' c! C+ J6 E
3. #include <Servo.h>
   5 @* x6 o8 Y' r' B/ K% D
4. 
   
5. #include "MePort.h"
     {* t9 A) r- v9 N' P" B" K
6. #include "MeUltrasonic.h"
   & M0 E, x/ M' @; K5 \
7. #include "MeDCMotor.h"
   " e2 W! u- d& t; U
8. 
   
9. //double Input, Output ;
   
10. float MaxSpeed = 255;
    + r: `& i6 m! J% I/ h" M: |2 d& o5 s
11. float MaxPower = 180;
    
12. float MinPower = 120;
    
13. float Error,ErrorAcc,ErrorDec;
    ) j/ c& n! @$ }6 P0 G/ _
14. 
    ; \9 u  @2 T- ?) u2 J- P6 d, j
15. float Kp=0.14;
    8 L6 R: }4 v" s% l, |/ u1 L
16. float Kd=0.00020;//23;
    . w& c/ y4 z6 v$ ~! v# r) [& T9 R
17. float Ki=0.000201;
    9 g5 ]5 H* _: ?
18. 
      \' j" q! V0 k* j9 [/ |/ |; Y
19. float nPower;
    / O4 F# L6 F- C, r  p7 }& X
20. MePort lightsensor_6(6);
    " B  m$ V6 v( Z
21. MePort lightsensor_8(8);
    " a& D, M! i0 _& T$ M5 E; q1 a' Q6 H
22. MeDCMotor motor_9(9);
    + U; n) g( r" _" P& ?9 i6 i
23. MeDCMotor motor_10(10);
    ; G; c4 ~" U# [3 q; D
24. unsigned long previousMillis = 0;
    + ?2 [/ n+ D" }# c5 g4 U
25. const long interval = 1;
    
26. 
    
27. void setup(){
    
28.     lightsensor_6.dWrite1(1);
    
29.     nPower = 160;
    4 u$ s1 ?# a$ C
30.     Error=0;
    
31.     ErrorAcc=0;
    
32. }
    4 q# |8 x0 n- R' W0 F; ^
33. 
    1 r5 J9 M, e7 Q8 Y
34. void loop(){
    
35.   unsigned long StartTime = millis();
    
36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    
37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    
38.     ErrorAcc +=  nError*Kd ;
    
39.     ErrorDec -=  nError*Ki ;
    
40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    
41.     if(nError < 80 && nError > -80){
    
42.       if(nPower < MaxPower){
    
43.         nPower += 3;
    1 `6 f. I& B" T3 b( r
44.       }
    
45.     } else{
    
46.       if(nPower > MinPower){
    
47.         nPower -= 2;
    
48.       }
    + c( v# |3 s; _7 E+ d/ i
49.     }
    . }* S& `8 C" x" L2 k0 `
50.     MotoL(nPower-Error);
    . V. e4 r$ a9 i1 o; }# c$ e
51.     MotoR(nPower+Error);   
    9 e  o- I& Q8 n
52.   }else{
    ) T$ E: E1 H4 U4 E% s: e  m7 q2 d
53.     motor_9.run(0);
    7 H& [; w2 S: `- h; @, k0 n2 ?
54.     motor_10.run(0);
    
55.   }
    $ Y0 Q. U2 _$ _  `* \6 ?
56.   do{}while(millis() - StartTime < interval);
    
57. }
      i' ]3 d. R6 G
58. 
    / A! E0 {, q2 v8 m
59. void MotoL(int Power){
    
60.   if (Power > MaxSpeed){
    : H" W; b1 D1 K4 V7 m
61.      Power = MaxSpeed;
    ( m$ j  }1 b, q
62.   }
    
63.   if (Power < -MaxSpeed){
    ( Q. b: G# R0 @% v/ s
64.      Power = -MaxSpeed;
    # ^* p  }; b+ R' _. R
65.   }
    ' l: p% T: Q! |5 C; J
66.   motor_9.run(Power);
    
67. }  
    
68. 
    
69. void MotoR(int Power){
    , F2 @! V4 R, |+ r, }9 _
70.   if (Power > MaxSpeed){
    
71.      Power = MaxSpeed;
    6 v" x' y* g0 _; I: Y( D/ I
72.   }
    
73.   if (Power < -MaxSpeed){
    0 @' V& l& q, {9 E, B8 }
74.      Power = -MaxSpeed;
    
75.   }
    
76.   motor_10.run(Power);
    
77. }  

複製代碼

, K0 b' c3 I; Z* c* `* `& B
- I/ j7 |7 y; [; E8 y9 k- H1 n" X

  }7 D) }0 `* Q* d/ X7 o
您好，不知是否能向您請教。
目前和宋修賢老師在處理Ardui Car
6 k/ R8 m! K; F3 A9 }雖然已使用較繁雜的方式處理了跑出黑線外的狀況

, @! ?+ V' w) m  S( i+ @8 }# i' k但基於想追求更精簡的程式所以還是想請問一下
5 V8 D+ [; N2 O4 Z就是如果我只以單純寫PID控制時，常遇到CNY70跑出黑線外後便往前衝
不知道您是否願意教我可以如何處理
; W; [1 P- }0 `  \
* ]1 m5 n  p2 g8 a4 W$ F
$ d7 J2 ~$ s$ p# G* l, J( t以下是我挑出我一般寫純PID控制的Code：

1. int MotoSpeed = 250;
   ! m9 U) \- p# b6 U8 G! m
2. double CNY70Val = 1000;
   
3. int interror = 0;
   - V& O: u1 ?1 B$ k
4. int olderror = 0;
   
5. double values;
   - a9 _3 w' i9 p3 Y/ [5 c) y5 v) Z
6. 
   ) C; r+ }) I2 N9 I
7. 
   ) ]; G+ U& ]$ f
8. void CNY70()
   % b- A6 ~; u2 k( \
9. {
   2 B$ n, B; h# J6 `% d; x# J1 ]5 N
10.   valuesRR = analogRead(RR)
    
11.   valuesMR = analogRead(MR);
    
12.   valuesMM = analogRead(MM);
    
13.   valuesML = analogRead(ML);
    
14.   valuesLL = analogRead(LL);
    
15. 
    : l" {0 B; S. }; M& P7 B6 h: B
16.   if (valuesRR > CNY70Val)
    
17.     valuesRR = CNY70Val;
    
18.   if (valuesMR > CNY70Val)
    * ~* L- {% Y/ ~1 l% ~* ^$ g9 A
19.     valuesMR = CNY70Val;
    
20.   if (valuesMM > CNY70Val)
    
21.     valuesMM = CNY70Val;
    2 A" J4 h5 i3 V) `( Z
22.   if (valuesML > CNY70Val)
    
23.     valuesML = CNY70Val;
    3 w0 G  e( T( ^$ b2 ^, X; S" ]
24.   if (valuesLL > CNY70Val)
    
25.     valuesLL = CNY70Val;
      B* Z/ A/ X7 K& w! v9 V0 b$ J
26. 
    7 d; `+ e% }) t0 w: A
27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    
28. }
    % `" V: s$ I1 d7 S5 v
29. 
    
30. void Car()
    
31. {
    
32.   while (1) {
    
33.     CNY70();
    1 I3 G' x" l9 F0 |4 }$ s, t
34. 
    - T) ?/ l  w! i; y
35.     int error = ((int)values);
    
36.     interror += error;
    ' }1 u1 p5 e5 Z5 [, ]
37.     int lasterror = error - olderror;
    
38.     olderror = error;
    
39.     int power = error / 5 + interror / 10000 + lasterror;
    2 y  ]* o2 p* }% S
40. 
    
41.     if (power > MotoSpeed)
    
42.       power = MotoSpeed;
    
43.     if (power < -MotoSpeed)
    
44.       power = -MotoSpeed;
      a% L' _' J  W2 _" ]5 I
45. 
    
46.     if (power > 0)
    
47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作，正數正轉 負數反轉。
    3 I( u3 i8 i6 q/ H6 O
48.     else
    . W% }5 U7 P- u2 W
49.       Speed(MotoSpeed + power, MotoSpeed);
    7 u/ x3 K8 E* ~2 K5 C
50.   }
    4 i# H# ]5 ~5 X
51. }

複製代碼

' v' ^& i5 \$ A0 c* Z  Q
# @* A/ [9 R' I+ V! W- ^" b

感謝分享

感謝分享