PID 循跡自走車範例

1. #include <Arduino.h>
   
2. #include <Wire.h>
   
3. #include <Servo.h>
   
4. 
   7 m" }- i4 t) z7 c
5. #include "MePort.h"
     T. G/ H2 Y9 z8 N) S# c# L' H
6. #include "MeUltrasonic.h"
   
7. #include "MeDCMotor.h"
   
8. 
   / K/ ~5 t$ ~: Q* F6 O
9. //double Input, Output ;
   3 p, T( E: }, w/ i- v
10. float MaxSpeed = 255;
    $ {& U' b  H& ?# P$ n2 t
11. float MaxPower = 180;
    0 s, v0 O, `% V9 Y8 b
12. float MinPower = 120;
    
13. float Error,ErrorAcc,ErrorDec;
    5 j& `' [& p5 @4 G  y
14. 
    
15. float Kp=0.14;
    6 s6 D6 h7 S+ ?, l
16. float Kd=0.00020;//23;
    % d1 O' Z3 o' i8 F. w* X" c' v
17. float Ki=0.000201;
    $ n9 N8 k$ ?+ P- \( y
18. 
    * t0 ^" J* R, _/ W* V5 _+ L: i4 I, M
19. float nPower;
    
20. MePort lightsensor_6(6);
    ! S3 D% z4 u, M" f( P- C
21. MePort lightsensor_8(8);
    
22. MeDCMotor motor_9(9);
    
23. MeDCMotor motor_10(10);
    
24. unsigned long previousMillis = 0;
    ( g9 f1 g5 g" c2 s  O
25. const long interval = 1;
    % {9 y/ _' _" Z3 B! S- n2 U; Z
26. 
    
27. void setup(){
    # V  p7 [5 r% d; e! @' F# w
28.     lightsensor_6.dWrite1(1);
    + q, [: A3 t- c) K- @* h
29.     nPower = 160;
    
30.     Error=0;
    
31.     ErrorAcc=0;
    
32. }
    
33. 
    : h! j0 Y1 e3 t5 ?9 N9 Y) F
34. void loop(){
    
35.   unsigned long StartTime = millis();
    
36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
      n6 e9 Z& [1 b2 o; R, C4 c
37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    $ w9 Z/ Z. w# D7 w0 I1 Q% z
38.     ErrorAcc +=  nError*Kd ;
    : }' M+ v3 x3 E, l
39.     ErrorDec -=  nError*Ki ;
    & P( ?: O6 N' ~( {$ Z
40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    3 s9 A# B9 j$ c
41.     if(nError < 80 && nError > -80){
    
42.       if(nPower < MaxPower){
    & t4 g' w9 ^, i& A' P4 H
43.         nPower += 3;
    
44.       }
    % [0 M6 q2 P3 u2 @6 M9 r9 B! E
45.     } else{
    5 s* n) [# q, e& F% j
46.       if(nPower > MinPower){
    , J/ [% }4 w: k( Y7 T
47.         nPower -= 2;
    7 D, J, x3 w* i$ k9 e0 G1 w4 o5 X& D
48.       }
    
49.     }
    
50.     MotoL(nPower-Error);
    
51.     MotoR(nPower+Error);   
    
52.   }else{
    
53.     motor_9.run(0);
    - j  U: t% J3 d; B8 @6 U
54.     motor_10.run(0);
    
55.   }
    
56.   do{}while(millis() - StartTime < interval);
    
57. }
    
58. 
    
59. void MotoL(int Power){
    - N3 ]3 z" I2 V; B% q+ y. d
60.   if (Power > MaxSpeed){
    ; I+ f6 v9 Q* y
61.      Power = MaxSpeed;
    * N' _% K4 _2 i; ^
62.   }
    
63.   if (Power < -MaxSpeed){
    
64.      Power = -MaxSpeed;
    
65.   }
    
66.   motor_9.run(Power);
    7 x5 J& g0 W8 B  F- i: N% o
67. }  
    
68. 
    / y* N3 I" J7 i/ ~+ i2 h( c
69. void MotoR(int Power){
    3 `' u) q& Y8 Z# B+ [
70.   if (Power > MaxSpeed){
    9 v1 q/ J* G, S- W
71.      Power = MaxSpeed;
    ' `0 b$ j+ Q0 w; }0 Q
72.   }
    
73.   if (Power < -MaxSpeed){
    
74.      Power = -MaxSpeed;
    $ v4 s, y& J4 T6 ]4 I
75.   }
    % w! T* Z# w/ y, c* B) n* x0 [1 ~
76.   motor_10.run(Power);
      H* W- l. v2 q
77. }  

複製代碼

2 J, q) A7 t; o- d- d+ ?: _

