PID 循跡自走車範例

1. #include <Arduino.h>
   
2. #include <Wire.h>
   - ^6 ?$ P1 K1 {% t" @) s
3. #include <Servo.h>
   & I$ |6 V% F% \
4. 
   
5. #include "MePort.h"
   : t, b0 O: E+ l2 J) N( m8 f( _
6. #include "MeUltrasonic.h"
   
7. #include "MeDCMotor.h"
   - l$ p8 F, s2 s! v) G) T3 |+ ?
8. 
   
9. //double Input, Output ;
   ! M( b3 K! m) J3 J. x1 r4 }
10. float MaxSpeed = 255;
    1 r* O6 ^9 h7 w
11. float MaxPower = 180;
    
12. float MinPower = 120;
    
13. float Error,ErrorAcc,ErrorDec;
    . g- i% K8 h5 w
14. 
      c* }# b% c& `: B* i7 Y+ W
15. float Kp=0.14;
    
16. float Kd=0.00020;//23;
    
17. float Ki=0.000201;
    5 D* F' ?. U6 y) o' L
18. 
    ; W4 h7 u* a6 W0 Z3 e1 f& `
19. float nPower;
    
20. MePort lightsensor_6(6);
    # N/ W7 [! ?+ x* u: `' G2 ~
21. MePort lightsensor_8(8);
      @1 }3 }% `6 P/ v" @( ?- p
22. MeDCMotor motor_9(9);
    8 [" b8 t* t/ b( g5 a- w7 V3 L
23. MeDCMotor motor_10(10);
    " ]  K' X0 G5 [+ m+ w
24. unsigned long previousMillis = 0;
    
25. const long interval = 1;
    
26. 
    
27. void setup(){
    4 D7 }; b/ ]$ }- _9 _- m
28.     lightsensor_6.dWrite1(1);
    ; M* T+ M8 k  M  r
29.     nPower = 160;
    
30.     Error=0;
    
31.     ErrorAcc=0;
    : K$ W& o! m  ~
32. }
    
33. 
    % o; j0 L3 n' O* ]
34. void loop(){
    
35.   unsigned long StartTime = millis();
    0 B: [2 ]& E# W, ^& C# N
36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
      _. l, }3 L& x8 m
37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    / x7 F3 S$ B0 h, p9 w7 e
38.     ErrorAcc +=  nError*Kd ;
    1 i8 s# Y9 q1 r$ g
39.     ErrorDec -=  nError*Ki ;
    
40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    8 ^7 l' U% c8 ]+ K1 T
41.     if(nError < 80 && nError > -80){
    7 J4 T5 K. b3 V
42.       if(nPower < MaxPower){
    ! J9 w5 t& {9 X7 J/ M9 y4 j
43.         nPower += 3;
    , `) Q; u/ l; u5 F8 J5 X6 w
44.       }
    - Q- ~7 @" _. Y! w; z" T
45.     } else{
    
46.       if(nPower > MinPower){
    9 M2 P  H2 o' M; y) g2 [7 H; Y
47.         nPower -= 2;
    
48.       }
    0 N7 O+ ~" i. v" ^. T7 W
49.     }
    " w  m. I  k$ }/ V" R( O
50.     MotoL(nPower-Error);
    0 g) H7 t5 N; {: h& A: Q/ o
51.     MotoR(nPower+Error);   
    ' s! G3 j7 ~3 ~' k  ~
52.   }else{
    
53.     motor_9.run(0);
    ( \/ g( D! \1 s1 }4 X0 k2 w
54.     motor_10.run(0);
    & Q/ V' o8 I  l8 a" J5 W( t
55.   }
    8 \* r5 i; I! B1 q3 ?
56.   do{}while(millis() - StartTime < interval);
    
57. }
    
58. 
    
59. void MotoL(int Power){
    
60.   if (Power > MaxSpeed){
    2 w4 }: [; h2 F# }5 _$ |* M5 n' D
61.      Power = MaxSpeed;
    
62.   }
    ) I2 I1 H8 s0 A' N
63.   if (Power < -MaxSpeed){
    
64.      Power = -MaxSpeed;
    
65.   }
    
66.   motor_9.run(Power);
    
67. }  
    
68. 
    
