PID 循跡自走車範例

1. #include <Arduino.h>
   
2. #include <Wire.h>
   % h  W8 r4 A: {5 y0 k! w4 z1 \
3. #include <Servo.h>
   
4. 
   
5. #include "MePort.h"
   ( O# k+ i$ r8 ~0 |4 {4 r- @( s
6. #include "MeUltrasonic.h"
     j0 c" i1 r% R% j8 e
7. #include "MeDCMotor.h"
   + x* N, [& S% v3 Q
8. 
   
9. //double Input, Output ;
   8 ~  \4 T1 u4 C- y* n9 b
10. float MaxSpeed = 255;
    2 s& Z* V- c$ }# T8 |
11. float MaxPower = 180;
    
12. float MinPower = 120;
    
13. float Error,ErrorAcc,ErrorDec;
    ) y2 k+ a* u/ N
14. 
    
15. float Kp=0.14;
    ) h9 g& G/ h. C% v" s$ K$ K
16. float Kd=0.00020;//23;
    8 Y9 |* x$ F) P4 a0 }& ]1 I" H1 v
17. float Ki=0.000201;
    
18. 
    ! `8 Y" ~: i* n& J, V# E6 L
19. float nPower;
    
20. MePort lightsensor_6(6);
    
21. MePort lightsensor_8(8);
    ) }/ X3 l5 {+ I" q
22. MeDCMotor motor_9(9);
    
23. MeDCMotor motor_10(10);
    # T5 U0 g; J+ _$ o
24. unsigned long previousMillis = 0;
    * l" v: [& g$ D& O* I4 z
25. const long interval = 1;
    ; R8 E; y8 ^3 }- \/ y' r; Y, E
26. 
    . d- m9 i& O* v& j" ]
27. void setup(){
    ) G3 V4 W" W6 E
28.     lightsensor_6.dWrite1(1);
    
29.     nPower = 160;
    9 n. D% L2 H4 Y0 w
30.     Error=0;
    1 H* T  y. F  u; L- D
31.     ErrorAcc=0;
    ' N7 t2 `. A  N# ]+ m6 J
32. }
    / m$ |* C3 t% i- M1 A- ^0 j
33. 
    ! _2 e1 b% E1 Y8 q) t: A% Y' V! z6 V
34. void loop(){
    
35.   unsigned long StartTime = millis();
    - V# u3 g$ r; w. e5 k
36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    + {3 v1 h, }' m, J! I  R% V" Q) _
37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    
38.     ErrorAcc +=  nError*Kd ;
    . s6 `4 l1 C! {3 B7 i
39.     ErrorDec -=  nError*Ki ;
    ) C+ [* i5 H2 C9 L
40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    
41.     if(nError < 80 && nError > -80){
    : r/ M* t$ W* e* _9 Q
42.       if(nPower < MaxPower){
    
43.         nPower += 3;
    
44.       }
    
45.     } else{
    * a# O: O/ q( D5 C5 Y
46.       if(nPower > MinPower){
    
47.         nPower -= 2;
    * f8 }0 X) i4 f+ m
48.       }
    6 W" U+ M4 @. W7 r, X% a4 ]6 D
49.     }
    
50.     MotoL(nPower-Error);
    + t% E$ `/ a& j. b+ j  B3 _9 b: \
51.     MotoR(nPower+Error);   
    2 c8 I# Y! \% P; V
52.   }else{
    
53.     motor_9.run(0);
    
54.     motor_10.run(0);
    
55.   }
    
56.   do{}while(millis() - StartTime < interval);
    , W3 o5 l# o% g% S
57. }
    6 e) T. j- ^: l2 f; V3 h$ ?7 n
58. 
    + c, G3 W9 J- _0 ^( O
59. void MotoL(int Power){
    
60.   if (Power > MaxSpeed){
    - e; Q3 L0 ^$ U9 H7 I0 J( @7 w) D  k
61.      Power = MaxSpeed;
    
62.   }
    . _( O' g# Y9 n' I/ ]
63.   if (Power < -MaxSpeed){
    
64.      Power = -MaxSpeed;
    
65.   }
    6 l. L) \# n# c% F) t+ z+ V: r" E
66.   motor_9.run(Power);
    
67. }  
    
68. 
    1 j3 y/ ?' K( J& m
