PID 循跡自走車範例

1. #include <Arduino.h>
   - b1 z% u2 K7 f: p$ J
2. #include <Wire.h>
   / S1 w2 b2 G. m- p) |1 A/ G
3. #include <Servo.h>
   
4. 
   7 H/ x$ E* Q+ ~+ N
5. #include "MePort.h"
   
6. #include "MeUltrasonic.h"
   
7. #include "MeDCMotor.h"
   $ X9 Q( l, }& O3 {  j0 J; W
8. 
   
9. //double Input, Output ;
   
10. float MaxSpeed = 255;
    
11. float MaxPower = 180;
    
12. float MinPower = 120;
    
13. float Error,ErrorAcc,ErrorDec;
    
14. 
    
15. float Kp=0.14;
    
16. float Kd=0.00020;//23;
    
17. float Ki=0.000201;
    3 Y- V" _9 z  n, w* H7 ?5 h! \3 M
18. 
    
19. float nPower;
    
20. MePort lightsensor_6(6);
    2 }4 |$ e! Z  f+ s( \
21. MePort lightsensor_8(8);
    
22. MeDCMotor motor_9(9);
    
23. MeDCMotor motor_10(10);
    
24. unsigned long previousMillis = 0;
    " N- g$ n- g, y
25. const long interval = 1;
    
26. 
    
27. void setup(){
    
28.     lightsensor_6.dWrite1(1);
    
29.     nPower = 160;
    / }' ]# p/ K/ a2 _# y6 X
30.     Error=0;
    ' g' y  F; e$ _. g, r+ N
31.     ErrorAcc=0;
    1 i) g8 [& Z- u: _
32. }
    + c. M& O- e4 @
33. 
    
34. void loop(){
    9 a+ m% q* U+ z; N: @7 p
35.   unsigned long StartTime = millis();
    
36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    * S( Q. ?+ ^. B% b% ]
37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    
38.     ErrorAcc +=  nError*Kd ;
    1 l; e6 W6 p! m1 a# H
39.     ErrorDec -=  nError*Ki ;
    & {* f! \1 l  r1 C
40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    2 @0 ^) E# b4 ]* J+ z' e
41.     if(nError < 80 && nError > -80){
    
42.       if(nPower < MaxPower){
    
43.         nPower += 3;
    3 v& N  v( z/ R: O
44.       }
    * e$ [* A! U: U/ p
45.     } else{
    
46.       if(nPower > MinPower){
    & j# J# u, |8 s; w) s' e; c$ W! Z& i
47.         nPower -= 2;
    
48.       }
    . d. W' @( E: Y) F, o
49.     }
    
50.     MotoL(nPower-Error);
    7 n  J" b2 \  j+ U0 K
51.     MotoR(nPower+Error);   
    5 U6 E0 ^! ^# |% i7 ]8 O" }5 V
52.   }else{
    3 C' t- S$ U6 v* d
53.     motor_9.run(0);
    
54.     motor_10.run(0);
    0 ]2 W! }* I, j5 A9 o
55.   }
    
56.   do{}while(millis() - StartTime < interval);
    3 }3 _4 I. K; L1 T' y$ ]) p
57. }
    $ v& E) L! Z' Y
58. 
    
59. void MotoL(int Power){
    9 y  k/ o5 o3 `3 y& t: T' M
60.   if (Power > MaxSpeed){
    
61.      Power = MaxSpeed;
    
62.   }
    
63.   if (Power < -MaxSpeed){
    
64.      Power = -MaxSpeed;
      `; Q0 C) T& V, ?
65.   }
    ; `& g7 |+ G1 W4 |; R" m
66.   motor_9.run(Power);
    ! g* h. J% a% ?6 T& n; X
67. }  
    
68. 
      x  ^' o4 f' T* f* r8 s
69. void MotoR(int Power){
    
70.   if (Power > MaxSpeed){
    
71.      Power = MaxSpeed;
    
72.   }
    
73.   if (Power < -MaxSpeed){
    
74.      Power = -MaxSpeed;
    
75.   }
    / i+ b, D4 v. l/ Q; t
76.   motor_10.run(Power);
    
77. }  

複製代碼

# U/ r+ N3 \. B

1 ^5 z" z1 D. A/ g
您好，不知是否能向您請教。
目前和宋修賢老師在處理Ardui Car
. `4 s" A' O* w: B; H& r雖然已使用較繁雜的方式處理了跑出黑線外的狀況
, {1 B. P2 F. N! g  t
但基於想追求更精簡的程式所以還是想請問一下
就是如果我只以單純寫PID控制時，常遇到CNY70跑出黑線外後便往前衝
9 N$ r4 i- [8 i* W9 j3 z4 K不知道您是否願意教我可以如何處理

0 w( s1 |" k4 \5 N
以下是我挑出我一般寫純PID控制的Code：

1. int MotoSpeed = 250;
   
2. double CNY70Val = 1000;
   # W/ U! T$ ]/ G: T8 G) |7 s
3. int interror = 0;
   / J; J4 p/ s3 @$ g7 @' c
4. int olderror = 0;
   # I" q3 j7 p+ Y# B- K3 L
5. double values;
   
6. 
   ' ?4 x$ g" E$ }
7. 
   ! {& S. z: ^; k$ Y
8. void CNY70()
   - r# s9 g& v  A
9. {
   5 _! r( g$ n6 X" z1 s+ U  G) q/ `
10.   valuesRR = analogRead(RR)
    
11.   valuesMR = analogRead(MR);
    % e- V: O, z( V/ c# t  `5 ^
12.   valuesMM = analogRead(MM);
    
13.   valuesML = analogRead(ML);
    ) |7 D0 C! z$ w( T% K7 c
14.   valuesLL = analogRead(LL);
    
15. 
    
16.   if (valuesRR > CNY70Val)
    ; K# \' U# L% n
17.     valuesRR = CNY70Val;
    
18.   if (valuesMR > CNY70Val)
      i1 E6 @. I+ w0 B
19.     valuesMR = CNY70Val;
    
20.   if (valuesMM > CNY70Val)
    9 u: l, {' @* f8 v' |
21.     valuesMM = CNY70Val;
    2 H% J! V- U/ i" [) t3 N, L
22.   if (valuesML > CNY70Val)
    ( \& ?( s0 j$ ?+ a
23.     valuesML = CNY70Val;
    
24.   if (valuesLL > CNY70Val)
    
25.     valuesLL = CNY70Val;
    
26. 
    . e; ]6 h; @* ]2 ^; R# `1 v
27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    
28. }
    
29. 
    ) ^/ t  n$ ]5 }/ M
30. void Car()
    ) D: N7 O3 i, d' B! y4 v, u9 v1 ^, I
31. {
    - n8 T) ?! p& ^) d
32.   while (1) {
    ' v" M. B  ^( O- e" l; P% n
33.     CNY70();
    
34. 
    
35.     int error = ((int)values);
    
36.     interror += error;
    + w& v8 t" a6 c) [
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)
    
44.       power = -MotoSpeed;
      O' G" ?0 J  F  S% b
45. 
      i1 z4 B7 e9 e+ l& M/ R
46.     if (power > 0)
    
47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作，正數正轉 負數反轉。
    
48.     else
    
49.       Speed(MotoSpeed + power, MotoSpeed);
    0 k5 W4 X- W: @$ z' C  B
50.   }
      i- e& Y. o  S& Q9 A5 I4 T9 p6 D& x
51. }

複製代碼

感謝分享

感謝分享