PID 循跡自走車範例

1. #include <Arduino.h>
   
2. #include <Wire.h>
   
3. #include <Servo.h>
   
4. 
   
5. #include "MePort.h"
   $ A& O' B' G0 \4 a( x, d" Q
6. #include "MeUltrasonic.h"
   
7. #include "MeDCMotor.h"
   
8. 
   - }) ~, a/ ]7 k! S
9. //double Input, Output ;
   
10. float MaxSpeed = 255;
    . u. u+ V) I+ G1 Z; I& I0 q. ^4 g4 C
11. float MaxPower = 180;
    2 j. @. Q8 g) Q2 g
12. float MinPower = 120;
    * S% c+ g) s' p" G3 D
13. float Error,ErrorAcc,ErrorDec;
    
14. 
    6 }. n  q4 |) G9 T% j( f3 `
15. float Kp=0.14;
    
16. float Kd=0.00020;//23;
    9 L( g8 N5 j6 R( i2 f& W
17. float Ki=0.000201;
    ! {# i: d, M0 C, P' h3 \
18. 
    
19. float nPower;
    
20. MePort lightsensor_6(6);
    5 {1 C) j/ c& M' ^  L
21. MePort lightsensor_8(8);
    
22. MeDCMotor motor_9(9);
    4 J* Y: E1 y/ J
23. MeDCMotor motor_10(10);
    : p/ a3 ?; r4 R+ B/ J( {
24. unsigned long previousMillis = 0;
    
25. const long interval = 1;
    
26. 
    
27. void setup(){
    6 k5 z( N8 }! t3 r1 B
28.     lightsensor_6.dWrite1(1);
    
29.     nPower = 160;
    , B6 [( z6 r4 M9 B
30.     Error=0;
    0 R0 x! g8 X6 w) e; ^
31.     ErrorAcc=0;
    : {4 j% f- ~" M2 G) z
32. }
    $ J' m/ v& ?- F; s) F0 ?2 c7 O5 q
33. 
    9 z' d% y/ x2 c' I; }1 Z) j5 ~
34. void loop(){
    * i( q" J) |1 m) x  w
35.   unsigned long StartTime = millis();
    
36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    5 M8 z; S7 D% I4 w* M. `
37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    % A! {" n# u) O( E& i9 Q. G
38.     ErrorAcc +=  nError*Kd ;
    % k0 G& a: a2 ^# V! M9 ?2 I7 u5 D; @
39.     ErrorDec -=  nError*Ki ;
    
40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    
41.     if(nError < 80 && nError > -80){
    ; ^9 D6 [1 H+ }* W
42.       if(nPower < MaxPower){
    & |( f7 }1 a# x
43.         nPower += 3;
    
44.       }
    
45.     } else{
    ( b" l( O) ]1 a$ m* j( ^+ w
46.       if(nPower > MinPower){
    
47.         nPower -= 2;
    
48.       }
    ! a! w' C+ F! V
49.     }
    
50.     MotoL(nPower-Error);
    & {) h& ^- w7 U8 m& }% _! a: D
51.     MotoR(nPower+Error);   
    
52.   }else{
    
53.     motor_9.run(0);
    2 p8 _6 u' I# u7 _3 c/ n
54.     motor_10.run(0);
    
55.   }
    5 U* @% ^* ~+ E" ~( @
56.   do{}while(millis() - StartTime < interval);
    8 R% g$ P  b3 {1 B) _& t
57. }
    
58. 
    
59. void MotoL(int Power){
    
60.   if (Power > MaxSpeed){
    
61.      Power = MaxSpeed;
    
62.   }
    & \* n, P; ~" Z4 I
63.   if (Power < -MaxSpeed){
    * t" C8 C. M% S' S: ?' D0 p
64.      Power = -MaxSpeed;
    3 i, d# p9 _. U
65.   }
    
66.   motor_9.run(Power);
    : m. s, H1 O- s, I& n+ G$ m
67. }  
    