69. void MotoR(int Power){
    
70.   if (Power > MaxSpeed){
    6 e* X2 g- K3 {# [  L( U# U
71.      Power = MaxSpeed;
    
72.   }
    ' O7 ], F& o1 u. K
73.   if (Power < -MaxSpeed){
    " w& f! h- s$ u1 H. u
74.      Power = -MaxSpeed;
    9 A" e5 ^% x2 |; J
75.   }
    
76.   motor_10.run(Power);
    8 l+ }4 o2 E4 S8 A# @
77. }  

複製代碼

' j5 `+ H/ Q2 c- h2 s' E$ T您好，不知是否能向您請教。
  N% g4 V" f2 y目前和宋修賢老師在處理Ardui Car
雖然已使用較繁雜的方式處理了跑出黑線外的狀況
0 A# p( n$ K+ Y. x2 b0 f9 }
- @2 G* n# B6 ]9 l- G) i$ r) c但基於想追求更精簡的程式所以還是想請問一下
7 y6 h# ?2 j! C+ A就是如果我只以單純寫PID控制時，常遇到CNY70跑出黑線外後便往前衝
5 d+ D& J+ W8 [& k, K不知道您是否願意教我可以如何處理

- C& R' S! B' y
7 M8 E8 M2 O* i以下是我挑出我一般寫純PID控制的Code：

1. int MotoSpeed = 250;
   & H4 q# O* _: P$ e7 W
2. double CNY70Val = 1000;
   
3. int interror = 0;
   
4. int olderror = 0;
   - c. ^+ }- r! N) X' j% }! w
5. double values;
   + ]# y" W7 H( Q7 U- `9 E
6. 
   % U6 j8 I& M* k2 p
7. 
   " q. \& w6 S$ T4 s. V) S+ @: M
8. void CNY70()
   
9. {
   1 l9 }4 y7 z7 w
10.   valuesRR = analogRead(RR)
    
11.   valuesMR = analogRead(MR);
    
12.   valuesMM = analogRead(MM);
    
13.   valuesML = analogRead(ML);
    ; h* H. t$ @: D( V+ ]1 Z3 n7 `
14.   valuesLL = analogRead(LL);
    
15. 
    ' o( o3 e) \/ k9 M0 @! k
16.   if (valuesRR > CNY70Val)
    ; O: B/ n& R( _0 D" P$ D
17.     valuesRR = CNY70Val;
    
18.   if (valuesMR > CNY70Val)
    % f/ w& a: [+ R0 N) w
19.     valuesMR = CNY70Val;
    
20.   if (valuesMM > CNY70Val)
    2 E" O; R- D4 M, I3 s
21.     valuesMM = CNY70Val;
    
22.   if (valuesML > CNY70Val)
    
23.     valuesML = CNY70Val;
    
24.   if (valuesLL > CNY70Val)
    
25.     valuesLL = CNY70Val;
    
26. 
    ! x5 e) h8 f+ J
27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
      P( f( K$ [% t! s4 Q9 h" U2 J
28. }
    ! ?; n3 f7 o; R4 s) w
29. 
    ) l# y! n9 r  Q& ?4 e0 H
30. void Car()
    
31. {
    
32.   while (1) {
    
33.     CNY70();
    
34. 
    + Y6 q' l5 c$ _( l' r/ v
35.     int error = ((int)values);
    
36.     interror += error;
    % m! H9 _2 S4 s7 H7 C. N! a- L
37.     int lasterror = error - olderror;
    
38.     olderror = error;
    * H$ W; i) A! k" ]# Y# E
39.     int power = error / 5 + interror / 10000 + lasterror;
    
40. 
    
41.     if (power > MotoSpeed)
    + J, V6 [  l' I7 k: ?+ B5 o
42.       power = MotoSpeed;
    ( N" L" {. r' O5 ?
43.     if (power < -MotoSpeed)
    8 Y0 b7 h& H- M  \" X4 ?# ?
44.       power = -MotoSpeed;
    & H2 ]% \0 U; g
45. 
    / F4 R4 |$ C& d$ F
46.     if (power > 0)
    
47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作，正數正轉 負數反轉。
    
48.     else
    ' E: a- P5 j- j: o
49.       Speed(MotoSpeed + power, MotoSpeed);
    5 a$ h. ?0 _5 E' ^; x( Q
50.   }
    3 L8 g, G7 U2 _& q/ u  P. W
51. }

複製代碼

8 ^3 ?$ P/ n: q: r6 L1 G5 }

感謝分享

感謝分享