PID 循跡自走車範例

1. #include <Arduino.h>
   
2. #include <Wire.h>
     s! Z1 W# [7 w4 S9 U: s
3. #include <Servo.h>
   
4. 
   ' C3 |% v9 r9 @2 t  U
5. #include "MePort.h"
   / ?# c1 h$ L6 t: \  I6 ~. `
6. #include "MeUltrasonic.h"
   : G; j+ {' `- M: E% ]
7. #include "MeDCMotor.h"
   + d6 F9 i& }5 Y6 p% u; W* k
8. 
     s9 P! q  r. [4 Y# T3 s' E
9. //double Input, Output ;
   
10. float MaxSpeed = 255;
    
11. float MaxPower = 180;
    4 z/ [% I) m# ?% P( S. i- B7 I& E
12. float MinPower = 120;
    9 @: y8 ]' s/ v, D4 D; u  J
13. float Error,ErrorAcc,ErrorDec;
    
14. 
    
15. float Kp=0.14;
    # _5 \; O% I4 [8 r5 f. X, F
16. float Kd=0.00020;//23;
    9 F; o1 ]! r6 K, ]1 A0 p+ r
17. float Ki=0.000201;
    
18. 
    
19. float nPower;
    
20. MePort lightsensor_6(6);
    # t/ O6 j6 K/ f7 R8 H  B2 F
21. MePort lightsensor_8(8);
    ' G) V8 f/ T! N. p& g) f
22. MeDCMotor motor_9(9);
    " C0 [% c( m7 l6 W/ H
23. MeDCMotor motor_10(10);
    
24. unsigned long previousMillis = 0;
      ?! w# O- e* w# J
25. const long interval = 1;
    
26. 
    
27. void setup(){
    
28.     lightsensor_6.dWrite1(1);
      I% k1 |9 O+ g! g* ~
29.     nPower = 160;
    0 q/ X3 w" F  }2 c7 z
30.     Error=0;
    
31.     ErrorAcc=0;
    
32. }
    * m: j0 K3 a$ ~' \, r
33. 
    $ u3 X) X5 U% m
