PID 循跡自走車範例

1. #include <Arduino.h>
   
2. #include <Wire.h>
   6 K' \9 ]" ?" |
3. #include <Servo.h>
   7 C: {7 b- q1 h" [4 X
4. 
   
5. #include "MePort.h"
   
6. #include "MeUltrasonic.h"
   $ V( }8 B, Q" i5 f, H
7. #include "MeDCMotor.h"
   
8. 
   8 E& `/ |3 }: _
9. //double Input, Output ;
   
10. float MaxSpeed = 255;
    0 @8 C9 `' L: M
11. float MaxPower = 180;
    6 ?& d9 Y- _! L: Y4 O, `( V
12. float MinPower = 120;
    : e4 N( O' d- q5 X9 H
13. float Error,ErrorAcc,ErrorDec;
    
14. 
    ' p9 E: x0 l; }
15. float Kp=0.14;
    
16. float Kd=0.00020;//23;
    
17. float Ki=0.000201;
    
18. 
    - @! C% [( _) b( n
19. float nPower;
    
20. MePort lightsensor_6(6);
    
21. MePort lightsensor_8(8);
    
22. MeDCMotor motor_9(9);
    
23. MeDCMotor motor_10(10);
    
24. unsigned long previousMillis = 0;
    ! Q  U! Y9 p4 |( o4 _2 q+ T
25. const long interval = 1;
    
26. 
    ( k* G: G8 I. I8 T( P2 ~; v3 o
27. void setup(){
    % B; G% S; U. l& I; B* v+ k! O
28.     lightsensor_6.dWrite1(1);
    
29.     nPower = 160;
    
30.     Error=0;
    
31.     ErrorAcc=0;
    
32. }
    5 A4 x$ n8 K4 G8 W# _
33. 
    
34. void loop(){
    8 M5 ?0 y4 O) _" b8 |/ w  ]$ c
35.   unsigned long StartTime = millis();
    
36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    
37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    8 j/ e( T9 h/ b0 J! P# H0 G2 \# V9 w
38.     ErrorAcc +=  nError*Kd ;
    % A( O1 X5 S6 V6 }) Y
39.     ErrorDec -=  nError*Ki ;
    
40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    * `: y3 t+ E' {
41.     if(nError < 80 && nError > -80){
    
42.       if(nPower < MaxPower){
    
43.         nPower += 3;
    
44.       }
    $ ]" q, j4 I3 H5 L6 Z
45.     } else{
    
46.       if(nPower > MinPower){
    * p& t, N$ f, B) `  H
47.         nPower -= 2;
    - Y2 u0 Q1 H: ?8 [% \+ C
48.       }
    ) |, Q: |/ b1 `! Z5 K
49.     }
    
50.     MotoL(nPower-Error);
    ( I) {) V, i2 P5 s; m. f
51.     MotoR(nPower+Error);   
    
52.   }else{
    5 d+ {8 w& ?5 l: m6 `: W; ]/ `
53.     motor_9.run(0);
    
54.     motor_10.run(0);
    & x+ h- L* y9 A/ y) o  S! E# l
55.   }
    - r" z% N5 M4 m" @5 g
56.   do{}while(millis() - StartTime < interval);
    
57. }
    3 a$ ^" [5 Y# H# d4 M
58. 
    ; k3 O% }. O# P
59. void MotoL(int Power){
    , F2 m" m+ D% n" x/ u6 E
60.   if (Power > MaxSpeed){
      v# d0 p$ ^1 p( T! R& |
61.      Power = MaxSpeed;
    
62.   }
    
63.   if (Power < -MaxSpeed){
    
64.      Power = -MaxSpeed;
    6 c: l, c: E- L5 L- Y" m
65.   }
    5 [+ y7 m* T: O
66.   motor_9.run(Power);
    
67. }  
    8 q1 o& D# k2 {2 ~, B* G' n" M
68. 
    . I* |! s4 o$ D
69. void MotoR(int Power){
    ! p6 `+ X3 p% W! q+ b
70.   if (Power > MaxSpeed){
    " l& h: l: M& J: ?5 \' z3 P/ x
71.      Power = MaxSpeed;
    7 a1 J1 \' s% m( @: o5 x) U0 j( b
72.   }
    
73.   if (Power < -MaxSpeed){
    - L5 s' y8 I9 Y& p# ^) j
74.      Power = -MaxSpeed;
      X( V9 `* j/ o6 C4 n0 o4 k
75.   }
      |) S  n$ P& J: \- }+ c( T
76.   motor_10.run(Power);
    
77. }  

