PID 循跡自走車範例

1. #include <Arduino.h>
   
2. #include <Wire.h>
   
3. #include <Servo.h>
   
4. 
   ! E+ ?! T( z! z' ^
5. #include "MePort.h"
   
6. #include "MeUltrasonic.h"
     \. _: f2 M( U' s% a/ I& x% `9 L5 K
7. #include "MeDCMotor.h"
   $ n/ |7 i8 x* V$ o5 ^# U
8. 
   1 R# {7 a9 v' Z7 T; S' {- L% }1 v
9. //double Input, Output ;
   4 M9 }  V* w: @8 G: V: {; z
10. float MaxSpeed = 255;
    * g- E5 H1 @6 h: C0 L  z0 n' m
11. float MaxPower = 180;
    
12. float MinPower = 120;
    ) D- @. ~/ e2 g- y4 j& B
13. float Error,ErrorAcc,ErrorDec;
    
14. 
    % A( [  n7 E$ o, k1 _+ a
15. float Kp=0.14;
    4 k  {. n& @; D* ?: J' x
16. float Kd=0.00020;//23;
    6 M% C. m/ N6 Q8 w) x) B
17. float Ki=0.000201;
    * L, a" O. u& L4 x7 r; E5 X
18. 
    2 S  |- O, `; Y' E$ J1 S. x
19. float nPower;
    
20. MePort lightsensor_6(6);
    1 \- Z! D1 V4 A& o
21. MePort lightsensor_8(8);
    . ^- s1 y- ?* \$ n# v
22. MeDCMotor motor_9(9);
    * P$ Q5 d; M% r) r6 p
23. MeDCMotor motor_10(10);
    $ Y1 y) P# W7 m: p0 [4 w! {# d& e
24. unsigned long previousMillis = 0;
    
25. const long interval = 1;
    $ q% D+ r  Y1 q$ P4 G0 P
26. 
    7 u% M( g, D/ j7 I
27. void setup(){
    7 Q# P. W6 W; Y$ [$ ]
28.     lightsensor_6.dWrite1(1);
    9 [4 ^/ F1 v  F2 I& }2 o
29.     nPower = 160;
    9 ?" w" f: S* ?$ @9 k* }1 j8 [6 e
30.     Error=0;
    # q5 r2 o6 `; y  r2 B) U5 G
31.     ErrorAcc=0;
    
32. }
    
33. 
    + ]' @, R5 ?3 Y8 ?; p. V  v  o7 M
