PID 循跡自走車範例

1. #include <Arduino.h>
   
2. #include <Wire.h>
   5 I; ^0 r' r, a2 a& `( c" p
3. #include <Servo.h>
   
4. 
   
5. #include "MePort.h"
   . V# m8 F) r) K# z6 j/ n
6. #include "MeUltrasonic.h"
   1 O% h. h' P9 y
7. #include "MeDCMotor.h"
   : }, G' r, M1 _+ \* p9 x& [, E
8. 
   
9. //double Input, Output ;
   + n$ g1 S: y. Q$ {% n$ L
10. float MaxSpeed = 255;
    , p) W! c$ z+ v; I/ j  q
11. float MaxPower = 180;
    
12. float MinPower = 120;
    
13. float Error,ErrorAcc,ErrorDec;
    ' p! P% y( K7 c' U
14. 
    . l, d7 g, [& w8 s2 f. w( m8 v
15. float Kp=0.14;
    3 ]1 F: c7 H) D6 I( E
16. float Kd=0.00020;//23;
    / ^$ [- F% ^+ y' ?: R
17. float Ki=0.000201;
    7 q5 Z* z; }& S8 h7 y+ S
18. 
    
19. float nPower;
    
20. MePort lightsensor_6(6);
    + E% b' ^8 ?- a* i+ j. U  N
21. MePort lightsensor_8(8);
    
22. MeDCMotor motor_9(9);
    $ ?$ S) V8 p4 K/ `% w
23. MeDCMotor motor_10(10);
    0 `& v2 C4 J' o! G/ L
24. unsigned long previousMillis = 0;
    * }. a1 O6 s9 ~2 \
25. const long interval = 1;
    
26. 
    - v, X6 y; x7 F% \9 R
27. void setup(){
    1 N! C3 \  ], T$ F# S% t7 `
28.     lightsensor_6.dWrite1(1);
    * K0 W1 c/ f, T$ i: Y0 l
29.     nPower = 160;
    , w1 o$ z1 Z5 }2 Z
30.     Error=0;
    1 j% y+ Z% e% K6 H8 x" k+ ^
31.     ErrorAcc=0;
    
32. }
    ( q" F- \! I- {: P! K
33. 
    , ^- g5 I! N! n! H
34. void loop(){
    
35.   unsigned long StartTime = millis();
    
36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    
37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    
38.     ErrorAcc +=  nError*Kd ;
    . \5 P6 g* a% `8 j: n
39.     ErrorDec -=  nError*Ki ;
    + X; y: C7 x3 R1 \0 {4 l4 s9 \
40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    
41.     if(nError < 80 && nError > -80){
    
42.       if(nPower < MaxPower){
    + q; }2 p9 K5 }  J  |' A
43.         nPower += 3;
    $ @, p9 M0 I  x) _0 y3 J3 Y
44.       }
    
45.     } else{
    
46.       if(nPower > MinPower){
    : b  |5 G. q- p, g
47.         nPower -= 2;
    
48.       }
    
49.     }
    8 O& ]4 U; z% f, _# W; i6 |
50.     MotoL(nPower-Error);
    / d2 p6 t+ o4 L' e  H/ j8 e
51.     MotoR(nPower+Error);   
    $ {$ p" p: i% A% |
52.   }else{
    6 ]* \. ?. u& t( [7 e, G
53.     motor_9.run(0);
    1 M& |! T* Z5 q# y" p
54.     motor_10.run(0);
    
55.   }
    
56.   do{}while(millis() - StartTime < interval);
    ( P) A6 T: K. [4 P
57. }
    
58. 
    6 M+ J9 Z! J. U7 S; c
59. void MotoL(int Power){
    
60.   if (Power > MaxSpeed){
    
61.      Power = MaxSpeed;
    
62.   }
    9 s, _. M$ K2 s  w3 x4 T
63.   if (Power < -MaxSpeed){
    
64.      Power = -MaxSpeed;
    
65.   }
    
66.   motor_9.run(Power);
    
67. }  
    
68. 
    
69. void MotoR(int Power){
    
70.   if (Power > MaxSpeed){
    5 F5 t# z0 [- P) ]
71.      Power = MaxSpeed;
    ( `- ?( {5 I: N; H1 w
72.   }
    
73.   if (Power < -MaxSpeed){
    
74.      Power = -MaxSpeed;
    
75.   }
    / X! l8 ^6 s4 J! ~# H
76.   motor_10.run(Power);
    ) ?0 K3 O2 l. K; X8 j8 ?4 V& R
77. }  

複製代碼

0 ?$ G4 T# ?/ t

您好，不知是否能向您請教。
目前和宋修賢老師在處理Ardui Car
2 {8 p! K" M* i* h9 S雖然已使用較繁雜的方式處理了跑出黑線外的狀況

但基於想追求更精簡的程式所以還是想請問一下
% d! y2 ]% I3 E; N$ B就是如果我只以單純寫PID控制時，常遇到CNY70跑出黑線外後便往前衝
( d) k) V! ]6 l' V0 _$ M不知道您是否願意教我可以如何處理