68. 
    & M/ W$ ?! _) f5 ^) N6 i( d, y
69. void MotoR(int Power){
    3 A0 C; _: x  I: d: O
70.   if (Power > MaxSpeed){
    * K* x- |. P  G0 Y- y( f
71.      Power = MaxSpeed;
    
72.   }
    
73.   if (Power < -MaxSpeed){
    0 v! g9 R8 r. x4 \, k" t
74.      Power = -MaxSpeed;
    
75.   }
    
76.   motor_10.run(Power);
    
77. }  

複製代碼

9 x! J' E5 D2 N( s

0 ~5 W6 O! j6 y1 ~2 `+ b您好，不知是否能向您請教。
* y# w9 s( y. B) S目前和宋修賢老師在處理Ardui Car
+ j+ M. S4 h* U- y雖然已使用較繁雜的方式處理了跑出黑線外的狀況
% _( V2 c* p8 ^4 p. R1 p
, g# E0 K; M/ M8 S7 G  M8 w& D/ h, b但基於想追求更精簡的程式所以還是想請問一下
. n! q! d3 E4 W3 a1 c就是如果我只以單純寫PID控制時，常遇到CNY70跑出黑線外後便往前衝
: y: ]1 P4 d/ R! |/ a6 ^; A9 E/ L不知道您是否願意教我可以如何處理

; J  q# w. S9 n5 u4 c, e
8 R7 X; g. C* T, z+ @* h, H% r以下是我挑出我一般寫純PID控制的Code：

1. int MotoSpeed = 250;
   : r. j3 K3 ^3 z9 N2 ~7 T: ?; b* }3 b
2. double CNY70Val = 1000;
   
3. int interror = 0;
   8 X8 J, C+ m8 K; u+ M8 I' d% N
4. int olderror = 0;
   
5. double values;
   
6. 
   6 p& Z6 ]9 g- K9 W3 t) y
7. 
   ( `' x  i( H% {- u1 f: P0 ]7 c) R" E
8. void CNY70()
   * Z& k' R3 ~4 g
9. {
   
10.   valuesRR = analogRead(RR)
    
11.   valuesMR = analogRead(MR);
    
12.   valuesMM = analogRead(MM);
    
13.   valuesML = analogRead(ML);
    
14.   valuesLL = analogRead(LL);
    8 s0 {3 y1 T0 Y! H& e  B1 E
15. 
    ( X* c4 s( R  C9 h; }
16.   if (valuesRR > CNY70Val)
    
17.     valuesRR = CNY70Val;
      i# Z5 {$ ^$ `  ^9 k, l( ^9 ^$ Y
18.   if (valuesMR > CNY70Val)
    
19.     valuesMR = CNY70Val;
    4 }$ }3 w2 U5 n  b: O( @
20.   if (valuesMM > CNY70Val)
    
21.     valuesMM = CNY70Val;
    6 y. Z$ i+ C2 I4 s/ ?, v0 ^
22.   if (valuesML > CNY70Val)
    6 ^+ s7 G3 I9 @$ x- x  z% j- o
23.     valuesML = CNY70Val;
    6 T3 z3 U9 R+ o
24.   if (valuesLL > CNY70Val)
    . _$ O% y; I/ J" j- e' C& }! O6 W
25.     valuesLL = CNY70Val;
    
26. 
    
27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    : d2 g( ?9 _4 E7 n9 I% U
28. }
    4 e. k2 k9 X% I  n& U) }
29. 
    
30. void Car()
    
31. {
    
32.   while (1) {
    
33.     CNY70();
    
34. 
    
35.     int error = ((int)values);
    + K  s$ L+ T$ p7 O8 i5 m3 j
36.     interror += error;
    
37.     int lasterror = error - olderror;
    1 N5 y' V* B  o1 [
38.     olderror = error;
    5 P. Z% ?& @1 G: [5 x) T, S
39.     int power = error / 5 + interror / 10000 + lasterror;
    7 z  y6 D+ [+ d& `0 s. P% @( Z
40. 
    
41.     if (power > MotoSpeed)
    
42.       power = MotoSpeed;
    
43.     if (power < -MotoSpeed)
    
44.       power = -MotoSpeed;
      z; r0 C3 U3 Q5 W, L7 o% g
45. 
    
46.     if (power > 0)
    
47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作，正數正轉 負數反轉。
    
48.     else
    
49.       Speed(MotoSpeed + power, MotoSpeed);
    
50.   }
    
51. }

複製代碼

% h  b, E! V9 U* M/ ?, z8 v. _
3 A1 m# W/ V8 M# c

感謝分享

感謝分享