PID 循跡自走車範例

1. #include <Arduino.h>
   
2. #include <Wire.h>
   ) J7 F, @$ s- H) B) g/ w
3. #include <Servo.h>
   0 c0 u( E: ^( ~0 W! e. q
4. 
   
5. #include "MePort.h"
   " q5 x8 x5 M! W- S
6. #include "MeUltrasonic.h"
   
7. #include "MeDCMotor.h"
   + t: H8 f! H: @' M* ]
8. 
   & J2 I1 r6 s% ~) M- e
9. //double Input, Output ;
   
10. float MaxSpeed = 255;
    
11. float MaxPower = 180;
    
12. float MinPower = 120;
    
13. float Error,ErrorAcc,ErrorDec;
    ( O5 m6 ~' n5 }. {9 N1 b/ M% @' F
14. 
    
15. float Kp=0.14;
    
16. float Kd=0.00020;//23;
    9 p/ ^4 f( ]  Z, B+ y0 L
17. float Ki=0.000201;
    
18. 
    
19. float nPower;
    5 h3 n9 l& O/ {) {
20. MePort lightsensor_6(6);
    ) ^, ]' H& ~( f; u; T' V2 f3 s
21. MePort lightsensor_8(8);
    
22. MeDCMotor motor_9(9);
    " b4 z7 e/ y$ S- J) G% Q" f
23. MeDCMotor motor_10(10);
    7 X1 g1 e# F: X' S' m
24. unsigned long previousMillis = 0;
    
25. const long interval = 1;
    ( ^2 f4 B; w1 T/ i6 u' J
26. 
    
27. void setup(){
    3 n5 i5 c7 H. l; X: {5 v; w
28.     lightsensor_6.dWrite1(1);
    : M  x8 J" Z( U, E. V$ J
29.     nPower = 160;
    8 N* X  U9 G# I3 p' H
30.     Error=0;
    3 @4 X4 X( ~$ F; }, f9 ^; n( Y6 R
31.     ErrorAcc=0;
    % H3 y4 U6 s4 `4 _
32. }
    , A' H. l/ ]6 b5 J  T2 z5 s0 L
33. 
    
34. void loop(){
    
35.   unsigned long StartTime = millis();
    ( ~  F# q7 c; f
36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    
37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    
38.     ErrorAcc +=  nError*Kd ;
    
39.     ErrorDec -=  nError*Ki ;
    
40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    4 p0 l: v5 A0 I0 [6 }7 n8 o4 a) `+ _
41.     if(nError < 80 && nError > -80){
    
42.       if(nPower < MaxPower){
    3 x  Z9 b% R1 J% ~' X
43.         nPower += 3;
    
44.       }
    2 L7 \. K- Z7 V8 ]! k7 c" h: x% _
45.     } else{
    
46.       if(nPower > MinPower){
    
47.         nPower -= 2;
    
48.       }
    
49.     }
    ! b% p' t, Z. x% L, L( Y+ X+ m
50.     MotoL(nPower-Error);
    % }, @+ Z) J. q) B
51.     MotoR(nPower+Error);   
    
52.   }else{
    
53.     motor_9.run(0);
    
54.     motor_10.run(0);
    3 v! r3 @) U' O. H
55.   }
    ; L  [4 v' d$ G( Y6 L% m- O
56.   do{}while(millis() - StartTime < interval);
    
57. }
    
58. 
    % ~) P1 o( z9 D& ?  \
59. void MotoL(int Power){
    
60.   if (Power > MaxSpeed){
    
61.      Power = MaxSpeed;
    
62.   }
    
63.   if (Power < -MaxSpeed){
    
64.      Power = -MaxSpeed;
    
65.   }
    * Z4 s( H+ S" V9 g! }' T6 k: P# k5 u- X
66.   motor_9.run(Power);
    
67. }  
    , I& I  T) L1 B9 [( `$ `* z( n. D
68. 
    
69. void MotoR(int Power){
    
70.   if (Power > MaxSpeed){
    
71.      Power = MaxSpeed;
    9 Q  v7 Y2 I6 R7 Y7 e' k" B3 r' R) _0 L
72.   }
    - l$ m! ]) ^# H) c; Q% r
73.   if (Power < -MaxSpeed){
    
74.      Power = -MaxSpeed;
    
75.   }
    3 r& M; I% m% ]* C% Q, S0 r$ G5 G
76.   motor_10.run(Power);
    
77. }  

