PID 循跡自走車範例

1. #include <Arduino.h>
   
2. #include <Wire.h>
   , N/ N3 p% R1 |- x: v/ ~
3. #include <Servo.h>
   / v( P7 K6 \  d) ~" K- B$ P
4. 
   7 r3 r# H" i" z+ j  |! T% e5 [0 n
5. #include "MePort.h"
   : F4 d, v* j, Q$ e1 J4 O! ~7 U
6. #include "MeUltrasonic.h"
   
7. #include "MeDCMotor.h"
   , z" s# i( E- |+ e  P* E  m
8. 
   
9. //double Input, Output ;
   / P5 M0 S) O: p( u( O$ e
10. float MaxSpeed = 255;
    0 X  B# ?1 L0 U3 |: [
11. float MaxPower = 180;
    / H# }  e1 z3 e: ~
12. float MinPower = 120;
    
13. float Error,ErrorAcc,ErrorDec;
    4 a6 M* w; i  C* A/ u+ a
14. 
    1 u: C9 |& c3 v
15. float Kp=0.14;
    
16. float Kd=0.00020;//23;
    # g: ~, z; M2 y/ G
17. float Ki=0.000201;
    
18. 
    
19. float nPower;
    2 e* K' A. a+ f9 }+ j% E2 y
20. MePort lightsensor_6(6);
      T* M6 {( y4 T' l( Z- x
21. MePort lightsensor_8(8);
    
22. MeDCMotor motor_9(9);
    
23. MeDCMotor motor_10(10);
    ) F( A$ ^' }* F1 P& a# M7 B
24. unsigned long previousMillis = 0;
    3 Q: ~* `" ?% l# s% E1 t5 {
25. const long interval = 1;
    * a9 k  U9 E4 L7 s. n% \: }
26. 
    
27. void setup(){
    
28.     lightsensor_6.dWrite1(1);
    " Q) e1 j7 B# L( G8 g) t1 W. I/ }
29.     nPower = 160;
    4 y: \  C( K9 V0 e
30.     Error=0;
    , ~8 E9 \9 ]1 Z$ L9 m: x' q5 Z
31.     ErrorAcc=0;
    
32. }
    1 g! ?  r6 \6 V% Y
33. 
    
34. void loop(){
    & I$ T- \8 l/ m/ G( J. g# M
35.   unsigned long StartTime = millis();
    5 {+ v' d; B6 i: Z2 i$ R
36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    ' `! a2 m* o, `: s% D; G
37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    
38.     ErrorAcc +=  nError*Kd ;
    
39.     ErrorDec -=  nError*Ki ;
    ' x2 d9 B: h* \2 S2 [
40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    / H- V. ?& \' w$ U
41.     if(nError < 80 && nError > -80){
    ; Q2 I6 q" P" z% Q0 Y( @
42.       if(nPower < MaxPower){
    6 z$ L. B7 D7 l0 A3 S8 x- O2 T
43.         nPower += 3;
    
44.       }
    
45.     } else{
    & \, N) b* H0 \% C; r, B+ o& Y
46.       if(nPower > MinPower){
    & u- `4 N% H) H$ k  g
47.         nPower -= 2;
    
48.       }
    7 U! c2 @  @; u( ?6 l
49.     }
    1 s& r5 j% s3 l7 m  b* N
50.     MotoL(nPower-Error);
    & M( p1 M4 N1 j" n1 X
51.     MotoR(nPower+Error);   
    - [" h' w, }/ ~% \, }0 y3 X
52.   }else{
    
53.     motor_9.run(0);
    
54.     motor_10.run(0);
    7 k! x8 [' k9 `
55.   }
    
56.   do{}while(millis() - StartTime < interval);
    
57. }
    & v: p) C5 ~; L
58. 
    ' t! ?, g  s% [+ N8 s
59. void MotoL(int Power){
    
60.   if (Power > MaxSpeed){
    
61.      Power = MaxSpeed;
    . x$ a9 _9 n# i6 P9 @
62.   }
    9 [1 o. k, e9 g7 a/ [7 C# G
63.   if (Power < -MaxSpeed){
      [/ E, x0 M4 s: I( ]$ a
64.      Power = -MaxSpeed;
    
65.   }
      x/ W% ~& X4 X+ H. g
66.   motor_9.run(Power);
    
67. }  
    
68. 
    " P+ d7 d- P& ?: I4 B+ j" d" ~
69. void MotoR(int Power){
    % i4 n! n; [7 o' {4 V* E; N
70.   if (Power > MaxSpeed){
    
71.      Power = MaxSpeed;
    
72.   }
    
73.   if (Power < -MaxSpeed){
    ; S/ i4 @. D; ?9 Z5 \; F5 {
74.      Power = -MaxSpeed;
    
75.   }
    
76.   motor_10.run(Power);
    
77. }  

複製代碼

) ?4 s3 p# w  J2 y# }6 Y
+ F  N& ^, _3 X% D! |6 I' \+ y. |2 v# O

5 X+ q! h5 d! p, ]8 z( W. j
您好，不知是否能向您請教。
目前和宋修賢老師在處理Ardui Car
* _; j9 H5 ^# \; B$ I8 `雖然已使用較繁雜的方式處理了跑出黑線外的狀況
/ i0 ?. {- ]+ N; p9 b
. ^2 F7 F; {" I5 g: L$ [. I但基於想追求更精簡的程式所以還是想請問一下
就是如果我只以單純寫PID控制時，常遇到CNY70跑出黑線外後便往前衝
$ j: r/ j, W  w7 P不知道您是否願意教我可以如何處理


4 U3 V3 p( b) t# q5 }7 N% Y以下是我挑出我一般寫純PID控制的Code：

1. int MotoSpeed = 250;
   
2. double CNY70Val = 1000;
   
3. int interror = 0;
   3 F; ?- e% h$ Y, a, g5 I
4. int olderror = 0;
   
5. double values;
   " [. f2 _8 S1 o5 B% J
6. 
   
7. 
   
8. void CNY70()
   
9. {
   
10.   valuesRR = analogRead(RR)
    
11.   valuesMR = analogRead(MR);
    
12.   valuesMM = analogRead(MM);
    
13.   valuesML = analogRead(ML);
      h& s9 T6 o$ }. P1 L7 N$ J, {' z
14.   valuesLL = analogRead(LL);
    
15. 
    
16.   if (valuesRR > CNY70Val)
    , p1 o7 Q- n! x. W# R
17.     valuesRR = CNY70Val;
    
18.   if (valuesMR > CNY70Val)
    ' z% }7 n2 d3 I5 O0 `& `2 Q
19.     valuesMR = CNY70Val;
    
20.   if (valuesMM > CNY70Val)
    % x/ v( W8 O3 x5 _' k) c
21.     valuesMM = CNY70Val;
    
22.   if (valuesML > CNY70Val)
    
23.     valuesML = CNY70Val;
    7 g$ W  g) K2 F; U5 Z0 Q3 T
24.   if (valuesLL > CNY70Val)
    . D; |  B9 h2 v: S" j0 G" b
25.     valuesLL = CNY70Val;
    ! C2 B' b! K0 a) w. N
26. 
    4 o3 Y& O) P9 t) X2 Y4 Q$ Z
27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    
28. }
    
29. 
    / S3 I/ f- J6 p4 A- R
30. void Car()
    7 U2 j: B; ?* I2 @. B: n
31. {
    0 P9 R* m  B. d% y0 }
32.   while (1) {
    
33.     CNY70();
    
34. 
    9 Y1 [2 j1 }+ M7 g6 J1 y
35.     int error = ((int)values);
    8 @7 v) v, A9 l/ c0 }. G
36.     interror += error;
    
37.     int lasterror = error - olderror;
    
38.     olderror = error;
    
39.     int power = error / 5 + interror / 10000 + lasterror;
    
40. 
    
41.     if (power > MotoSpeed)
    ; Z$ L- l5 h7 }1 v8 }
42.       power = MotoSpeed;
    
43.     if (power < -MotoSpeed)
    
44.       power = -MotoSpeed;
    2 ~* ?. M' P) y# t# q6 A7 j' n
45. 
    
46.     if (power > 0)
    . ]3 o; [- ?( }  T
47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作，正數正轉 負數反轉。
    1 Z0 L! w9 D* c' M+ R' m* X
48.     else
    
49.       Speed(MotoSpeed + power, MotoSpeed);
    . S1 S# }3 H% m/ o' l
50.   }
    
51. }

複製代碼

感謝分享

感謝分享