PID 循跡自走車範例

1. #include <Arduino.h>
   
2. #include <Wire.h>
   9 b* n. _6 N5 V, W  b9 k0 W* E
3. #include <Servo.h>
   
4. 
   
5. #include "MePort.h"
   8 x- {) ?: y$ g# l" I4 \
6. #include "MeUltrasonic.h"
   9 s! U. y# _; X/ o
7. #include "MeDCMotor.h"
   8 d& D" C, W& x
8. 
   
9. //double Input, Output ;
   ! V! b# V2 S6 C2 L% S
10. float MaxSpeed = 255;
    
11. float MaxPower = 180;
    
12. float MinPower = 120;
    
13. float Error,ErrorAcc,ErrorDec;
    ( S: |( K7 X/ g8 L& b. W# v9 q7 b
14. 
    
15. float Kp=0.14;
    
16. float Kd=0.00020;//23;
    
17. float Ki=0.000201;
    ; T9 k$ _! i, O# e7 V# e  V) u
18. 
    
19. float nPower;
    
20. MePort lightsensor_6(6);
    
21. MePort lightsensor_8(8);
    " O+ f0 E; C+ l3 k9 I5 i
22. MeDCMotor motor_9(9);
    
23. MeDCMotor motor_10(10);
    9 [( H2 w/ g! B6 m" ], W
24. unsigned long previousMillis = 0;
    
25. const long interval = 1;
    + \2 D# X9 W5 I! D' p: }
26. 
    
27. void setup(){
    
28.     lightsensor_6.dWrite1(1);
    1 P& v4 E0 U- I* I
29.     nPower = 160;
    
30.     Error=0;
    
31.     ErrorAcc=0;
    
32. }
    8 N# T0 J9 w+ |! ]* K# Q& r$ J1 q. N
33. 
    
34. void loop(){
    
35.   unsigned long StartTime = millis();
      \. x" S0 e, p7 ]- j  z
36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    & e0 m0 K& T9 Z, z0 ]' k% A
37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    8 A# l5 w8 s( Y5 V* N
38.     ErrorAcc +=  nError*Kd ;
    
39.     ErrorDec -=  nError*Ki ;
    ! j: ?; Y6 R. _; \
40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    , g4 R: K; B% f& R' v2 Z1 A# U
41.     if(nError < 80 && nError > -80){
    0 e8 m% H- z: n2 M) V0 e4 Q5 a' P
42.       if(nPower < MaxPower){
    : j4 g8 _- {9 B# ^8 h5 }' w, h
43.         nPower += 3;
    , {1 t6 o( S. f4 ~" g; t
44.       }
    - h& a' \- D6 ~% `/ |# ?
45.     } else{
    9 l3 j: K+ i$ X% u
46.       if(nPower > MinPower){
    ! {' C3 V% O0 d- G( U( d9 k- k* a
47.         nPower -= 2;
    
48.       }
    7 |! G, e2 h; G0 W
49.     }
    & i6 A- E/ j% K- P6 Y
50.     MotoL(nPower-Error);
    2 M. s( ~* s# B* Y) J" ^# {
51.     MotoR(nPower+Error);   
    ) A  C9 a, x2 Z9 n( }& u  g
52.   }else{
    
53.     motor_9.run(0);
    
54.     motor_10.run(0);
    
55.   }
    
56.   do{}while(millis() - StartTime < interval);
    
57. }
    3 @% u' e* p+ i- b
58. 
    9 Q$ J/ q4 ~, F! W6 W
59. void MotoL(int Power){
    . t0 I/ `/ w0 T* V7 x5 m
60.   if (Power > MaxSpeed){
    / i; |+ A! v' o; ^
61.      Power = MaxSpeed;
    1 c; u  ?# p  W( Q* H- T; d
62.   }
    9 [1 m. r, M. E) N  [6 }$ y1 i4 l) r
63.   if (Power < -MaxSpeed){
    * C/ \- ^" u8 h# M8 L& G; f
64.      Power = -MaxSpeed;
    
65.   }
    
66.   motor_9.run(Power);
    
67. }  
    
68. 
    
69. void MotoR(int Power){
    
70.   if (Power > MaxSpeed){
    % \' C1 Z, o# {% {% V
71.      Power = MaxSpeed;
    " K# W. c+ o) D  w0 e
72.   }
    
73.   if (Power < -MaxSpeed){
    $ V% j% @4 h) t: @8 O7 x% L
74.      Power = -MaxSpeed;
    * @! f4 T; c2 `0 Q# K
75.   }
    * U5 X2 |+ R- R1 u
76.   motor_10.run(Power);
    
77. }  

複製代碼

3 y; u1 _2 [% B9 \' n2 D5 Q4 @- I" d

您好，不知是否能向您請教。
2 X8 w4 u; G) M) U- r目前和宋修賢老師在處理Ardui Car
雖然已使用較繁雜的方式處理了跑出黑線外的狀況
( a1 U; X7 I. Y+ m/ O
但基於想追求更精簡的程式所以還是想請問一下
就是如果我只以單純寫PID控制時，常遇到CNY70跑出黑線外後便往前衝
不知道您是否願意教我可以如何處理


: ]2 s; `" z, Z5 H+ L以下是我挑出我一般寫純PID控制的Code：

1. int MotoSpeed = 250;
   # w* D! L' K( m% g* t: q, a) l
2. double CNY70Val = 1000;
   # ?9 ~7 B4 t* G% T0 U# u7 M
3. int interror = 0;
   8 q/ A$ i1 O8 w/ `* }  E8 x) z" }
4. int olderror = 0;
   
5. double values;
   3 w1 I8 O8 g3 L6 Z, A
6. 
   
7. 
   
8. void CNY70()
   
9. {
   6 z# }9 D$ D! ?- Z/ b% M
10.   valuesRR = analogRead(RR)
    
11.   valuesMR = analogRead(MR);
    # k0 R  `5 e6 c: z
12.   valuesMM = analogRead(MM);
    
13.   valuesML = analogRead(ML);
    1 }* B4 y1 x+ `. u+ X5 P+ P
14.   valuesLL = analogRead(LL);
    : B' n: [7 I1 U% a
15. 
    
16.   if (valuesRR > CNY70Val)
    % _3 W$ |4 H3 N7 h; o+ x# ]
17.     valuesRR = CNY70Val;
    ( G0 D9 o2 S; G& R7 A
18.   if (valuesMR > CNY70Val)
    ' O- K' B) p2 |' w8 G+ i! V
19.     valuesMR = CNY70Val;
    + g( |' J+ E5 v5 w
20.   if (valuesMM > CNY70Val)
    8 v7 ~9 P3 ^. R) `0 i
21.     valuesMM = CNY70Val;
    
22.   if (valuesML > CNY70Val)
    $ }; K. P4 P9 P% r: M8 _$ F( N3 S
23.     valuesML = CNY70Val;
    4 j: ^+ H' q$ y% z# [  `6 _$ p
24.   if (valuesLL > CNY70Val)
    0 x. G: _6 ?, q8 E. x. e
25.     valuesLL = CNY70Val;
    8 l2 Y$ f9 Q. }) n1 g; ?
