PID 循跡自走車範例

1. #include <Arduino.h>
   0 v1 e" _/ U/ I- @0 l+ x2 w9 K
2. #include <Wire.h>
   
3. #include <Servo.h>
   ' s! }% f. x, m5 y1 m3 I3 R
4. 
   
5. #include "MePort.h"
   % B: P. [6 k3 u5 j" h$ T$ V
6. #include "MeUltrasonic.h"
   
7. #include "MeDCMotor.h"
   
8. 
   
9. //double Input, Output ;
   
10. float MaxSpeed = 255;
    
11. float MaxPower = 180;
    
12. float MinPower = 120;
    
13. float Error,ErrorAcc,ErrorDec;
    . ]0 O8 @8 t, t' ?! y
14. 
    7 p: ~1 D5 Z. l
15. float Kp=0.14;
    
16. float Kd=0.00020;//23;
    
17. float Ki=0.000201;
    8 M) h3 \; M" j! c+ c9 S6 y) h
18. 
    0 l7 r' U- V" w# X& Q4 ?/ G
19. float nPower;
    8 G2 l/ T4 O% [8 M
20. MePort lightsensor_6(6);
    
21. MePort lightsensor_8(8);
    ! r. k) d$ b: u- A
22. MeDCMotor motor_9(9);
    * t7 ~( X, o' T: Y1 Y' v) N! Y
23. MeDCMotor motor_10(10);
    
24. unsigned long previousMillis = 0;
    6 P% W0 x4 l7 ]- x7 w
25. const long interval = 1;
    . G+ ^' P4 m4 {  q* Q' V! ]
26. 
    ! v3 H2 G9 l+ P8 y& T! ~9 I; _
27. void setup(){
    / p8 {# E3 ]3 |2 ]% C7 a0 o9 h
28.     lightsensor_6.dWrite1(1);
    5 q; M0 t/ }5 Y
29.     nPower = 160;
    3 n6 D8 O; K5 ?# C
30.     Error=0;
      {9 B! H& ~. z) d4 L
31.     ErrorAcc=0;
    
32. }
    
33. 
    
34. void loop(){
    
35.   unsigned long StartTime = millis();
    
36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    ) v* m- a  B7 A2 x$ A8 L# }5 K4 U
37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    
38.     ErrorAcc +=  nError*Kd ;
    4 ~. c. I1 @( v6 ^; x% u8 V
39.     ErrorDec -=  nError*Ki ;
    
40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    $ m8 D9 f% e' u; O0 I
41.     if(nError < 80 && nError > -80){
    & v1 t( J2 f2 P+ D/ U. z
42.       if(nPower < MaxPower){
    . Z# k, ~: v# n6 s
43.         nPower += 3;
    
44.       }
    3 m8 c. ^9 R& V$ V% G2 Z; U
45.     } else{
    
46.       if(nPower > MinPower){
    , e6 E4 v, V' [+ O+ K
47.         nPower -= 2;
    
48.       }
    
49.     }
    6 {) f8 n) ]  |" A  U5 T% |