, @  Z) u5 k2 a( v% ^1 g
# y5 l7 w( `% B! p, U1 {以下是我挑出我一般寫純PID控制的Code：

1. int MotoSpeed = 250;
   
2. double CNY70Val = 1000;
   
3. int interror = 0;
   1 q4 s+ C4 R8 h/ ]4 ?
4. int olderror = 0;
   ; M8 e; o+ \: ~/ w
5. double values;
   6 X4 [& m1 e# ~6 m4 ?3 a
6. 
   & U1 x4 b; @  f4 }) [% U# J" L
7. 
   
8. void CNY70()
   
9. {
   : o& J2 R% P3 X; C
10.   valuesRR = analogRead(RR)
    
11.   valuesMR = analogRead(MR);
    
12.   valuesMM = analogRead(MM);
    
13.   valuesML = analogRead(ML);
    . d0 ~5 k1 s! h% J- e) s* m
14.   valuesLL = analogRead(LL);
    
15. 
    
16.   if (valuesRR > CNY70Val)
    1 o) W3 ?1 P" `1 P
17.     valuesRR = CNY70Val;
    
18.   if (valuesMR > CNY70Val)
    6 \4 P* x" N; Z0 f
19.     valuesMR = CNY70Val;
    
20.   if (valuesMM > CNY70Val)
    $ X6 {1 n1 e# h' O0 p- D+ j$ g
21.     valuesMM = CNY70Val;
    1 G2 V: m7 s* k, Y- u# z6 H* I
22.   if (valuesML > CNY70Val)
    ' E  P, u3 N5 E* u# |% b# D
23.     valuesML = CNY70Val;
    
24.   if (valuesLL > CNY70Val)
    ; T  |. Z7 C! q# b
25.     valuesLL = CNY70Val;
    7 O: E5 h  {/ @: l+ B
26. 
    ' K7 j4 @5 M: ^) @+ b
27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    5 K2 B5 y1 @! @6 w- ~9 b
28. }
    
29. 
    $ l. t4 }; D; j4 H+ Y0 m
30. void Car()
    2 A, J0 I& L, k* _1 `. X: n
31. {
    * }# l  ~1 ^" X0 G. U1 W
32.   while (1) {
    
33.     CNY70();
    
34. 
    
35.     int error = ((int)values);
    
36.     interror += error;
    . W# F$ p, v3 a5 ?& E) v
37.     int lasterror = error - olderror;
    
38.     olderror = error;
    
39.     int power = error / 5 + interror / 10000 + lasterror;
    
40. 
    
41.     if (power > MotoSpeed)
    
42.       power = MotoSpeed;
    / K1 g4 E2 R* _  T
43.     if (power < -MotoSpeed)
    : C" w6 n, y) {! a3 s4 E2 F
44.       power = -MotoSpeed;
    1 A# f: y7 a7 @6 d7 w
45. 
    
46.     if (power > 0)
    ( e/ q$ F: c2 n
47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作，正數正轉 負數反轉。
    
48.     else
    
49.       Speed(MotoSpeed + power, MotoSpeed);
    
50.   }
    
51. }

複製代碼

; A9 K  D4 C9 u  e/ I: I" Y

感謝分享

感謝分享