69. void MotoR(int Power){
    
70.   if (Power > MaxSpeed){
    * X; e$ o) ^' Y" k
71.      Power = MaxSpeed;
    
72.   }
    , N- D: `) [! E5 _( b' P
73.   if (Power < -MaxSpeed){
    0 Q3 l$ @, N7 L; v2 m! e% Y* ^
74.      Power = -MaxSpeed;
    
75.   }
    6 q" `8 A% q8 P9 a# v4 A& g: S
76.   motor_10.run(Power);
    : j) H4 S  S) y* Z- f2 ^4 h/ q
77. }  

複製代碼

; z, a5 i3 [, f7 h! ?( Y

; y9 q/ o! l$ z' I" Y3 i  M% [$ e6 V您好，不知是否能向您請教。
目前和宋修賢老師在處理Ardui Car
* E  }" @& S  ]0 W' t1 K! _* x雖然已使用較繁雜的方式處理了跑出黑線外的狀況
5 }1 }, q; |5 H, g" v: ?0 ~7 I
: ~+ U5 `8 K' i但基於想追求更精簡的程式所以還是想請問一下
( I8 ]9 ?) D) k0 H. A4 ~就是如果我只以單純寫PID控制時，常遇到CNY70跑出黑線外後便往前衝
不知道您是否願意教我可以如何處理


& T; r, x" A. h, _% r; l以下是我挑出我一般寫純PID控制的Code：

1. int MotoSpeed = 250;
   
2. double CNY70Val = 1000;
   
3. int interror = 0;
   2 x- t4 a; y% C5 ]! ?* Z  h* t
4. int olderror = 0;
   
5. double values;
   
6. 
   ) Z7 Z$ p7 f: W6 I5 g) K8 X* a! d* Q
7. 
   - S1 ]4 p; N  t& Z
8. void CNY70()
   
9. {
   
10.   valuesRR = analogRead(RR)
    
11.   valuesMR = analogRead(MR);
    
12.   valuesMM = analogRead(MM);
    * U  R9 i8 R2 i2 r# z& L( \( k
13.   valuesML = analogRead(ML);
    1 j9 e1 v- A" b
14.   valuesLL = analogRead(LL);
    # q2 ^7 M0 O& {. ^- y! e1 G5 {3 G, h
15. 
    0 E3 _6 [4 B' N6 d0 M
16.   if (valuesRR > CNY70Val)
    0 D' P& u6 o4 i
17.     valuesRR = CNY70Val;
    
18.   if (valuesMR > CNY70Val)
    7 i% z0 q: [9 q1 p$ V
19.     valuesMR = CNY70Val;
    
20.   if (valuesMM > CNY70Val)
    
21.     valuesMM = CNY70Val;
    2 D; ~8 Q; U1 t+ I; ^
22.   if (valuesML > CNY70Val)
      E: R7 |' A8 y: q
23.     valuesML = CNY70Val;
    ; X7 X: u8 d9 E
24.   if (valuesLL > CNY70Val)
    
25.     valuesLL = CNY70Val;
    ; F6 ?4 @: i" s8 R
26. 
    . O8 Q( a( P' T4 u. C- G& p
27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    ' v: T0 @. |1 T: Z; Q
28. }
    ' `- o3 H2 U+ x( R3 j: u
29. 
    
30. void Car()
    " A+ u" @& c2 s
31. {
    ! U+ H% i! x' J/ k
32.   while (1) {
    * I8 s/ O' i9 X, b) q) u: ~
33.     CNY70();
      {  E- r0 p( Q# ]! \: c1 g3 _
34. 
    : ~- `0 g' B0 O$ `: X" p  g
35.     int error = ((int)values);
    : c0 I! ]  G+ r+ s! y) X
36.     interror += error;
    
37.     int lasterror = error - olderror;
    
38.     olderror = error;
    
39.     int power = error / 5 + interror / 10000 + lasterror;
    
40. 
    & a' u- N9 F  Q5 e
41.     if (power > MotoSpeed)
    
42.       power = MotoSpeed;
    
43.     if (power < -MotoSpeed)
    $ A1 _8 m; t. I- x+ U* t! y
44.       power = -MotoSpeed;
    
45. 
    
46.     if (power > 0)
    
47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作，正數正轉 負數反轉。
    
48.     else
    5 a& |1 l& q5 ^* R5 t# o8 E& b7 A
49.       Speed(MotoSpeed + power, MotoSpeed);
    
50.   }
    7 a' G+ P0 q- N" b
51. }

複製代碼

) h; {2 Y$ A. z  K& Y
6 u3 @! k3 t% s9 A3 }

感謝分享

感謝分享