PID 循跡自走車範例

1. #include <Arduino.h>
   : D& Z: ?0 Y0 O- Q4 j2 Q
2. #include <Wire.h>
   2 b$ U* y# U1 `4 U  C
3. #include <Servo.h>
   
4. 
   
5. #include "MePort.h"
   5 D+ i* Q. N5 ~( X) n* n: j
6. #include "MeUltrasonic.h"
   2 |' n  H0 _) k" R4 \' N, ?1 p
7. #include "MeDCMotor.h"
   & a+ F2 m" K$ S6 z1 g
8. 
   & B; D- |& d' a$ Z) b! G- H
9. //double Input, Output ;
   
10. float MaxSpeed = 255;
    2 G' n! v# r: d2 I# v
11. float MaxPower = 180;
    & e/ |1 p( B" g, Q5 _
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;
    
18. 
    
19. float nPower;
    $ S/ L% x; L( d6 g1 P
20. MePort lightsensor_6(6);
    # `. q% g: F* S" c* Q3 {
21. MePort lightsensor_8(8);
    
22. MeDCMotor motor_9(9);
    
23. MeDCMotor motor_10(10);
    9 T7 z3 z; ^- {- I! S: k
24. unsigned long previousMillis = 0;
    
25. const long interval = 1;
    $ t0 B& Z6 d- t
26. 
    
27. void setup(){
    
28.     lightsensor_6.dWrite1(1);
    $ w/ Y$ w$ ]; |$ L- I$ F: ?, X
29.     nPower = 160;
    / Y2 D# U! J. k, {6 f
30.     Error=0;
    / G" Z. d5 G/ H: R
31.     ErrorAcc=0;
    ! T+ G3 H9 H. {) P# g( {% n* `( T. ?+ H
32. }
      \, N% A% }( o+ I
33. 
    1 ~9 L3 p4 L5 s$ F4 W1 ^
34. void loop(){
    
35.   unsigned long StartTime = millis();
    % e) \4 e8 H+ S3 u  \9 {
36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    / D7 k5 F3 i  J
37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    2 M4 s9 _- P: L2 D5 }
38.     ErrorAcc +=  nError*Kd ;
    6 B, F# X! T# K. ]2 A/ j5 w2 Q1 {
39.     ErrorDec -=  nError*Ki ;
    
40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    
41.     if(nError < 80 && nError > -80){
    2 A+ n) i2 f0 T
42.       if(nPower < MaxPower){
    : X/ Z, ?& W/ A3 w
43.         nPower += 3;
    
44.       }
    
45.     } else{
    
46.       if(nPower > MinPower){
    6 S, h7 a* N* ^( Y  L3 J3 w
47.         nPower -= 2;
    
48.       }
    7 m) s0 E/ w! x
49.     }
    * _3 C& A/ O& V  p) }3 s
50.     MotoL(nPower-Error);
    , O1 A" T" q3 m% p5 D  P
51.     MotoR(nPower+Error);   
    
52.   }else{
    
53.     motor_9.run(0);
    
54.     motor_10.run(0);
    
55.   }
    
56.   do{}while(millis() - StartTime < interval);
    
57. }
    
58. 
    
59. void MotoL(int Power){
    
60.   if (Power > MaxSpeed){
    , `  d- _0 {9 `. J2 g& q9 L
61.      Power = MaxSpeed;
    
62.   }
    
63.   if (Power < -MaxSpeed){
    # {+ X; G# C& w( Y: r% N( m$ y
64.      Power = -MaxSpeed;
    
65.   }
    
66.   motor_9.run(Power);
    
67. }  
    ) g) Z& V! A' u2 d# ]" _; I
68. 
    1 C  t- B4 d8 s  _1 S5 S+ F
69. void MotoR(int Power){
    1 F. q$ b! E) N- F$ R5 ?