34. void loop(){
    
35.   unsigned long StartTime = millis();
    
36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    % N2 h8 l) t/ p  [
37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    0 a2 i5 p6 k2 _' e. e: H
38.     ErrorAcc +=  nError*Kd ;
    3 x! w- L6 P- M1 e3 ~! l; I3 A" p
39.     ErrorDec -=  nError*Ki ;
    
40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
      Z4 r4 [# X% K7 T, G8 O8 R5 r- k
41.     if(nError < 80 && nError > -80){
      {- i# B( [% m4 M
42.       if(nPower < MaxPower){
    ) x6 r+ x2 S6 L: A; q' Y- t1 J* P
43.         nPower += 3;
    
44.       }
    
45.     } else{
    
46.       if(nPower > MinPower){
    ( x# r3 z/ m  Y
47.         nPower -= 2;
    ( C* s' v9 {: m" C7 D
48.       }
    
49.     }
    
50.     MotoL(nPower-Error);
    * ~, o+ r7 S* _! b( ?/ y! \  w
51.     MotoR(nPower+Error);   
    
52.   }else{
    
53.     motor_9.run(0);
    
54.     motor_10.run(0);
    
55.   }
    
56.   do{}while(millis() - StartTime < interval);
    
57. }
    : r1 F9 S0 N( Q7 B1 U+ k4 f
58. 
    
59. void MotoL(int Power){
    9 v* q; ^7 t( s2 b% U; N) B
60.   if (Power > MaxSpeed){
    . n4 g+ Q2 G' b+ B# x
61.      Power = MaxSpeed;
      T! h% k% _! q& s
62.   }
    
63.   if (Power < -MaxSpeed){
    
64.      Power = -MaxSpeed;
    4 Z% ?6 s, `6 L  Y( [" Q
65.   }
    + f1 x+ t5 |# _1 F4 v! A
66.   motor_9.run(Power);
    
67. }  
    # t6 K0 P+ y" f& F/ W
68. 
    
69. void MotoR(int Power){
    ! t9 z/ a% d1 g' Q2 A' j" {& i
70.   if (Power > MaxSpeed){
    
71.      Power = MaxSpeed;
    
72.   }
    & b7 y0 ]: s" D2 o4 \
73.   if (Power < -MaxSpeed){
    3 h8 s9 t5 o) z, d7 O; T
74.      Power = -MaxSpeed;
    
75.   }
    
76.   motor_10.run(Power);
    
77. }  

複製代碼

( i) [1 {; M6 H+ a& l

, r9 U4 I& T; E您好，不知是否能向您請教。
目前和宋修賢老師在處理Ardui Car
雖然已使用較繁雜的方式處理了跑出黑線外的狀況
7 `* o3 g6 s6 r  y% L2 y3 W) E
但基於想追求更精簡的程式所以還是想請問一下
0 l1 H' i0 O2 B3 {% v就是如果我只以單純寫PID控制時，常遇到CNY70跑出黑線外後便往前衝
不知道您是否願意教我可以如何處理

2 s5 p7 S& q  R6 }. I; I4 D
1 X# W  E  T, f4 j4 u- `以下是我挑出我一般寫純PID控制的Code：

1. int MotoSpeed = 250;
   
2. double CNY70Val = 1000;
   ) R+ ?/ W8 F7 f1 e
3. int interror = 0;
   
4. int olderror = 0;
   
5. double values;
   
6. 
   0 O( E& J0 I9 ~
7. 
   
8. void CNY70()
   
9. {
   : k1 z. j$ a) e+ G  [, k# s0 P
10.   valuesRR = analogRead(RR)
    # a4 U' O% V: d% o/ r! f& [4 O
11.   valuesMR = analogRead(MR);
    
12.   valuesMM = analogRead(MM);
    : z$ ], s3 t$ `: l1 o. N
13.   valuesML = analogRead(ML);
    / a, d: c  o# k) v$ n
14.   valuesLL = analogRead(LL);
    0 M1 X  w' ^$ X$ M  V
15. 
    
16.   if (valuesRR > CNY70Val)
    ; R& z( q6 \' `" q, G
17.     valuesRR = CNY70Val;
    
18.   if (valuesMR > CNY70Val)
    1 r7 n' c& n" z# F
19.     valuesMR = CNY70Val;
    ( t8 F% w+ h7 c- m
20.   if (valuesMM > CNY70Val)
    , n$ `+ }: k* z4 v- M& _
21.     valuesMM = CNY70Val;
    
22.   if (valuesML > CNY70Val)
    - C: e$ `5 D) ]5 ~+ A
23.     valuesML = CNY70Val;
    5 E/ r/ F2 T- s* a2 y
24.   if (valuesLL > CNY70Val)
    
25.     valuesLL = CNY70Val;
    
26. 
    
27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    
28. }
    
29. 
    
30. void Car()
    7 g. V1 }* g" h+ A& h- n4 Y8 `8 x
31. {
    
32.   while (1) {
    
33.     CNY70();
    
34. 
    
35.     int error = ((int)values);
    # M0 |1 Q4 H. k5 F" W
36.     interror += error;
    ; s: b: w5 A8 W0 v' J
37.     int lasterror = error - olderror;
    - R; U1 w- H' t
38.     olderror = error;
    ( X+ @8 F3 e) I3 Y( f& B
39.     int power = error / 5 + interror / 10000 + lasterror;
    
40. 
    8 o' j. g) u3 T. n% H
41.     if (power > MotoSpeed)
    5 T2 D$ h3 I4 H" P+ G
42.       power = MotoSpeed;
    1 V) X, @+ a2 R- Z* z/ V
43.     if (power < -MotoSpeed)
    
44.       power = -MotoSpeed;
    . ~6 M* U5 }$ Q8 x* l6 y
45. 
    
46.     if (power > 0)
    
47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作，正數正轉 負數反轉。
    
48.     else
    
49.       Speed(MotoSpeed + power, MotoSpeed);
    
50.   }
    
51. }

複製代碼

( l. k# ~+ l8 m/ j1 k
+ a' e; C1 Z3 I

感謝分享

感謝分享