複製代碼

6 f! r, H/ K; R& H; K

您好，不知是否能向您請教。
目前和宋修賢老師在處理Ardui Car
' x6 e; M1 g2 v  v% y$ n雖然已使用較繁雜的方式處理了跑出黑線外的狀況
6 V. }, N" H) K4 A( I# \$ b
! _' s, p1 R" E" `但基於想追求更精簡的程式所以還是想請問一下
9 ]6 z- X* S4 o2 s- k3 C  k+ {' |就是如果我只以單純寫PID控制時，常遇到CNY70跑出黑線外後便往前衝
不知道您是否願意教我可以如何處理
4 p) K# F9 T- x, O, F

以下是我挑出我一般寫純PID控制的Code：

1. int MotoSpeed = 250;
   
2. double CNY70Val = 1000;
   ; b8 B1 d0 w7 O" {+ {( t
3. int interror = 0;
   
4. int olderror = 0;
   ! Z! E& I0 v+ T
5. double values;
   
6. 
   1 |1 I2 f& ~+ P2 e" J+ {# T0 b6 V
7. 
   2 p% F) n9 S! s4 A6 M) V% ^
8. void CNY70()
   
9. {
   9 i, g) s4 W8 s* W
10.   valuesRR = analogRead(RR)
    3 p+ w7 Z8 X/ {: V3 i
11.   valuesMR = analogRead(MR);
    1 D- ]8 O: l" ]
12.   valuesMM = analogRead(MM);
    . P4 Z1 u' H" R7 w- K/ ?( d7 E: e
13.   valuesML = analogRead(ML);
    
14.   valuesLL = analogRead(LL);
    1 k& f8 H) \$ n) @/ P  y% f
15. 
    ' t: P4 G0 n+ d  G* U8 `: P, R
16.   if (valuesRR > CNY70Val)
    
17.     valuesRR = CNY70Val;
    
18.   if (valuesMR > CNY70Val)
    " N7 a( e/ B6 A$ D" C$ p
19.     valuesMR = CNY70Val;
    % V* Y" _$ t6 d: s
20.   if (valuesMM > CNY70Val)
    $ ^4 c% V' M* G3 j; U; S1 Z
21.     valuesMM = CNY70Val;
    # _, T0 s, f+ k- c$ R. Q" B
22.   if (valuesML > CNY70Val)
    0 Y/ C- I# Q; l7 o* ]
23.     valuesML = CNY70Val;
    # I2 L+ r) ^9 d* a3 R+ A6 C5 e. M6 t
24.   if (valuesLL > CNY70Val)
    3 ~5 \( W9 ]" @/ v) G
25.     valuesLL = CNY70Val;
    
26. 
    
27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    
28. }
    - f: i1 T( p# w1 O' u7 ]
29. 
    
30. void Car()
    # t* N4 I- t' Q* c* n: I% `6 n
31. {
    
32.   while (1) {
    1 }# }3 R4 z# |4 n2 n* ]9 X
33.     CNY70();
    8 A  K! w5 r" T. Z+ t: T
34. 
    
35.     int error = ((int)values);
    
36.     interror += error;
    ) j; {' @# b1 \5 g7 _3 }
37.     int lasterror = error - olderror;
    
38.     olderror = error;
    5 h: \* [7 ?. S! P0 i+ Q/ f
39.     int power = error / 5 + interror / 10000 + lasterror;
    
40. 
    / c+ a+ t5 [; {- i
41.     if (power > MotoSpeed)
    
42.       power = MotoSpeed;
    3 A3 }% {6 h9 R, y; K" j$ }
43.     if (power < -MotoSpeed)
    ' i$ k/ V# F' V' s$ |( C1 O
44.       power = -MotoSpeed;
    ( r/ R3 @7 c$ g* Y
45. 
    
46.     if (power > 0)
    
47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作，正數正轉 負數反轉。
    
48.     else
    2 o/ }: }, M# q+ I3 K5 [: ]* y
49.       Speed(MotoSpeed + power, MotoSpeed);
    ; L0 I0 F+ T- V4 K; }" D1 K
50.   }
    - R( W+ |) B- w! x  J6 ?7 Q! l' G) ^6 R
51. }

複製代碼

/ o7 ?- F/ a2 }6 n' y" Q

感謝分享

感謝分享