PID 循跡自走車範例

1. #include <Arduino.h>
   
2. #include <Wire.h>
   
3. #include <Servo.h>
     }' n3 h2 L+ E/ U
4. 
   % ]2 b2 c( `$ W7 M
5. #include "MePort.h"
   
6. #include "MeUltrasonic.h"
   3 F* T/ n. P8 i) J/ j
7. #include "MeDCMotor.h"
   / `( M/ V6 X! q8 p1 ]
8. 
   
9. //double Input, Output ;
   
10. float MaxSpeed = 255;
    * J: O: k" I3 Q8 r/ b
11. float MaxPower = 180;
    # D1 U5 l7 n' `2 d1 f
12. float MinPower = 120;
    / i3 j' z& X5 o( _- C# o
13. float Error,ErrorAcc,ErrorDec;
    
14. 
    
15. float Kp=0.14;
    
16. float Kd=0.00020;//23;
    , R" L7 o) G5 I* J2 h
17. float Ki=0.000201;
    
18. 
    4 U/ V% W( v  z5 @2 O: I1 j; x
19. float nPower;
    $ p4 y4 T4 h2 S  ]8 V# E, @% t
20. MePort lightsensor_6(6);
    + N% G- {9 J# K& x. a3 a
21. MePort lightsensor_8(8);
    
22. MeDCMotor motor_9(9);
    
23. MeDCMotor motor_10(10);
    
24. unsigned long previousMillis = 0;
    
25. const long interval = 1;
    . l  h, T+ b) D" ]" f
26. 
    1 J' j$ s5 t: @1 H5 D% i
27. void setup(){
    
28.     lightsensor_6.dWrite1(1);
    # }  [; Z0 M2 P3 H+ a
29.     nPower = 160;
    
30.     Error=0;
    . i2 c$ `8 ~' o  s5 m
31.     ErrorAcc=0;
    6 }  U/ u+ T  j
32. }
    
33. 
    6 H7 l/ _$ n0 Z1 U% s) q  T
34. void loop(){
    $ J: b! t6 ]5 F! b
35.   unsigned long StartTime = millis();
    0 ^' R. M7 _) i( y2 N) \% @8 C- S% `
36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    
37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    / U1 k0 B+ W6 ?0 K* S/ A2 ?
38.     ErrorAcc +=  nError*Kd ;
    
39.     ErrorDec -=  nError*Ki ;
    
40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    
41.     if(nError < 80 && nError > -80){
    4 a* [$ K, _8 G. O+ \
42.       if(nPower < MaxPower){
    
43.         nPower += 3;
    & E9 o. \7 }$ v0 Q" `
44.       }
    # m; Q6 L) Q6 m3 Z  M
45.     } else{
    ' B% k& m0 n5 k5 j
46.       if(nPower > MinPower){
    ! N' K/ r" t. B" L8 n
47.         nPower -= 2;
    
48.       }
    - r/ w" x5 I3 C6 n
49.     }
    - L+ r& ^* x' ^; c9 i& c( B
50.     MotoL(nPower-Error);
    
51.     MotoR(nPower+Error);   
    ; t+ R/ K& m5 [- @5 M( g" K
52.   }else{
    
53.     motor_9.run(0);
    
54.     motor_10.run(0);
    6 G5 d( R) t! r: E2 D, f8 _9 c
55.   }
    ; D# G. m! Q. ]1 {
56.   do{}while(millis() - StartTime < interval);
    
57. }
    
58. 
    4 K2 o7 C* \8 j1 U8 n
59. void MotoL(int Power){
    
60.   if (Power > MaxSpeed){
    
61.      Power = MaxSpeed;
    
62.   }
    
63.   if (Power < -MaxSpeed){
      q( }* O. y' X" {9 P" `8 Y, C
64.      Power = -MaxSpeed;
    
65.   }
    
66.   motor_9.run(Power);
    
67. }  
    - w8 r& S! n3 i
68. 
    
69. void MotoR(int Power){
    . A: b  O: p6 W& T3 E7 g
70.   if (Power > MaxSpeed){
    # t6 b7 R% I* g& A4 w  F
71.      Power = MaxSpeed;
    
72.   }
    
73.   if (Power < -MaxSpeed){
    8 u* o; c( K: L' _2 J+ X7 C
74.      Power = -MaxSpeed;
    
75.   }
    & _% e4 u% x# Q
76.   motor_10.run(Power);
    
77. }  

複製代碼

" f* G# e$ |' J) S  E$ U: ~4 S' j1 E

# K; v6 N; ^& {, s- K& o$ ?您好，不知是否能向您請教。
, G% J$ _3 L* V! Q2 P目前和宋修賢老師在處理Ardui Car
0 S6 n) x/ m0 {! P雖然已使用較繁雜的方式處理了跑出黑線外的狀況

2 ]4 r- G& }/ E1 U: d0 n" P, p但基於想追求更精簡的程式所以還是想請問一下
* P6 ?# ]! ?* O( x* c  X就是如果我只以單純寫PID控制時，常遇到CNY70跑出黑線外後便往前衝
不知道您是否願意教我可以如何處理
1 e6 M/ }+ K( r& z( p
- U) J1 W* F* b2 }( }4 U3 Q
% R/ }. Z9 n% v+ i! j+ C# p  _以下是我挑出我一般寫純PID控制的Code：

