PID 循跡自走車範例

1. #include <Arduino.h>
   
2. #include <Wire.h>
   $ c, x3 {9 [6 g( `3 w  T* M
3. #include <Servo.h>
   / u* n  X: X  p; ]; Y1 p9 w
4. 
   3 N3 ?& ?/ u8 y. H" ^5 @* X) v
5. #include "MePort.h"
   + }2 H( W/ D# m# Z
6. #include "MeUltrasonic.h"
   
7. #include "MeDCMotor.h"
   % E* {. J" t) d/ W! J  x* L
8. 
   ' U4 Z2 B; n( Y  X2 s) N
9. //double Input, Output ;
   : J5 F3 q' q% _2 _  w
10. float MaxSpeed = 255;
    
11. float MaxPower = 180;
    
12. float MinPower = 120;
    
13. float Error,ErrorAcc,ErrorDec;
    $ l6 E3 o9 {$ _3 K7 E' I5 [
14. 
    0 [% W  ?4 V: o& T  x. [. _, v
15. float Kp=0.14;
    4 B, y9 K% L2 K+ f, b7 \4 m
16. float Kd=0.00020;//23;
    
17. float Ki=0.000201;
    
18. 
    * M9 ]" _6 |& s+ Q* ?
19. float nPower;
    ' S4 U1 X5 O4 A* H  V% n2 E
20. MePort lightsensor_6(6);
    1 {$ K( w$ X% Z
21. MePort lightsensor_8(8);
    : f5 ^: R- }5 A1 \6 E/ Q" [$ m
22. MeDCMotor motor_9(9);
    # x% R8 r6 Y7 A; K% v( q
23. MeDCMotor motor_10(10);
    ' @8 g3 @( z2 B. u! r! n) E$ z. O
24. unsigned long previousMillis = 0;
    9 [) v7 I6 Q7 U* m2 p
25. const long interval = 1;
    
26. 
    3 Y- T9 h# x+ D5 i
27. void setup(){
    % L' ~( S7 W$ L/ ?7 j+ k5 Y1 i6 v
28.     lightsensor_6.dWrite1(1);
    ) K) L! n. u- a8 O2 V
29.     nPower = 160;
    
30.     Error=0;
    + ~7 t8 r3 y. n8 X3 @
31.     ErrorAcc=0;
    
32. }
    9 F6 \) E3 q/ I3 s7 R, T4 L4 j$ @
33. 
    
34. void loop(){
    / N5 o3 W7 Z5 c$ w; ?
35.   unsigned long StartTime = millis();
    $ E. L- _; ~! I8 y) p" O3 ~. v
36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    
37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    
38.     ErrorAcc +=  nError*Kd ;
    % C( j% w2 b5 ~
39.     ErrorDec -=  nError*Ki ;
    
40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    
41.     if(nError < 80 && nError > -80){
    
42.       if(nPower < MaxPower){
    
43.         nPower += 3;
    
44.       }
    
45.     } else{
    $ z: n% \3 m  U7 A9 m
46.       if(nPower > MinPower){
    
47.         nPower -= 2;
    
48.       }
    8 p/ s8 O: J( [* F, p) Z
49.     }
    
50.     MotoL(nPower-Error);
      X# R3 V) `5 p
51.     MotoR(nPower+Error);   
    
52.   }else{
    
53.     motor_9.run(0);
    
54.     motor_10.run(0);
    
55.   }
    
56.   do{}while(millis() - StartTime < interval);
      n- z# w8 y, l0 H$ M6 ~, V
57. }
    
58. 
    9 j1 ^# y( y) `4 w3 R
59. void MotoL(int Power){
    
60.   if (Power > MaxSpeed){
    # c5 Q$ L* W: e' ~1 S
61.      Power = MaxSpeed;
    
62.   }
    4 A; b3 E$ E+ I- ~
63.   if (Power < -MaxSpeed){
    : M+ o4 I1 P8 _9 o0 L) a. E# b+ N* t
64.      Power = -MaxSpeed;
    9 d  {4 }5 `2 r
65.   }
    
66.   motor_9.run(Power);
    
67. }  
    ! r/ @( p. ^  _2 e/ m7 ?1 `8 X9 ~
68. 
      ^' `; l* e( O! K% L
69. void MotoR(int Power){
    ! t; P9 M% i& v% N0 s, H! _; q( ~
70.   if (Power > MaxSpeed){
    # X- ?7 E8 b3 n/ ^, v
71.      Power = MaxSpeed;
    $ L! a$ ]5 n: T' i4 }
72.   }
    
73.   if (Power < -MaxSpeed){
    
74.      Power = -MaxSpeed;
    $ ~7 x" X8 y6 [! h! J/ y- F
75.   }
    0 W4 ~# }* B. k' U" y% C+ r! P) a$ X
76.   motor_10.run(Power);
    
77. }  

複製代碼

2 S6 M; {* L. h1 g" V) }* P

+ k% L  x0 n5 H, \8 d" K您好，不知是否能向您請教。
- O1 G: ^( t+ ?* B7 p目前和宋修賢老師在處理Ardui Car
8 H" m1 f5 o9 i$ _雖然已使用較繁雜的方式處理了跑出黑線外的狀況
% R3 j8 \& n$ Q0 I5 \* X
' Q# d2 a- e2 Z% b) @但基於想追求更精簡的程式所以還是想請問一下
! v6 m1 ~$ ~3 k* _就是如果我只以單純寫PID控制時，常遇到CNY70跑出黑線外後便往前衝
/ h9 f1 Q9 a' d' B: F$ _不知道您是否願意教我可以如何處理
+ L  Z/ Z6 H5 h; D3 C  E3 I* h

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

1. int MotoSpeed = 250;
   ( n2 d8 ]! S& x& m7 L4 I3 q: b* D
2. double CNY70Val = 1000;
   
3. int interror = 0;
   % i' F: H6 w# H9 {
4. int olderror = 0;
   # Q4 ], Z, S/ q
5. double values;
   
6. 
   
7. 
   
8. void CNY70()
   ! D" K% I. R  C7 x
9. {
   / C. s' A' `% O8 c
10.   valuesRR = analogRead(RR)
    
11.   valuesMR = analogRead(MR);
    ' T5 p7 o0 l  U
12.   valuesMM = analogRead(MM);
    
13.   valuesML = analogRead(ML);
    
14.   valuesLL = analogRead(LL);
    
15. 
    * {, V9 i% c$ v" b
16.   if (valuesRR > CNY70Val)
    ( j3 l% W6 Z. d& j* G, c* w& O
17.     valuesRR = CNY70Val;
      a1 T0 t; m- J3 `8 b7 b) ^
18.   if (valuesMR > CNY70Val)
    
19.     valuesMR = CNY70Val;
    
20.   if (valuesMM > CNY70Val)
    
21.     valuesMM = CNY70Val;
    % L6 R! y8 K) ^6 S+ `
22.   if (valuesML > CNY70Val)
    
23.     valuesML = CNY70Val;
    
24.   if (valuesLL > CNY70Val)
    6 u6 E- _% t8 c- f1 Z8 F- Z
25.     valuesLL = CNY70Val;
    
26. 
    
27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    . b- J) j4 y  c
28. }
    
29. 
    $ L# ?- |" a1 x* H2 X" x
30. void Car()
    
31. {
    
32.   while (1) {
    
33.     CNY70();
    ; j* F: o# ~, t7 J' p) M
34. 
    
35.     int error = ((int)values);
    . ?4 O4 c0 Q7 u
36.     interror += error;
    ! ?/ ?9 t6 D9 }+ o/ b" t
37.     int lasterror = error - olderror;
    
38.     olderror = error;
    
39.     int power = error / 5 + interror / 10000 + lasterror;
    
40. 
      I1 }- B: @. X4 X; {; |3 Y/ T/ m, C
41.     if (power > MotoSpeed)
    . X( p3 e, h$ F8 n2 A& {
42.       power = MotoSpeed;
    
43.     if (power < -MotoSpeed)
    " M9 w) T' e1 O% |
44.       power = -MotoSpeed;
    
45. 
    2 X* o( s- q7 v1 e0 X* _! R
46.     if (power > 0)
    : E: B, y+ j- I2 `1 \
47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作，正數正轉 負數反轉。
    % v1 S( c0 _: |- @9 a
48.     else
    
49.       Speed(MotoSpeed + power, MotoSpeed);
      z% V& ^2 s1 p2 j/ u) G
50.   }
    8 g3 d- J7 J2 T0 s1 n, V! E
51. }

複製代碼

感謝分享

感謝分享