PID 循跡自走車範例

1. #include <Arduino.h>
   
2. #include <Wire.h>
   ! H# h& X' v/ j6 ~% ?' E
3. #include <Servo.h>
   
4. 
   
5. #include "MePort.h"
   
6. #include "MeUltrasonic.h"
   
7. #include "MeDCMotor.h"
   
8. 
   % g8 y+ B# K# l0 F  F2 p
9. //double Input, Output ;
   
10. float MaxSpeed = 255;
    
11. float MaxPower = 180;
    
12. float MinPower = 120;
    
13. float Error,ErrorAcc,ErrorDec;
    % ]) _; }$ u; a8 U
14. 
    ) ~! v2 P' o2 `. `: f
15. float Kp=0.14;
    
16. float Kd=0.00020;//23;
    4 V  ~) l+ t2 O5 J$ G) w8 }
17. float Ki=0.000201;
    
18. 
    
19. float nPower;
    / A" }/ i1 a) P3 z  b1 g' o( ?+ G
20. MePort lightsensor_6(6);
    2 ?! y: j4 ~7 _# A1 B' R- u
21. MePort lightsensor_8(8);
    
22. MeDCMotor motor_9(9);
    5 y. N! f& b9 E# ~& L- \) w
23. MeDCMotor motor_10(10);
    
24. unsigned long previousMillis = 0;
    
25. const long interval = 1;
    
26. 
    
