PID 循跡自走車範例

1. #include <Arduino.h>
   ! i6 B: k3 O8 @
2. #include <Wire.h>
   / K- G1 d3 g4 r4 T  U
3. #include <Servo.h>
   
4. 
   3 S$ l' Q' P# b, L3 F' l; S
5. #include "MePort.h"
   * G* H7 Y- B- L+ @& _
6. #include "MeUltrasonic.h"
   - T' d! {9 R6 r0 d% E
7. #include "MeDCMotor.h"
   
8. 
   
9. //double Input, Output ;
   + h  V3 g+ R$ `
10. float MaxSpeed = 255;
    
11. float MaxPower = 180;
    
12. float MinPower = 120;
    , g& A; ~2 V1 V. ~6 U; L3 D
13. float Error,ErrorAcc,ErrorDec;
    ( _; o7 h7 |/ p9 u% p8 `
14. 
    ) @, ?& k5 M6 P7 |9 ]' u' G
15. float Kp=0.14;
    
16. float Kd=0.00020;//23;
    
17. float Ki=0.000201;
    / J) c$ C1 w$ Z' i7 G9 d; f3 \
18. 
    5 `. N3 X, t! B
19. float nPower;
    
20. MePort lightsensor_6(6);
    6 `: R3 r: f9 b6 V/ |. Q
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;
    
26. 
    
27. void setup(){
    
28.     lightsensor_6.dWrite1(1);
    : t) }! f. p* _% t
29.     nPower = 160;
    
30.     Error=0;
    
31.     ErrorAcc=0;
    & _6 L# U5 ~0 P. [/ r
