PID 循跡自走車範例

1. #include <Arduino.h>
   
2. #include <Wire.h>
   * X) L4 r* U9 p; [- J" s
3. #include <Servo.h>
   
4. 
   7 g* s6 a2 {: d$ v) P9 ?& L( l
5. #include "MePort.h"
     L5 V) Y" m( G
6. #include "MeUltrasonic.h"
   
7. #include "MeDCMotor.h"
   " m4 ~  k; Q/ V, r9 Q
8. 
   
9. //double Input, Output ;
   
10. float MaxSpeed = 255;
    / g7 U; O/ s; {! K, K/ }+ P) c
11. float MaxPower = 180;
    
12. float MinPower = 120;
    
13. float Error,ErrorAcc,ErrorDec;
    
14. 
    
15. float Kp=0.14;
    ; z# Z) A, b9 l6 n  f
16. float Kd=0.00020;//23;
    $ o2 [9 @2 ~$ |
17. float Ki=0.000201;
    
18. 
    # B- C/ Y& ^% ~  R: i; ~0 `
19. float nPower;
    7 W  T2 V- u! Q/ c
20. MePort lightsensor_6(6);
    4 K" t$ i( f+ W, }
21. MePort lightsensor_8(8);
    
22. MeDCMotor motor_9(9);
    5 t- V4 |, O) ^2 c8 H
23. MeDCMotor motor_10(10);
    8 W- ~/ f1 s9 a4 o8 }# k' Y
24. unsigned long previousMillis = 0;
    5 ^  o3 G$ L% j2 H8 n6 b& b
