PID 循跡自走車範例

1. #include <Arduino.h>
   
2. #include <Wire.h>
   ' S! J, t: `# I$ y. u1 c4 x
3. #include <Servo.h>
   
4. 
   / f3 E6 R- l: O# a- o
5. #include "MePort.h"
   
6. #include "MeUltrasonic.h"
   
7. #include "MeDCMotor.h"
   
8. 
   4 z$ n  Y; `3 ?6 C
9. //double Input, Output ;
   $ j7 x, t( E3 f% e
10. float MaxSpeed = 255;
    6 J3 k6 U$ r; C/ V
11. float MaxPower = 180;
    ) b  p+ X: f- J; k. c
12. float MinPower = 120;
    
13. float Error,ErrorAcc,ErrorDec;
    
14. 
    ; ^1 o$ {0 M1 W' q( I
15. float Kp=0.14;
    
16. float Kd=0.00020;//23;
    ( D& c% z5 b( I' ^
17. float Ki=0.000201;
    
18. 
    
19. float nPower;
    
20. MePort lightsensor_6(6);
    . m  N1 X' f: n  D2 G- b
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. 
    6 _9 }$ y1 e& K
27. void setup(){
    / N9 N4 _0 a9 v# L- q
28.     lightsensor_6.dWrite1(1);
    5 t3 S1 X( U, @7 E- j6 B/ r8 e4 R
29.     nPower = 160;
    
30.     Error=0;
    " B, x& K4 D) S3 P, S0 W
31.     ErrorAcc=0;
    
32. }
    
33. 
    $ |- ^9 L6 c) |' D% @! |
34. void loop(){
    ) f( p# U+ p* _  j" a: E
35.   unsigned long StartTime = millis();
    
36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    & q9 ]7 x8 w4 [' n: V2 {
37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    ) y/ k' V/ t3 a' u
38.     ErrorAcc +=  nError*Kd ;
    
39.     ErrorDec -=  nError*Ki ;
    
40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    ; t, B" m6 R) ^4 @* A
41.     if(nError < 80 && nError > -80){
    ! [7 R- L1 v/ a9 ^
42.       if(nPower < MaxPower){
    9 U  m) R, g0 G0 a1 ^4 Q' {
43.         nPower += 3;
    
44.       }
    * V0 k/ X, V( x( W% i2 k4 \2 |" b
45.     } else{
    - A- P1 W$ B* s" A
46.       if(nPower > MinPower){
    " C1 A) V2 h# l, [) Z' D- p& n
47.         nPower -= 2;
    ! J) D) a6 E8 y6 c4 X
48.       }
    
49.     }
    
50.     MotoL(nPower-Error);
    7 [* U1 n' R; l2 `( v
51.     MotoR(nPower+Error);   
    
52.   }else{
    
53.     motor_9.run(0);
    % c/ |4 n+ ]8 v; e$ ~
54.     motor_10.run(0);
    8 N: Y! y8 H  \. I( {, P/ i
55.   }
    9 _+ y- F5 M# V3 J
56.   do{}while(millis() - StartTime < interval);
    
57. }
    & c& t" l$ P& }5 k  P9 G