34. void loop(){
    
35.   unsigned long StartTime = millis();
    $ M- Z0 u. X2 g; I# ^+ N- E( R
36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    . _$ I0 B1 i* c9 d
37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    
38.     ErrorAcc +=  nError*Kd ;
    
39.     ErrorDec -=  nError*Ki ;
    * N6 O( x. B' b0 s
40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    
41.     if(nError < 80 && nError > -80){
    6 i2 j! `% r4 r; _0 O
42.       if(nPower < MaxPower){
    $ {9 r/ M/ Q" M2 q- o) l" Z
43.         nPower += 3;
    
44.       }
    
45.     } else{
    ( r+ t) O% w( H* m  o" S9 ^7 ^* P1 P
46.       if(nPower > MinPower){
    
47.         nPower -= 2;
    
48.       }
    
49.     }
    
50.     MotoL(nPower-Error);
    ( h3 `9 q% x( w8 u8 ]+ Q& p5 ?
51.     MotoR(nPower+Error);   
    ( N: v0 i2 U$ n' m/ @/ u1 ~- V
52.   }else{
    # [% B% S7 Y  J, q/ W/ @! W9 K# }( R
53.     motor_9.run(0);
    
54.     motor_10.run(0);
    
55.   }
    - h8 F: ^/ K! G2 C
56.   do{}while(millis() - StartTime < interval);
    - g( T$ Z! I- K- B; ^' d7 X
57. }
    
58. 
    # }5 {8 X/ s3 E3 ?5 l5 J
59. void MotoL(int Power){
    * @: p( ]9 I+ [
60.   if (Power > MaxSpeed){
    , U+ Q  Z5 d+ c& g& _
61.      Power = MaxSpeed;
    + R' F" Z- O) ?. Z7 ]2 h
62.   }
    - f' z% }9 P- X8 X7 A: }- X
63.   if (Power < -MaxSpeed){
    
64.      Power = -MaxSpeed;
    ( I% A  D( U. y& r# [9 F  r
65.   }
    7 H7 h) F; k* Y9 U. W9 q
66.   motor_9.run(Power);
    
67. }  
    5 K& v' h1 j5 k9 n% O7 d: r* n& x
68. 
    : C% d. C& \6 x
69. void MotoR(int Power){
    
70.   if (Power > MaxSpeed){
    , M) E: u1 [' L- I( Y3 ?9 L
71.      Power = MaxSpeed;
    - T3 m1 Q7 H+ d) O% [  x
72.   }
    
73.   if (Power < -MaxSpeed){
    ; A5 K' \0 f# ^/ d- D
74.      Power = -MaxSpeed;
    
75.   }
    
76.   motor_10.run(Power);
    : @! v/ k  j+ C" f2 V" a
77. }  

複製代碼

. Y+ Y, z" F( k4 |0 ]5 [& K$ b
% H% O2 c4 |. a3 \

: C- g! v/ p9 M( i- O& d
您好，不知是否能向您請教。
目前和宋修賢老師在處理Ardui Car
% O  T( z9 e" y2 n) v雖然已使用較繁雜的方式處理了跑出黑線外的狀況

但基於想追求更精簡的程式所以還是想請問一下
! ]8 l8 G6 B& `# N6 C# k8 [& A就是如果我只以單純寫PID控制時，常遇到CNY70跑出黑線外後便往前衝
不知道您是否願意教我可以如何處理
! x3 t' L0 p& e" v* N0 U7 H! {
9 O4 t! n, c8 H) m
- c! O8 z4 v+ F1 Z: [% I以下是我挑出我一般寫純PID控制的Code：

1. int MotoSpeed = 250;
   + Z) E- q) t- M8 k" I8 b' U& @  S
2. double CNY70Val = 1000;
   
3. int interror = 0;
   
4. int olderror = 0;
   ! s8 s; p+ F# L5 K; n. q0 N, s1 j
5. double values;
   # G: x/ S6 t& k9 O3 V+ a' I
6. 
   . s9 _$ Y: ?( [5 h
7. 
   # i$ F6 F, m! f  R1 {3 Z0 R8 F
8. void CNY70()
   
9. {
   
10.   valuesRR = analogRead(RR)
    
11.   valuesMR = analogRead(MR);
    
12.   valuesMM = analogRead(MM);
    ! W7 w$ Q! o1 |, D
13.   valuesML = analogRead(ML);
    ( [" \9 b0 P1 h4 j  H+ ?; U
14.   valuesLL = analogRead(LL);
    
15. 
    
16.   if (valuesRR > CNY70Val)
    . A6 W9 L. y+ R7 O
17.     valuesRR = CNY70Val;
    
18.   if (valuesMR > CNY70Val)
    : {6 }+ g$ f" o( h) l
19.     valuesMR = CNY70Val;
    
20.   if (valuesMM > CNY70Val)
    1 j2 ~) J$ z4 v$ I3 M
21.     valuesMM = CNY70Val;
    * L% \7 ?" Q" ?# m: `0 t6 Z
22.   if (valuesML > CNY70Val)
    6 O* Q/ Z' }6 Y" H/ N2 D* N1 ~
23.     valuesML = CNY70Val;
    + T' A  ~4 g$ ^4 o+ Z, H" E3 x
24.   if (valuesLL > CNY70Val)
    
25.     valuesLL = CNY70Val;
    
26. 
    
27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    ' S& M4 A) s. ]( N- p8 O% B& p
28. }
    
29. 
    # N4 W3 h& H$ p$ D+ W6 p
30. void Car()
    ' ^& U7 f4 |! L2 [
31. {
    
32.   while (1) {
    
33.     CNY70();
    
34. 
    
35.     int error = ((int)values);
    
36.     interror += error;
    
37.     int lasterror = error - olderror;
    
38.     olderror = error;
    
39.     int power = error / 5 + interror / 10000 + lasterror;
    , p  z: ^5 r- D  p
40. 
    5 Q2 L( q! ]* G/ w) z3 W( |
41.     if (power > MotoSpeed)
    
42.       power = MotoSpeed;
    $ ^: `) K; H9 G
43.     if (power < -MotoSpeed)
    6 o% j/ d1 ]+ t
44.       power = -MotoSpeed;
    0 A5 V$ b/ J9 e0 T, x
45. 
    
46.     if (power > 0)
    
47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作，正數正轉 負數反轉。
    # o% Z, K9 a( {/ U
48.     else
    , Z+ l: P3 x' {9 q9 u  d
49.       Speed(MotoSpeed + power, MotoSpeed);
    1 z- t0 K4 v! l2 ]7 E
50.   }
    
51. }

複製代碼

" p; i1 V# e+ ~  X
/ }3 x' ]9 E6 z6 f: j6 g

感謝分享

感謝分享