50.     MotoL(nPower-Error);
    0 }. F, N3 Z; v  P  r% R( ^
51.     MotoR(nPower+Error);   
    % L- y) X) {, n! y/ O
52.   }else{
    
53.     motor_9.run(0);
    ; y2 \- P. b  e( y' S
54.     motor_10.run(0);
    
55.   }
    / {2 W' B6 L" U2 S9 n8 Y2 p
56.   do{}while(millis() - StartTime < interval);
    0 ~$ r. R, T9 o9 p" f( J" M$ n- T
57. }
    
58. 
    
59. void MotoL(int Power){
    # m' V7 s( A: i, S1 C
60.   if (Power > MaxSpeed){
    
61.      Power = MaxSpeed;
    
62.   }
    
63.   if (Power < -MaxSpeed){
    % i& `, _9 M) r! D
64.      Power = -MaxSpeed;
    ! C3 R! h! s' r4 L* ]2 [' P
65.   }
      a0 y  {" R8 T
66.   motor_9.run(Power);
    
67. }  
    
68. 
    
69. void MotoR(int Power){
    
70.   if (Power > MaxSpeed){
    ; l/ y* t6 |9 Q3 M. O
71.      Power = MaxSpeed;
    4 A7 l& X! m8 Z. V
72.   }
    + V$ M! \' R: B7 V, {& D! [
73.   if (Power < -MaxSpeed){
    
74.      Power = -MaxSpeed;
    
75.   }
    , P/ y& ?% y8 x& u
76.   motor_10.run(Power);
    & A  N6 B( `: x0 j# U
77. }  

複製代碼

# q( p# N/ R- f5 {( B

4 s! B/ o. Z( }9 p% S: ]2 a3 \
您好，不知是否能向您請教。
目前和宋修賢老師在處理Ardui Car
& X1 `5 n/ {0 l5 c3 S. q雖然已使用較繁雜的方式處理了跑出黑線外的狀況

$ l3 Q) H5 J- E1 K, ?: Y但基於想追求更精簡的程式所以還是想請問一下
就是如果我只以單純寫PID控制時，常遇到CNY70跑出黑線外後便往前衝
; I7 ?! m, \( w% ], ~  @2 H不知道您是否願意教我可以如何處理


+ O1 d* p- j% A+ A1 N; p以下是我挑出我一般寫純PID控制的Code：

1. int MotoSpeed = 250;
   
2. double CNY70Val = 1000;
   3 e: R/ o" H9 D0 j$ O' a1 P# G
3. int interror = 0;
   + j9 _/ |8 {! Z3 p
4. int olderror = 0;
   
5. double values;
   & G/ O0 B  m/ X* ~1 k3 @5 D
6. 
   5 `+ \- Y! k, p$ ]5 Q4 K* s1 q; C
7. 
   
8. void CNY70()
   
9. {
   / V: i3 D. J. p9 T' G$ X
10.   valuesRR = analogRead(RR)
    % A. s) r9 d8 g3 l2 H% K! ~
11.   valuesMR = analogRead(MR);
    ! `; Y9 K4 }! s$ [: T$ F6 N& g4 k
12.   valuesMM = analogRead(MM);
    8 |' Y* Y3 G6 T$ @2 U
13.   valuesML = analogRead(ML);
    
14.   valuesLL = analogRead(LL);
    
15. 
    * Q5 `3 P( t) k3 T' T
16.   if (valuesRR > CNY70Val)
    
17.     valuesRR = CNY70Val;
    
18.   if (valuesMR > CNY70Val)
    ! g9 J- G4 ]  {7 O# D2 j1 h! C# z
19.     valuesMR = CNY70Val;
    
20.   if (valuesMM > CNY70Val)
    
21.     valuesMM = CNY70Val;
    + W% r& C. B" n7 _/ a
22.   if (valuesML > CNY70Val)
    
23.     valuesML = CNY70Val;
    
24.   if (valuesLL > CNY70Val)
    
25.     valuesLL = CNY70Val;
    
26. 
    , |9 ~& Y3 V% a7 p3 M. q% K
27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    / t6 w7 z2 Z& y# H4 p1 w
28. }
    2 o4 X+ l; c- I; ?& \
29. 
    
30. void Car()
    
31. {
    . o4 V, k* i3 f2 H7 v: T
32.   while (1) {
    % v) L/ E2 ?, m; @& t( ?
33.     CNY70();
    
34. 
      z7 Z1 M1 X& ?, \8 ~
35.     int error = ((int)values);
    : x, T; A; G. {
36.     interror += error;
    
37.     int lasterror = error - olderror;
    % a9 f" Z# p) Y
38.     olderror = error;
    8 m; X+ M  a5 F9 g, \9 k1 i
39.     int power = error / 5 + interror / 10000 + lasterror;
    , T. L4 ]$ T5 }8 [
40. 
    , [+ q$ }. J5 y2 Z% I5 O
41.     if (power > MotoSpeed)
    
42.       power = MotoSpeed;
    3 |) W2 S2 O3 A- [3 `  y! s
43.     if (power < -MotoSpeed)
    
44.       power = -MotoSpeed;
    ! G. a/ }) k: h' Q
45. 
    
46.     if (power > 0)
    ; K" D' C6 F: s+ Q: V& a+ h8 c1 ?
47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作，正數正轉 負數反轉。
    
48.     else
    3 u: V1 p, Y" b& A& V3 q
49.       Speed(MotoSpeed + power, MotoSpeed);
      Y1 r: z- }) v9 q# O* _& E5 f
50.   }
    5 f8 I( O2 {" D5 B! c4 v. ]
51. }

複製代碼

  ]' {1 _1 b; V6 c- ^3 t5 }
6 q( e2 e( d: N+ S5 q* O

感謝分享

感謝分享