PID 循跡自走車範例

1. #include <Arduino.h>
   
2. #include <Wire.h>
   
3. #include <Servo.h>
   
4. 
   ( x# E3 g9 F* R0 J+ S5 y" j
5. #include "MePort.h"
   4 t1 b, N; O$ ]! t) D
6. #include "MeUltrasonic.h"
   7 Y4 ^- K% E; ?8 U  G% l& v: T
7. #include "MeDCMotor.h"
   # o) l1 ~) m% s- t) M9 i
8. 
   
9. //double Input, Output ;
   3 j; a+ ^4 m: X( L- |* b
10. float MaxSpeed = 255;
    $ u& |  {: p  g' z+ |4 r, H6 g7 X  k
11. float MaxPower = 180;
    4 I$ }% ?$ J5 b
12. float MinPower = 120;
    
13. float Error,ErrorAcc,ErrorDec;
    * }9 X/ f, G+ Z5 ]5 D+ K
14. 
    
15. float Kp=0.14;
    
16. float Kd=0.00020;//23;
    # G/ ?. X7 ]+ f6 v
17. float Ki=0.000201;
    
18. 
    " A" I- ?  Z! r" L
19. float nPower;
    / x$ A" f: N) `& w+ z. l1 J' b, w
20. MePort lightsensor_6(6);
    
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. 
    + V$ G' u4 D& ]+ H$ d0 j* |5 G
27. void setup(){
    ( G2 ~/ q8 t; |2 p" m
28.     lightsensor_6.dWrite1(1);
    
29.     nPower = 160;
    ( H1 b/ w/ h1 Y8 B. K
30.     Error=0;
    + {9 X, N& n! q$ |6 Y. }; }
31.     ErrorAcc=0;
    
32. }
    9 d# ?- {) d/ z- t& S2 {
33. 
    ( `* }& n5 y2 J  H6 Z: F
34. void loop(){
    ; G. c4 s( K1 v  Y- c8 V5 w; A$ T
35.   unsigned long StartTime = millis();
    + R5 e& I' m6 x
36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    
37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    2 j5 B2 ?8 Z, \4 `1 @
38.     ErrorAcc +=  nError*Kd ;
    1 G4 D; ?) m) D  A" a& W5 f7 }
39.     ErrorDec -=  nError*Ki ;
    
40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    . \2 ^0 k7 F/ M% |6 q" h6 {
41.     if(nError < 80 && nError > -80){
    
42.       if(nPower < MaxPower){
    
43.         nPower += 3;
    
44.       }
    ' r7 k' Z# ^8 u4 s% n9 }
45.     } else{
    
46.       if(nPower > MinPower){
    4 N, @. v% |0 b
47.         nPower -= 2;
    
48.       }
    9 t8 Q5 |9 G" n# ~* e( k
49.     }
    
50.     MotoL(nPower-Error);
    
51.     MotoR(nPower+Error);   
    5 \% u, E( S. z) Z4 {
52.   }else{
    ! r0 u: A8 w$ F& h9 C2 g
53.     motor_9.run(0);
    6 Q8 ~" }& X2 U3 m
54.     motor_10.run(0);
    & a! S* m) y4 J( w+ u& D1 \# ?
55.   }
    
56.   do{}while(millis() - StartTime < interval);
    
57. }
    
58. 
    