26. 
    
27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    
28. }
    
29. 
    
30. void Car()
    
31. {
    , P2 s( C& W6 P; A3 i
32.   while (1) {
    2 Q8 A: N+ ]' e; u6 \
33.     CNY70();
    & N5 r/ r- }$ C4 u" h, G8 U
34. 
    
35.     int error = ((int)values);
    
36.     interror += error;
    
37.     int lasterror = error - olderror;
    ! w1 Y# _# M5 \6 L1 G  ^, ^
38.     olderror = error;
    8 n) t+ G5 D" K
39.     int power = error / 5 + interror / 10000 + lasterror;
    
40. 
    
41.     if (power > MotoSpeed)
    
42.       power = MotoSpeed;
    * \$ h2 b: j. a4 F3 y8 U4 R/ h( L
43.     if (power < -MotoSpeed)
    ) r. e/ f6 ~" |; D: H0 [
44.       power = -MotoSpeed;
    
45. 
    
46.     if (power > 0)
    , M9 f% {2 s  F- V$ j& U" [6 i
47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作，正數正轉 負數反轉。
    
48.     else
    
49.       Speed(MotoSpeed + power, MotoSpeed);
    ; x2 C& W2 \) ~$ @7 g6 C
50.   }
    ! R' ?/ C- g2 g5 b
51. }

複製代碼

( H3 N0 q' h! `) L. K' I. N

感謝分享

感謝分享