PID 循跡自走車範例

1. #include <Arduino.h>
   & G  T3 u9 A& [1 x) l
2. #include <Wire.h>
   ' e% c) G# m' ?$ n- ?7 U' q
3. #include <Servo.h>
   
4. 
   
5. #include "MePort.h"
   * |! ]! Q( m$ F: }' y
6. #include "MeUltrasonic.h"
   8 {+ G- Y" o) {
7. #include "MeDCMotor.h"
   * h5 }+ a% S) }% f
8. 
   
9. //double Input, Output ;
   # r1 w" w3 Z9 p, L3 |& ]
10. float MaxSpeed = 255;
    1 U2 p! t; p3 s8 r! j& e
11. float MaxPower = 180;
    
12. float MinPower = 120;
    
13. float Error,ErrorAcc,ErrorDec;
    4 ~* @% ?8 b5 N
14. 
    # D/ L# m. @! t
15. float Kp=0.14;
    
16. float Kd=0.00020;//23;
    
17. float Ki=0.000201;
    
18. 
    4 d6 q" Q% S8 z3 u
19. float nPower;
    
20. MePort lightsensor_6(6);
    
21. MePort lightsensor_8(8);
    * ?  E7 o+ \7 k5 X
22. MeDCMotor motor_9(9);
    + d$ V+ C" t* U3 R$ a" `
23. MeDCMotor motor_10(10);
    
24. unsigned long previousMillis = 0;
    
25. const long interval = 1;
    ; ]' ~; Q) p! `, q
26. 
    