59. void MotoL(int Power){
    
60.   if (Power > MaxSpeed){
    $ e6 _( o8 I- k% K! i2 B, v* c
61.      Power = MaxSpeed;
      R5 P6 A$ l, A7 `0 Y, O, j
62.   }
    # Z" I4 e8 _, a, ?  X; e
63.   if (Power < -MaxSpeed){
    ! t, F' M  m- h2 p9 W) B
64.      Power = -MaxSpeed;
    
65.   }
    7 x1 S  K% w/ ~. S/ P
66.   motor_9.run(Power);
    
67. }  
    
68. 
    
69. void MotoR(int Power){
    5 _: y, Y. F# m& B' a) }5 d6 a& f
70.   if (Power > MaxSpeed){
    
71.      Power = MaxSpeed;
    ! q1 Y. s, A0 a' A- F6 i
72.   }
    
73.   if (Power < -MaxSpeed){
    5 d7 k8 J6 z' D/ V7 o
74.      Power = -MaxSpeed;
    
75.   }
    
76.   motor_10.run(Power);
    
77. }  

複製代碼

/ _5 Q# p; t0 D- Q: G! a3 U, \

( }# C1 j. r+ O& Z# g5 G. {您好，不知是否能向您請教。
目前和宋修賢老師在處理Ardui Car
雖然已使用較繁雜的方式處理了跑出黑線外的狀況
/ S1 g$ \: f/ x- m! {# f* I
6 e! b3 L8 O% r$ T7 u" W+ I5 Y. |3 ^但基於想追求更精簡的程式所以還是想請問一下
8 T2 d! W0 H( W3 |! Q4 C" I( ^就是如果我只以單純寫PID控制時，常遇到CNY70跑出黑線外後便往前衝
不知道您是否願意教我可以如何處理
8 Y3 g; x: K9 F# }
: Q3 x" ]$ K# X$ F9 P3 s
以下是我挑出我一般寫純PID控制的Code：

1. int MotoSpeed = 250;
   " A* ~6 z) i- B" M  W$ D
2. double CNY70Val = 1000;
   
3. int interror = 0;
   
4. int olderror = 0;
   
5. double values;
   
6. 
   + [" t: G$ g1 Y4 p5 B3 {* K
7. 
   9 H: Z! N/ p7 U) V; S
8. void CNY70()
   7 T0 }% C' i3 `( N8 F" S7 R  d
9. {
   8 C/ m' ^0 {' D, r8 H* O, l
10.   valuesRR = analogRead(RR)
    3 j% q8 h% a5 s" L1 x: V
11.   valuesMR = analogRead(MR);
    
12.   valuesMM = analogRead(MM);
    
13.   valuesML = analogRead(ML);
      i$ M$ x9 [. v( t) w, _
14.   valuesLL = analogRead(LL);
    
15. 
    5 T; P/ f* u+ v
16.   if (valuesRR > CNY70Val)
    
17.     valuesRR = CNY70Val;
    7 l! Z7 w6 R" c3 C- Z
18.   if (valuesMR > CNY70Val)
    
19.     valuesMR = CNY70Val;
    
20.   if (valuesMM > CNY70Val)
    
21.     valuesMM = CNY70Val;
    
22.   if (valuesML > CNY70Val)
    
23.     valuesML = CNY70Val;
    
24.   if (valuesLL > CNY70Val)
    
25.     valuesLL = CNY70Val;
    
26. 
    ' P6 _# D5 J6 y1 R: z
27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    ; v+ L4 N: R& q; f0 G1 g
28. }
    1 a, g; l( A# n8 A$ F/ |
29. 
    5 a1 t, c8 N) P6 S! b
30. void Car()
    
31. {
    ; g$ Z$ L$ F) l3 u, w
32.   while (1) {
    
33.     CNY70();
    + o3 f$ I0 m- a
34. 
    
35.     int error = ((int)values);
    - j2 T' S& d5 k" s
36.     interror += error;
    5 }% X: N: F' l9 ?7 ^
37.     int lasterror = error - olderror;
    
38.     olderror = error;
    
39.     int power = error / 5 + interror / 10000 + lasterror;
    
40. 
    * n: o0 u  U; e
41.     if (power > MotoSpeed)
    1 @0 ]5 o6 v. B) v: j, e, k# O" u
42.       power = MotoSpeed;
    & C! y; K; O. y# O& i# ~; f
43.     if (power < -MotoSpeed)
    ( |: a& e9 E, U# t
44.       power = -MotoSpeed;
    
45. 
    ) f$ @3 V% e1 ]$ h/ I8 r0 Z
46.     if (power > 0)
    
47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作，正數正轉 負數反轉。
    % c6 b. |2 a6 h* ~4 j& d) O
48.     else
    , ?% u. L/ b+ l* u
49.       Speed(MotoSpeed + power, MotoSpeed);
    
50.   }
    $ q( W1 ~4 U# w" A: R
51. }

複製代碼

感謝分享

感謝分享