4 U: N* @; d5 c( |
1 w3 k# p- T: [4 K3 H2 j$ w' h2 ]; ?/ t您好，不知是否能向您請教。
目前和宋修賢老師在處理Ardui Car
雖然已使用較繁雜的方式處理了跑出黑線外的狀況
% U- ]' w0 f$ z; g0 ^
但基於想追求更精簡的程式所以還是想請問一下
就是如果我只以單純寫PID控制時，常遇到CNY70跑出黑線外後便往前衝
3 D; x  \$ C/ o5 r& ]6 I8 m不知道您是否願意教我可以如何處理
5 F% R* A- i) V7 t7 B0 j, g8 A
: |, p& Y' x. \7 f
以下是我挑出我一般寫純PID控制的Code：

1. int MotoSpeed = 250;
   4 ^) F7 K4 c. D- {) ~' P4 {8 N
2. double CNY70Val = 1000;
   $ l/ |9 F# R3 ]; Q  k: O
3. int interror = 0;
   
4. int olderror = 0;
   
5. double values;
   1 m% u5 J4 u8 I- H4 L# P
6. 
   . {+ x2 m, ?' Q5 I
7. 
   
8. void CNY70()
   ( _" r2 g+ \" K8 H) w
9. {
   
10.   valuesRR = analogRead(RR)
    
11.   valuesMR = analogRead(MR);
    
12.   valuesMM = analogRead(MM);
    
13.   valuesML = analogRead(ML);
    
14.   valuesLL = analogRead(LL);
    ; F/ n) u/ z( S1 a
15. 
    ' U" G, s6 T: M
16.   if (valuesRR > CNY70Val)
    
17.     valuesRR = CNY70Val;
    
18.   if (valuesMR > CNY70Val)
    
19.     valuesMR = CNY70Val;
    2 \7 N+ U7 N1 f. s
20.   if (valuesMM > CNY70Val)
    " b9 m  p) m1 E0 [/ b
21.     valuesMM = CNY70Val;
    - Y( O( \* j& J
22.   if (valuesML > CNY70Val)
    8 _* r: U* L( e1 e8 W( u, ?
23.     valuesML = CNY70Val;
    # U7 _$ S( }* a& K6 |2 a1 T
24.   if (valuesLL > CNY70Val)
    
25.     valuesLL = CNY70Val;
    ' p* \+ D' r1 a
26. 
    1 Z  B) `' z2 \! u( P6 N
27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    ! f  G/ Z/ q  g9 w
28. }
    
29. 
    . O0 C4 x+ V7 f; O0 m4 S/ a7 i! N
30. void Car()
    
31. {
    ) x& z. s2 r" A  \
32.   while (1) {
    
33.     CNY70();
    2 B5 W% A1 S4 i: U' C8 Z6 ^- a' P
34. 
      _, E! P0 }" Q- W& E( \
35.     int error = ((int)values);
    % f; X  [+ p- O
36.     interror += error;
    ( }& n! t1 z1 F7 \( `$ w/ N: t
37.     int lasterror = error - olderror;
    
38.     olderror = error;
    
39.     int power = error / 5 + interror / 10000 + lasterror;
    
40. 
    
41.     if (power > MotoSpeed)
    
42.       power = MotoSpeed;
    
43.     if (power < -MotoSpeed)
    , o! B$ ^0 P& w( p: B
44.       power = -MotoSpeed;
    
45. 
    ; @. P0 d* f( p# U: t: ?, c
46.     if (power > 0)
    2 y( `1 U8 [% Q4 q# k8 G2 x
47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作，正數正轉 負數反轉。
    
48.     else
    " j; `" I1 ?8 R, ^( T0 D) B
49.       Speed(MotoSpeed + power, MotoSpeed);
    & S) e; D# ^) M: X& e
50.   }
    
51. }

複製代碼

感謝分享

感謝分享