32. }
    ( Z  G& P" S4 d) q
33. 
    
34. void loop(){
    
35.   unsigned long StartTime = millis();
    $ m8 f; E3 a5 u( ^! Z
36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    * D- u( u7 e/ {& n4 v/ C  z
37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    9 u0 l9 z$ Y1 o( B. e0 o  b1 U
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;
    
44.       }
    
45.     } else{
    ( E, e8 D" J  Q6 \' Q  J, {: `
46.       if(nPower > MinPower){
    
47.         nPower -= 2;
    
48.       }
    , }2 Z) m9 `# r" z
49.     }
    ; v0 {/ E" ~% `7 L! T
50.     MotoL(nPower-Error);
    
51.     MotoR(nPower+Error);   
    3 [; K" k3 N/ X' O9 {  w
52.   }else{
    
53.     motor_9.run(0);
    
54.     motor_10.run(0);
    8 Q" C. x: L  s; g. y6 o- N3 a
55.   }
    / h( ^' x2 h; f. `+ l2 }
56.   do{}while(millis() - StartTime < interval);
    1 F- g  q  C* O* [
57. }
    
58. 
    
59. void MotoL(int Power){
    
60.   if (Power > MaxSpeed){
    " Z- y& }6 w( p' O" T7 U
61.      Power = MaxSpeed;
    ) o" V3 X5 h$ \( k8 ?8 G+ ~
62.   }
    
63.   if (Power < -MaxSpeed){
    ) f& f% A6 }$ u- T, f+ M3 ?
64.      Power = -MaxSpeed;
    
65.   }
    % S+ B9 N# J5 C: l9 y! }) u
66.   motor_9.run(Power);
    
67. }  
    * D( C1 b6 s( k; I
68. 
    ( C) W2 @9 h1 p$ Z: [/ I- l" e6 H9 c
69. void MotoR(int Power){
    6 }- g" l+ d9 p" a3 N9 @5 R( t
70.   if (Power > MaxSpeed){
    
71.      Power = MaxSpeed;
    0 q; |3 r1 v5 I0 M* Z
72.   }
    5 k4 P2 J7 P: ~2 ?1 d
73.   if (Power < -MaxSpeed){
    8 v! V; y1 c, F2 E
74.      Power = -MaxSpeed;
    # p( _" k. Q* h. k. z- D8 e* B
75.   }
    / e% }# v' L1 S% A% M6 M
76.   motor_10.run(Power);
    
77. }  

複製代碼

/ |% {* t3 S' _& i5 o

+ O/ z5 E4 M' k1 d& Y* c
. t3 H8 j+ C" W! @0 Y  G您好，不知是否能向您請教。
: W+ G0 i# {. y/ h1 P- ^* d目前和宋修賢老師在處理Ardui Car
雖然已使用較繁雜的方式處理了跑出黑線外的狀況

$ u, x& q9 w* h/ X  X1 }& X# }  }' q但基於想追求更精簡的程式所以還是想請問一下
$ C) @. P7 z5 x' B4 C% g% j就是如果我只以單純寫PID控制時，常遇到CNY70跑出黑線外後便往前衝
不知道您是否願意教我可以如何處理
8 d$ L. u- T3 d- X# _1 C5 [/ y
- @1 O6 D% U& _, a( N2 h( s& r2 {
5 p( M! a8 g; ^. M( ]5 ^7 {以下是我挑出我一般寫純PID控制的Code：

1. int MotoSpeed = 250;
   
2. double CNY70Val = 1000;
   # h( S. V. r8 Y! w4 h' ]
3. int interror = 0;
   
4. int olderror = 0;
   ; v, W8 Y5 Y: t, v# ~2 Y
5. double values;
   / {3 P9 ?+ m" E% R  ]
6. 
   " B9 N% e" r+ @: G+ s6 ^" _
7. 
   ) ~) U$ U8 P# v
8. void CNY70()
   7 Y& t( o" R: l/ u* u" m  g
9. {
   
10.   valuesRR = analogRead(RR)
    $ m& X! e# b0 G! W- O
11.   valuesMR = analogRead(MR);
    
12.   valuesMM = analogRead(MM);
    
13.   valuesML = analogRead(ML);
    + \: l) n+ i. C. s' ~; a
14.   valuesLL = analogRead(LL);
    
15. 
    5 t7 u9 \7 g1 U% s2 J
16.   if (valuesRR > CNY70Val)
    
17.     valuesRR = CNY70Val;
    & x$ y6 h% Q2 K  S# z2 W% A1 Z
18.   if (valuesMR > CNY70Val)
    $ z# {6 f( d. T8 b  G' p2 N0 c
19.     valuesMR = CNY70Val;
    
20.   if (valuesMM > CNY70Val)
    7 U( L- e5 ^" E7 P3 j5 U1 G
21.     valuesMM = CNY70Val;
    ' Q- {4 x7 C% J  Y5 _. d" N9 _
22.   if (valuesML > CNY70Val)
    
23.     valuesML = CNY70Val;
    . e/ u6 ?5 ^# B% j/ p3 ]/ {& b
24.   if (valuesLL > CNY70Val)
    
25.     valuesLL = CNY70Val;
    
26. 
    
27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    + i' f/ Y# k, q7 C8 s3 `8 }
28. }
    6 ]* \9 x/ b# Z- d- K
29. 
    9 ?3 r' v. U, T( i# p
30. void Car()
      U& p% ]' a  G8 Q. l6 C
31. {
    
32.   while (1) {
    
33.     CNY70();
    . O$ ^- e5 v$ N7 B, T9 F* x
34. 
    ( N7 X' S+ u4 I: {; X# B! V, l
35.     int error = ((int)values);
    
36.     interror += error;
    
37.     int lasterror = error - olderror;
    . {2 {( B# v( e
38.     olderror = error;
    
39.     int power = error / 5 + interror / 10000 + lasterror;
    
40. 
    
41.     if (power > MotoSpeed)
    
42.       power = MotoSpeed;
    
43.     if (power < -MotoSpeed)
    # {& g# a, |9 q) B- z" u
44.       power = -MotoSpeed;
    
45. 
    
46.     if (power > 0)
    
47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作，正數正轉 負數反轉。
    * ?, N# T6 m  f5 ]/ W. M( N
48.     else
    
49.       Speed(MotoSpeed + power, MotoSpeed);
    
50.   }
    . t5 v+ q* R0 N7 F
51. }

複製代碼

感謝分享

感謝分享