58. 
    & f- e$ y4 Q6 _* E( F- P/ R
59. void MotoL(int Power){
    3 M1 O' E8 _# X! h- g7 p
60.   if (Power > MaxSpeed){
    ) f* S. o# ?+ ^: Q2 G6 d! V+ f' P
61.      Power = MaxSpeed;
    / z6 ?* N- A4 D9 n( f4 o3 E: t% f
62.   }
    ) _9 Q! I0 n9 A
63.   if (Power < -MaxSpeed){
    & n  i8 k; d+ o$ p' o/ y
64.      Power = -MaxSpeed;
    3 z$ a8 O9 `1 y
65.   }
    5 m" |5 z; g5 }0 E
66.   motor_9.run(Power);
    ! b1 I4 q$ T# L5 m
67. }  
    * P* l" w  V* h7 J
68. 
    % c( X/ |/ g9 i' D/ E$ B6 l; t
69. void MotoR(int Power){
    
70.   if (Power > MaxSpeed){
    0 H8 j2 K9 S! R0 j4 T! V0 |
71.      Power = MaxSpeed;
    
72.   }
    7 r- y4 i/ Q3 O: H( X* w$ q% G
73.   if (Power < -MaxSpeed){
    
74.      Power = -MaxSpeed;
    
75.   }
    
76.   motor_10.run(Power);
    & q# [9 A2 t) X$ ?7 a2 I
77. }  

複製代碼

( U# p4 l, C4 M, Z您好，不知是否能向您請教。
! d2 c! J1 {9 ~, J8 k( Q目前和宋修賢老師在處理Ardui Car
) [) p. \3 W2 J% I雖然已使用較繁雜的方式處理了跑出黑線外的狀況
* S: l9 r$ v5 Z; _: ^8 u, I/ c
但基於想追求更精簡的程式所以還是想請問一下
就是如果我只以單純寫PID控制時，常遇到CNY70跑出黑線外後便往前衝
不知道您是否願意教我可以如何處理
  F+ i# G4 a- r5 J/ @4 o
) H* _; J& H' q
1 _) R) i: \& ]) m4 i以下是我挑出我一般寫純PID控制的Code：

1. int MotoSpeed = 250;
   5 L) s0 g7 t" K2 Q6 y$ |, e; c+ \; F
2. double CNY70Val = 1000;
   + n# i9 w. C3 q5 F) ~
3. int interror = 0;
   
4. int olderror = 0;
   ! \# Q3 C. ~4 ?" I- `
5. double values;
   
6. 
   . P+ u' h, O( t; r4 P( z- R
7. 
   
8. void CNY70()
   ! a- Y0 S3 d; m
9. {
   + Y8 m% ]9 Z# ~/ e* w4 y; ~8 G3 X3 e! j
10.   valuesRR = analogRead(RR)
    9 j* ^% _8 w" d1 n! F% e
11.   valuesMR = analogRead(MR);
    . j6 r" b- u$ R, m. k
12.   valuesMM = analogRead(MM);
    / r' d3 ?8 Q% I2 m! F; l6 \3 ~, m
13.   valuesML = analogRead(ML);
    5 T2 Q0 m0 o( N, J
14.   valuesLL = analogRead(LL);
    1 G. v! b# [3 S  D: V# t2 u) R
15. 
    
16.   if (valuesRR > CNY70Val)
    
17.     valuesRR = CNY70Val;
    
18.   if (valuesMR > CNY70Val)
    
19.     valuesMR = CNY70Val;
    * N. ^- k9 I) q& m
20.   if (valuesMM > CNY70Val)
    
21.     valuesMM = CNY70Val;
      [( P( e* b9 N: x
22.   if (valuesML > CNY70Val)
    
23.     valuesML = CNY70Val;
    
24.   if (valuesLL > CNY70Val)
    . h% ?- s# o9 o9 Q! N: G
25.     valuesLL = CNY70Val;
    4 P2 S5 t: D! d9 Y
26. 
    
27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    * Q" W" b2 Y8 R9 @! I
28. }
    7 p/ @& ^7 H' X/ m; E9 I& o
29. 
    
30. void Car()
    
31. {
    5 J  S$ Z$ l+ j4 n, T4 z# R
32.   while (1) {
    1 \) e6 ?7 i  `
33.     CNY70();
    
34. 
    
35.     int error = ((int)values);
    
36.     interror += error;
    4 E! d& w1 M1 f( A5 m% r% g8 r
37.     int lasterror = error - olderror;
    , j5 r) W% a- \1 Y! p
38.     olderror = error;
    ( q" K* P6 D- ~- c6 U
39.     int power = error / 5 + interror / 10000 + lasterror;
    ( i* }* V7 N; |) d+ {+ o' }2 M% s
40. 
    
41.     if (power > MotoSpeed)
    
42.       power = MotoSpeed;
    
43.     if (power < -MotoSpeed)
    5 Y3 B1 \) {5 u. b- m
44.       power = -MotoSpeed;
    
45. 
    
46.     if (power > 0)
    2 j2 e' j* ^/ S- L3 G
47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作，正數正轉 負數反轉。
    
48.     else
    
49.       Speed(MotoSpeed + power, MotoSpeed);
    
50.   }
    
51. }

複製代碼

感謝分享

感謝分享