27. void setup(){
    
28.     lightsensor_6.dWrite1(1);
    
29.     nPower = 160;
    + E( a1 l7 `# F
30.     Error=0;
    
31.     ErrorAcc=0;
    
32. }
    
33. 
    
34. void loop(){
    + Q: y3 Z0 Q. U& W( w
35.   unsigned long StartTime = millis();
    1 @: d$ }" C2 t8 t
36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    & z. T& t7 O5 {1 l1 D" j, G
37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    % f1 N- \4 v/ V7 x+ n
38.     ErrorAcc +=  nError*Kd ;
    . [7 U5 o1 i8 u# k
39.     ErrorDec -=  nError*Ki ;
    
40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    7 S2 E0 r4 Z* B! P
41.     if(nError < 80 && nError > -80){
    , c( A  G5 ~) @( J. d1 K) N1 a
42.       if(nPower < MaxPower){
    $ N$ U6 n' u- T5 w$ h
43.         nPower += 3;
    
44.       }
    
45.     } else{
    
46.       if(nPower > MinPower){
    
47.         nPower -= 2;
    
48.       }
    
49.     }
    5 J/ J( F. A# i! v; J2 a7 g& {
50.     MotoL(nPower-Error);
    
51.     MotoR(nPower+Error);   
    " N& B0 T2 b0 Y& U, x2 Z; l
52.   }else{
    
53.     motor_9.run(0);
    
54.     motor_10.run(0);
    . b) _$ i* B$ L7 E! S; _6 J7 c
55.   }
    1 q' S1 t. }4 I9 E  w' r
56.   do{}while(millis() - StartTime < interval);
    
57. }
    
58. 
    3 |' n" i/ i+ Y2 F
59. void MotoL(int Power){
    + _8 \) v0 I8 Y* \- Z
60.   if (Power > MaxSpeed){
    
61.      Power = MaxSpeed;
    5 n5 w5 C+ M3 r7 s# p* N- O
62.   }
    8 s$ Q: r  d0 h6 x2 A
63.   if (Power < -MaxSpeed){
    
64.      Power = -MaxSpeed;
    # C( m" }  m+ ]6 V
65.   }
    
66.   motor_9.run(Power);
    $ N0 W' U) l  t' \3 k0 J
67. }  
    
68. 
    2 r( Q  G7 W/ A4 J8 ~9 K; c
69. void MotoR(int Power){
    
70.   if (Power > MaxSpeed){
    
71.      Power = MaxSpeed;
    , k* T4 E) |9 S1 X7 Z; {
72.   }
    : k& U9 p* h" j) I# l. k
73.   if (Power < -MaxSpeed){
    & G; U2 g9 D+ l) I! J1 M& f  p4 A) Z
74.      Power = -MaxSpeed;
    
75.   }
    ; p3 ]3 K+ D( }" {
76.   motor_10.run(Power);
    
77. }  

複製代碼

3 ]* N6 T' P0 x0 }+ E
您好，不知是否能向您請教。
目前和宋修賢老師在處理Ardui Car
雖然已使用較繁雜的方式處理了跑出黑線外的狀況

* _( o$ a. D) J- a" X2 B9 U但基於想追求更精簡的程式所以還是想請問一下
就是如果我只以單純寫PID控制時，常遇到CNY70跑出黑線外後便往前衝
不知道您是否願意教我可以如何處理
% Z" n% R4 p3 X# u9 H4 v

6 s; M  a& O( X0 ?/ s. M以下是我挑出我一般寫純PID控制的Code：

1. int MotoSpeed = 250;
   ) b" I; a- T" c+ }# P% b
2. double CNY70Val = 1000;
   
3. int interror = 0;
   
4. int olderror = 0;
   : p. y1 K* q/ U$ D; o7 j- M
5. double values;
   - v  z& G7 |, T) U
6. 
   
7. 
   7 {' y: B* R5 s8 A8 b* ]
8. void CNY70()
   % q9 {* B9 G! O9 l9 v. y$ ]% G  t! P  ~
9. {
   
10.   valuesRR = analogRead(RR)
    0 R+ k- U; f9 v( j+ M1 b
11.   valuesMR = analogRead(MR);
    ' }' @$ D6 O9 x# ~# D, E( ?5 X
12.   valuesMM = analogRead(MM);
    + o* g" U2 F/ y' }5 p& p( ^
13.   valuesML = analogRead(ML);
    , p% E5 Q8 Y; B" L0 f" J! W
14.   valuesLL = analogRead(LL);
      H0 C: f  Y/ G
15. 
    7 B, t* o. T! ~: x: Q
16.   if (valuesRR > CNY70Val)
    
17.     valuesRR = CNY70Val;
    
18.   if (valuesMR > CNY70Val)
    
19.     valuesMR = CNY70Val;
    
20.   if (valuesMM > CNY70Val)
    
21.     valuesMM = CNY70Val;
    ' C% n" O: I  f  d) I* i
22.   if (valuesML > CNY70Val)
    
23.     valuesML = CNY70Val;
    
24.   if (valuesLL > CNY70Val)
    9 X0 w& P2 k+ [- I9 M
25.     valuesLL = CNY70Val;
    7 `2 M7 e' r1 Y" x
26. 
    * c9 ~; M' K$ q* V9 V/ r
27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    
28. }
    : z1 L" j% ]2 I; t5 V. H
29. 
    
30. void Car()
    - ~' @, V/ i0 c
31. {
    
32.   while (1) {
    / \6 O+ |+ l8 M! ]( f9 E5 m
33.     CNY70();
    
34. 
    : G: p- Q& h' a7 Z" a, _# T+ ^
35.     int error = ((int)values);
    ) n4 Q  A2 d" |2 s0 P- Y# \+ l
36.     interror += error;
    1 v( t5 k" ^8 x4 t  b9 Z
37.     int lasterror = error - olderror;
    
38.     olderror = error;
    
39.     int power = error / 5 + interror / 10000 + lasterror;
    
40. 
    9 T/ J: o' d0 A; q6 \
41.     if (power > MotoSpeed)
    
42.       power = MotoSpeed;
    
43.     if (power < -MotoSpeed)
    
44.       power = -MotoSpeed;
    
45. 
    + n" Q- Y" `+ F( w
46.     if (power > 0)
    
47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作，正數正轉 負數反轉。
    
48.     else
    8 ^8 \3 i6 Z, M6 e
49.       Speed(MotoSpeed + power, MotoSpeed);
    
50.   }
    * V' V! j* E7 I6 v6 `  k2 i
51. }

複製代碼

) w7 P4 i6 ~4 k: X6 U+ }

感謝分享

感謝分享