複製代碼

# r7 q  i. T6 P+ [" |  w7 n

; v( r7 m) Z3 B: e( b
* s7 L* ?! G% ?% O* V  ?您好，不知是否能向您請教。
) f+ I) Q+ w* u! l7 q' }目前和宋修賢老師在處理Ardui Car
  @& _6 r; ^0 y8 M: J7 @0 e雖然已使用較繁雜的方式處理了跑出黑線外的狀況

但基於想追求更精簡的程式所以還是想請問一下
9 i5 \2 j! k0 z$ ^4 _就是如果我只以單純寫PID控制時，常遇到CNY70跑出黑線外後便往前衝
不知道您是否願意教我可以如何處理
5 K  a) ~1 d. W' b. V

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

1. int MotoSpeed = 250;
   : w) R* i  V( Z/ h  a7 L
2. double CNY70Val = 1000;
   7 S+ Q" I3 n; U& u
3. int interror = 0;
   
4. int olderror = 0;
   & {0 ]8 B4 j) J, N
5. double values;
   7 Y0 |+ {1 W8 k) k$ ~2 z- g2 n: O
6. 
   
7. 
   1 A, P! f, w; D8 q; T5 Q. n% ^
8. void CNY70()
   3 I5 d3 M- n5 z" y4 \- R
9. {
   
10.   valuesRR = analogRead(RR)
    0 g9 [7 R" Z% @, D- q
11.   valuesMR = analogRead(MR);
    7 v* ^. M* |& A  O
12.   valuesMM = analogRead(MM);
    7 j8 w* y' u) D3 \7 ~5 @+ k0 x
13.   valuesML = analogRead(ML);
    ' W) \2 L/ C+ {1 h/ g  I
14.   valuesLL = analogRead(LL);
    0 K' J2 @' s" h. h( {
15. 
    
16.   if (valuesRR > CNY70Val)
    + P+ u9 g( N: @% M1 c1 N
17.     valuesRR = CNY70Val;
    
18.   if (valuesMR > CNY70Val)
    
19.     valuesMR = CNY70Val;
    2 C6 f  P. F( }8 R' u/ v* j* v" P
20.   if (valuesMM > CNY70Val)
    
21.     valuesMM = CNY70Val;
    
22.   if (valuesML > CNY70Val)
    & C9 P7 ^% j, O' K7 m1 K. K0 ~0 N
23.     valuesML = CNY70Val;
    9 @1 u8 ?1 E( z9 R8 @
24.   if (valuesLL > CNY70Val)
    
25.     valuesLL = CNY70Val;
    
26. 
    
27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    
28. }
      y( w. N5 g! D0 m2 F- Z
29. 
    ' ]3 M1 g' L, z
30. void Car()
    
31. {
    0 ?/ E+ a" X- m8 m. Q
32.   while (1) {
    
33.     CNY70();
    
34. 
    
35.     int error = ((int)values);
    6 J; s) W6 b! T( H: a* E+ c
36.     interror += error;
    8 G5 w5 _) h6 l& x, g0 v8 k3 r3 o
37.     int lasterror = error - olderror;
    
38.     olderror = error;
    
39.     int power = error / 5 + interror / 10000 + lasterror;
    + N% e& Z, R, L6 n( b6 ?
40. 
    
41.     if (power > MotoSpeed)
    
42.       power = MotoSpeed;
    
43.     if (power < -MotoSpeed)
    $ Z: h3 p7 Y7 i. Q7 }. R, R
44.       power = -MotoSpeed;
    % \1 y: M+ m5 ~) H1 [
45. 
    
46.     if (power > 0)
    
47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作，正數正轉 負數反轉。
    ) l+ V& I2 C# P0 K- c$ M
48.     else
    4 s% a& A! V8 z/ e8 Y+ |9 E
49.       Speed(MotoSpeed + power, MotoSpeed);
    1 i9 m( {% d- T! D
50.   }
    & F% _: I' u. U; x& W3 w5 x
51. }

複製代碼

% E+ J- |# X) l. V1 N/ Z$ h

感謝分享

感謝分享