70.   if (Power > MaxSpeed){
    9 {6 s5 V; s. [$ M& u9 i
71.      Power = MaxSpeed;
    9 b& `- R8 L3 c- a! i
72.   }
    
73.   if (Power < -MaxSpeed){
    
74.      Power = -MaxSpeed;
    
75.   }
    : f! f9 t/ N, Z0 N$ X0 M; {
76.   motor_10.run(Power);
    
77. }  

複製代碼

- C; `- ^3 g" M! [# ?

5 L! O, P5 \) f您好，不知是否能向您請教。
目前和宋修賢老師在處理Ardui Car
8 W7 B4 m! V( K1 D: R雖然已使用較繁雜的方式處理了跑出黑線外的狀況
) }& T" s  E4 p" D( C
但基於想追求更精簡的程式所以還是想請問一下
1 h/ J' }0 v. i% a2 b7 v; N1 F, n) H  J% C就是如果我只以單純寫PID控制時，常遇到CNY70跑出黑線外後便往前衝
不知道您是否願意教我可以如何處理
9 S: ?5 e7 \+ ?) }2 T

以下是我挑出我一般寫純PID控制的Code：

1. int MotoSpeed = 250;
   7 h4 j+ b7 m3 B
2. double CNY70Val = 1000;
   % W) f1 _+ G( S; E5 V
3. int interror = 0;
   + E- v, O  P* V' C
4. int olderror = 0;
   . y9 C8 @- A) z$ C8 }8 I, l9 {
5. double values;
   
6. 
   4 |* G1 R1 b! h% x# }7 [* H0 `
7. 
   
8. void CNY70()
   
9. {
   
10.   valuesRR = analogRead(RR)
    
11.   valuesMR = analogRead(MR);
    ; E" v, q1 K, X7 s6 g+ i0 j, A
12.   valuesMM = analogRead(MM);
    7 g8 d6 [! T6 J& D+ \; i0 a) E  y
13.   valuesML = analogRead(ML);
    
14.   valuesLL = analogRead(LL);
    
15. 
      O5 H. ?9 }+ Q9 d( ?1 w- X% D  T. m
16.   if (valuesRR > CNY70Val)
    ' U7 ^' k/ V! k& r3 t) o+ k
17.     valuesRR = CNY70Val;
    5 G; T6 s+ z' }0 T
18.   if (valuesMR > CNY70Val)
    
19.     valuesMR = CNY70Val;
    7 \4 C7 Q* p" o1 ^
20.   if (valuesMM > CNY70Val)
    
21.     valuesMM = CNY70Val;
    3 D6 _9 [- ^2 l; F- M! L
22.   if (valuesML > CNY70Val)
    : O: ?7 W: S( I. W/ C* ]
23.     valuesML = CNY70Val;
    / g5 H; {+ L6 R
24.   if (valuesLL > CNY70Val)
    / q  k% P9 i& c, ?) R" i9 m+ O3 P
25.     valuesLL = CNY70Val;
    " ]) ?" b% i6 H' }) W) J
26. 
    
27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    7 P' S* Q/ `' ?3 X- w+ s
28. }
    3 J9 o& J8 g- N. F. Q' Z
29. 
    
30. void Car()
    0 t# N8 L, G, z; H% j
31. {
    
32.   while (1) {
    
33.     CNY70();
    
34. 
    2 O' t5 _" ?) w
35.     int error = ((int)values);
    
36.     interror += error;
    7 I% O. F: V1 O2 W9 L( |0 d
37.     int lasterror = error - olderror;
    : K4 Y& E4 v2 T  F5 u4 x
38.     olderror = error;
    : U7 e( ~5 w! ~: G% c, m
39.     int power = error / 5 + interror / 10000 + lasterror;
    
40. 
    ; H- q7 [, k" d  r7 G
41.     if (power > MotoSpeed)
    
42.       power = MotoSpeed;
    
43.     if (power < -MotoSpeed)
    ' i% z' W5 b$ y( t4 d* C: M7 V" H" `8 V
44.       power = -MotoSpeed;
      u* z' \, s( Z) T" ]% X
45. 
    
46.     if (power > 0)
    . ^& T& f2 C* f9 w* Y5 j
47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作，正數正轉 負數反轉。
    + D5 I- Z* R. H1 w! c+ P/ Y! D! h
48.     else
    
49.       Speed(MotoSpeed + power, MotoSpeed);
    , n. O' s. p0 K7 Q5 s; s
50.   }
    
51. }

複製代碼

, o( k& D; ?# o$ D0 \  G

感謝分享

感謝分享