27. void setup(){
    
28.     lightsensor_6.dWrite1(1);
    $ o8 d5 M! k5 e( h, s& Y5 O
29.     nPower = 160;
    
30.     Error=0;
    ( e: a8 z7 Y4 h/ Z+ }4 v5 c
31.     ErrorAcc=0;
    6 _4 M, z8 H; B. Q
32. }
    7 w2 Y9 ~3 d+ E1 r6 s, b5 h
33. 
    
34. void loop(){
    
35.   unsigned long StartTime = millis();
    
36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    . o# h& T# j* X) I  r* e! D% o
37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    + x4 D7 E& z; D( ^) ~7 `3 }
38.     ErrorAcc +=  nError*Kd ;
    3 V$ r" ~1 J1 c2 y
39.     ErrorDec -=  nError*Ki ;
    ( M5 s% H+ g) K. Y& B
40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    
41.     if(nError < 80 && nError > -80){
    ! X( J) ~; B" V% |4 C; h
42.       if(nPower < MaxPower){
    1 A7 _5 q! Z2 A! M! F& Q
43.         nPower += 3;
    
44.       }
    % D9 I6 V( L4 B9 c1 o
45.     } else{
    0 Y) z$ O! y/ b6 h2 B
46.       if(nPower > MinPower){
    + b0 \2 Z( ?7 R! `1 {' G0 a- t
47.         nPower -= 2;
    
48.       }
    
49.     }
    
50.     MotoL(nPower-Error);
    8 e( p: W$ n: X9 a5 m( Q5 g$ [
51.     MotoR(nPower+Error);   
    7 j7 D2 X" N$ J. p$ V0 M
52.   }else{
    
53.     motor_9.run(0);
    
54.     motor_10.run(0);
    " U3 G" N1 f; Z5 O8 y; _( z' \
55.   }
    
56.   do{}while(millis() - StartTime < interval);
    
57. }
    
58. 
    ; f5 x! n8 F+ Z8 O
59. void MotoL(int Power){
      S# b$ _/ R( M! l* _/ e
60.   if (Power > MaxSpeed){
    
61.      Power = MaxSpeed;
    1 ?8 }7 _7 v- M$ e
62.   }
    
63.   if (Power < -MaxSpeed){
    
64.      Power = -MaxSpeed;
    , c; j. n- `) }+ Q$ U# P, ~
65.   }
    1 f# H! A8 y9 H! P' Q
66.   motor_9.run(Power);
    7 t5 W& @- E# A3 `9 y0 v
67. }  
    " m* p. p3 m1 l
68. 
    
69. void MotoR(int Power){
    
70.   if (Power > MaxSpeed){
    
71.      Power = MaxSpeed;
    
72.   }
    % V7 I& ]# p7 M% X* G' m: `
73.   if (Power < -MaxSpeed){
    
74.      Power = -MaxSpeed;
    
75.   }
    7 e8 V4 M2 g; G8 B/ `( F' x
76.   motor_10.run(Power);
    2 t% L$ z* \/ x" p) v
77. }  

複製代碼

# m+ Y+ u4 Q9 o+ G, ]5 r
您好，不知是否能向您請教。
目前和宋修賢老師在處理Ardui Car
2 Y) ~# J. H7 x5 \# t) L$ G$ H0 Y雖然已使用較繁雜的方式處理了跑出黑線外的狀況

但基於想追求更精簡的程式所以還是想請問一下
就是如果我只以單純寫PID控制時，常遇到CNY70跑出黑線外後便往前衝
不知道您是否願意教我可以如何處理
" v6 h8 j, h1 q* \* z
/ Q: w9 y8 {9 [+ {
以下是我挑出我一般寫純PID控制的Code：

1. int MotoSpeed = 250;
   
2. double CNY70Val = 1000;
   
3. int interror = 0;
   , @' R( o" B% e6 Y$ N- w0 i8 r
4. int olderror = 0;
   
5. double values;
   
6. 
   7 m' v: P, v2 j3 l) c& z
7. 
   
8. void CNY70()
   9 K1 q2 T5 r. [9 H7 F
9. {
   4 u) S& {& _# ~& R- K
10.   valuesRR = analogRead(RR)
    , t1 _. \5 M6 {) M) I8 E: [) ~
11.   valuesMR = analogRead(MR);
    
12.   valuesMM = analogRead(MM);
    
13.   valuesML = analogRead(ML);
    
14.   valuesLL = analogRead(LL);
    
15. 
    
16.   if (valuesRR > CNY70Val)
    0 ^1 {# w; V8 s" L1 x5 @7 Z, A
17.     valuesRR = CNY70Val;
    
18.   if (valuesMR > CNY70Val)
    ' n( D4 }) k: O
19.     valuesMR = CNY70Val;
    
20.   if (valuesMM > CNY70Val)
    
21.     valuesMM = CNY70Val;
    , D! ~% R5 r6 J) m
22.   if (valuesML > CNY70Val)
    ; E: C  W/ O/ x9 s9 M7 X0 g
23.     valuesML = CNY70Val;
    # R: a7 |& \0 ~; v! C
24.   if (valuesLL > CNY70Val)
    ) O8 v6 v7 ]2 d  y, ?) k% E
25.     valuesLL = CNY70Val;
    1 \* W. `4 L6 M& P( T
26. 
    
27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    8 l$ N$ m. H; D- C0 S: @
28. }
    4 t- O" b) N  Y- Q
29. 
    . l$ N* G. N& o1 R: V
30. void Car()
    
31. {
    / ]# F, h4 z& h6 E7 H! T3 P
32.   while (1) {
    4 S" t& _7 ^( Y/ y! N/ R
33.     CNY70();
    
34. 
    
35.     int error = ((int)values);
    
36.     interror += error;
    
37.     int lasterror = error - olderror;
    
38.     olderror = error;
    & V4 K0 x2 ]6 \% w+ n! i: |1 H$ }
39.     int power = error / 5 + interror / 10000 + lasterror;
    
40. 
    $ _4 m: }- R+ }2 _+ o
41.     if (power > MotoSpeed)
    
42.       power = MotoSpeed;
    1 L& f1 r* M1 l1 \% s
43.     if (power < -MotoSpeed)
    5 O: s/ w! L+ H  t6 E8 c
44.       power = -MotoSpeed;
    & d2 T4 s" r+ r+ H3 B; _
45. 
    5 k" P! [6 R+ ?( q
46.     if (power > 0)
    7 u2 m4 J1 W, P+ P! R8 r
47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作，正數正轉 負數反轉。
    
48.     else
    - y* p; T) X0 {1 @4 P
49.       Speed(MotoSpeed + power, MotoSpeed);
      C: p% }/ S) Y
50.   }
    
51. }

複製代碼

感謝分享

感謝分享