25. const long interval = 1;
    , i/ K. {7 G( {& b/ D
26. 
    - n5 V' i  U/ H$ b& a: x$ S
27. void setup(){
    
28.     lightsensor_6.dWrite1(1);
    
29.     nPower = 160;
    9 N: [4 N( }- y: ]- _) U
30.     Error=0;
    ' q/ b* b! m$ R! W7 o
31.     ErrorAcc=0;
    
32. }
    ' Z" L( T! P6 A3 i+ _3 b
33. 
    
34. void loop(){
    , l$ n9 N  i- f7 H; D
35.   unsigned long StartTime = millis();
    9 n  [( K# a( f" ^
36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    
37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    3 E& N" F% o; \: i
38.     ErrorAcc +=  nError*Kd ;
    % x' x% E/ H! _- D* ^& F6 T$ u: \6 s; |
39.     ErrorDec -=  nError*Ki ;
    8 T9 m0 k! h# L% W" v" M7 L9 C
40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    & f8 n0 ?" `* o( z0 ?$ V1 q
41.     if(nError < 80 && nError > -80){
    ! n* w3 c8 p# J
42.       if(nPower < MaxPower){
    0 N: Y. V$ o! g5 X* k2 x
43.         nPower += 3;
    
44.       }
    
45.     } else{
    1 o& u7 @9 f- F. X2 t
46.       if(nPower > MinPower){
    
47.         nPower -= 2;
    
48.       }
    6 ]( e! [4 S. |, J9 d+ K5 k
49.     }
    
50.     MotoL(nPower-Error);
    
51.     MotoR(nPower+Error);   
    
52.   }else{
    4 H6 _! q& W" b: K
53.     motor_9.run(0);
    
54.     motor_10.run(0);
    
55.   }
    ) u4 t4 U' Q$ _6 G& t1 B# u9 t
56.   do{}while(millis() - StartTime < interval);
    ) g4 V8 m4 N. p
57. }
    5 i" m( d- |1 Y- c; R
58. 
    
59. void MotoL(int Power){
    4 f5 a: G5 G# D" e4 V  D
60.   if (Power > MaxSpeed){
    % F7 `2 _) g2 [' C6 L( g
61.      Power = MaxSpeed;
    
62.   }
    
63.   if (Power < -MaxSpeed){
    , d" ^. P3 g; e7 L( Q9 a- [0 q: v) K
64.      Power = -MaxSpeed;
    , W! `; s  z6 i+ w7 }3 v* H" U
65.   }
    
66.   motor_9.run(Power);
    
67. }  
    
68. 
    
69. void MotoR(int Power){
    
70.   if (Power > MaxSpeed){
    
71.      Power = MaxSpeed;
    
72.   }
    , C7 w. M( \6 \. {
73.   if (Power < -MaxSpeed){
    ' e7 U/ C  x5 F$ [0 m9 N1 p
74.      Power = -MaxSpeed;
    ( L" r/ C- x  e' E
75.   }
    
76.   motor_10.run(Power);
    ' S/ V  P2 n& A0 _4 i2 q1 o
77. }  

複製代碼

% q% H! i/ V" l  M* D. b1 u6 j
; Q( d6 M& S& R0 E您好，不知是否能向您請教。
3 Y! L. w7 Y, g7 ?: b  `目前和宋修賢老師在處理Ardui Car
雖然已使用較繁雜的方式處理了跑出黑線外的狀況

但基於想追求更精簡的程式所以還是想請問一下
就是如果我只以單純寫PID控制時，常遇到CNY70跑出黑線外後便往前衝
不知道您是否願意教我可以如何處理


7 q6 l. o4 Z$ x2 N以下是我挑出我一般寫純PID控制的Code：

1. int MotoSpeed = 250;
   
2. double CNY70Val = 1000;
   1 y. }1 {( u% B1 c0 F4 ]
3. int interror = 0;
   $ w' i, D# c0 H. m6 q: S* I9 Q5 H
4. int olderror = 0;
   $ k" J8 D. y- r4 d9 x; {* U2 |
5. double values;
   8 Y1 H+ b: Z3 N
6. 
   
7. 
   1 N' p- g" `2 u
8. void CNY70()
   
9. {
   
10.   valuesRR = analogRead(RR)
    & m: j0 i2 f8 @; z
11.   valuesMR = analogRead(MR);
    
12.   valuesMM = analogRead(MM);
    
13.   valuesML = analogRead(ML);
    3 V- k- U, n2 m6 D! M1 Q( K
14.   valuesLL = analogRead(LL);
    
15. 
    " W6 w' r6 s9 d* W+ J
16.   if (valuesRR > CNY70Val)
    
17.     valuesRR = CNY70Val;
    ( G5 l: t3 d6 B9 y$ s: a
18.   if (valuesMR > CNY70Val)
    
19.     valuesMR = CNY70Val;
    & t- M& n" v2 \# S, L
20.   if (valuesMM > CNY70Val)
    1 y6 [* E6 q% F
21.     valuesMM = CNY70Val;
    + E: F1 x$ `& S3 F
22.   if (valuesML > CNY70Val)
    
23.     valuesML = CNY70Val;
    $ ~% Q% C0 A/ S
24.   if (valuesLL > CNY70Val)
    % J+ u7 ~0 J4 e5 ], \
25.     valuesLL = CNY70Val;
    ! j1 x' C# i3 H; z3 K4 N  _
26. 
    
27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    
28. }
    2 q$ o- P8 q, S- p7 |4 u
29. 
    
30. void Car()
    6 ^% O$ r6 v  g3 L
31. {
    
32.   while (1) {
    # |8 h# U( k/ j. C% t
33.     CNY70();
    : e' g, [# e4 S. P, h8 A8 l. K+ {
34. 
    1 N! s" Z0 [- _2 R$ C. e; T3 B
35.     int error = ((int)values);
    
36.     interror += error;
      _1 ~6 l; C1 V4 m7 L: K
37.     int lasterror = error - olderror;
    9 {, m8 Y8 B1 y/ R5 z# g
38.     olderror = error;
    8 ?+ e7 r" ^) h( z
39.     int power = error / 5 + interror / 10000 + lasterror;
    
40. 
    
41.     if (power > MotoSpeed)
    2 q; l6 w  F0 }
42.       power = MotoSpeed;
    
43.     if (power < -MotoSpeed)
    
44.       power = -MotoSpeed;
    & c3 M4 [+ x+ q' O" g
45. 
    6 `4 I. F2 r8 u8 z2 X- h
46.     if (power > 0)
    & L# c6 \1 n& m  A4 K. i" F
47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作，正數正轉 負數反轉。
    
48.     else
    . C' X9 @& d% C$ w! _
49.       Speed(MotoSpeed + power, MotoSpeed);
    
50.   }
    
51. }

複製代碼

* c6 o$ y: s& T; ~' x# ^& [" Y

感謝分享

感謝分享