1. int MotoSpeed = 250;
   4 b: k) @1 @! p4 |
2. double CNY70Val = 1000;
   
3. int interror = 0;
   5 J' V5 ]$ D6 D9 ^1 I' c
4. int olderror = 0;
   
5. double values;
   * {7 E- O3 a) c: N' H  u$ a! f
6. 
   2 W) Z( v$ g- T5 p' M
7. 
   , B6 X" F  A' a
8. void CNY70()
   
9. {
   
10.   valuesRR = analogRead(RR)
    6 E! u  `- v! @8 z3 a
11.   valuesMR = analogRead(MR);
    
12.   valuesMM = analogRead(MM);
    
13.   valuesML = analogRead(ML);
    5 ^5 K+ _$ S& J- R4 [
14.   valuesLL = analogRead(LL);
    
15. 
    
16.   if (valuesRR > CNY70Val)
    
17.     valuesRR = CNY70Val;
    
18.   if (valuesMR > CNY70Val)
    
19.     valuesMR = CNY70Val;
      k6 o# u) z: R: n/ d. q3 F6 b
20.   if (valuesMM > CNY70Val)
    : a. F" Q) l2 ]7 _- N% D; o& W
21.     valuesMM = CNY70Val;
    
22.   if (valuesML > CNY70Val)
    ) z4 Q7 n5 I1 ^. m8 D
23.     valuesML = CNY70Val;
    9 s# e6 v- e7 `" Y! P" d4 f
24.   if (valuesLL > CNY70Val)
    2 M% |* m+ Q6 q1 ~# q0 L2 U
25.     valuesLL = CNY70Val;
    $ h7 g7 ]) X( k. [1 ]) h* {- Z
26. 
    5 \5 I9 |* r5 `4 n  K
27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    
28. }
    7 K6 u) L4 w4 }/ ^4 A7 p
29. 
    / f# G$ k9 w; S- Y1 w
30. void Car()
    
31. {
    $ B6 a" X& I- F7 ?
32.   while (1) {
    + F# c. H9 o( K! ^2 e9 m3 P6 E2 `
33.     CNY70();
    
34. 
    
35.     int error = ((int)values);
    
36.     interror += error;
    
37.     int lasterror = error - olderror;
    . C) {# U& w. ], l" a; `- n) P. _
38.     olderror = error;
    
39.     int power = error / 5 + interror / 10000 + lasterror;
    $ |* h3 z* g  F8 [) |
40. 
    / r% W6 Q% g3 n* Z4 z, C
41.     if (power > MotoSpeed)
    
42.       power = MotoSpeed;
    
43.     if (power < -MotoSpeed)
    
44.       power = -MotoSpeed;
    + |" B! i4 W) o$ Y  W
45. 
    
46.     if (power > 0)
    
47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作，正數正轉 負數反轉。
    
48.     else
    % h/ b; ^- W8 {* L5 m1 K
49.       Speed(MotoSpeed + power, MotoSpeed);
    ' f3 L% B/ y0 N" j  Z2 g, a
50.   }
    
51. }

複製代碼

! R0 W( }, A0 O9 t  n

感謝分享

感謝分享