PID 循跡自走車範例

1. #include <Arduino.h>
   
2. #include <Wire.h>
   
3. #include <Servo.h>
     ^- g3 @; i) S1 K1 ~. L
4. 
   4 ]* L5 w' l1 V3 [0 x
5. #include "MePort.h"
   
6. #include "MeUltrasonic.h"
   6 }4 H4 m- F  g$ h, U7 ?  k" m
7. #include "MeDCMotor.h"
   2 W/ c  T8 \  l) I0 k2 v/ H
8. 
   9 m: n, O, G' r0 C3 D! n' \6 }
9. //double Input, Output ;
   3 w" }2 k* p5 l& Z7 o; F. e# a: `4 R
10. float MaxSpeed = 255;
    
11. float MaxPower = 180;
    * @5 h0 a' ~9 T' ^
12. float MinPower = 120;
    ( e9 l. C, T- \4 r' n/ _% ]8 M2 e
13. float Error,ErrorAcc,ErrorDec;
    
14. 
    
15. float Kp=0.14;
    2 h$ V7 A9 i# ^
16. float Kd=0.00020;//23;
    " l' o2 |7 i6 Y! ^) g8 n
17. float Ki=0.000201;
    
18. 
    9 o$ S- [2 S' u: [4 n: ~
19. float nPower;
    * F5 O1 `/ x" o8 f) d/ J
20. MePort lightsensor_6(6);
    7 ^: X, {$ m, d+ S( e& o1 i5 G
21. MePort lightsensor_8(8);
    $ ~& Z/ ~9 ~2 V5 x3 J
22. MeDCMotor motor_9(9);
    ) F2 ^5 ~. M/ C" ]
23. MeDCMotor motor_10(10);
    1 ]* ~( U5 s2 p! {" K% h
24. unsigned long previousMillis = 0;
    
25. const long interval = 1;
    ! G4 J5 ]" y5 Y. ?" l. {0 T
26. 
    
27. void setup(){
    4 ~% s9 p% C" L! ^# k8 T9 E
28.     lightsensor_6.dWrite1(1);
    
29.     nPower = 160;
    4 `# s/ _; ]0 A
30.     Error=0;
    
31.     ErrorAcc=0;
    
32. }
    
33. 
    
34. void loop(){
    
35.   unsigned long StartTime = millis();
    9 p' }" n& A+ Z1 P6 _. V
36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    
37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    % _  A' i! I/ C# Q. L( V
38.     ErrorAcc +=  nError*Kd ;
    / |# b- u3 _0 |" x- V4 H
39.     ErrorDec -=  nError*Ki ;
    
40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    . f  x. Z$ y  ]2 [; A
41.     if(nError < 80 && nError > -80){
    
42.       if(nPower < MaxPower){
    
43.         nPower += 3;
      R/ l) M/ q' {0 [5 p$ Q0 f
44.       }
    
45.     } else{
    
46.       if(nPower > MinPower){
    
47.         nPower -= 2;
    ) L- \" G; ?7 C& A. U
48.       }
    
49.     }
    + R1 Y' R8 o: V! M( i  l
50.     MotoL(nPower-Error);
    $ {  C+ v) m  }* ]: V: V: |- z
51.     MotoR(nPower+Error);   
    6 L2 n3 p* W& B; H9 X6 ^
52.   }else{
    + k: j. U. M1 @
53.     motor_9.run(0);
    
54.     motor_10.run(0);
    
55.   }
    
56.   do{}while(millis() - StartTime < interval);
    3 o9 x. N# G0 k6 t6 Q! Z' @$ d) P
57. }
    * P4 B/ [; l; W4 I( K
58. 
    
59. void MotoL(int Power){
    " n8 D; F% e. j3 U
60.   if (Power > MaxSpeed){
    9 s! d& d4 c+ C* `
61.      Power = MaxSpeed;
    
62.   }
    
63.   if (Power < -MaxSpeed){
    9 ]6 ~! @0 T' {
64.      Power = -MaxSpeed;
    
65.   }
    1 h* V' o8 }+ t7 q. }2 e
66.   motor_9.run(Power);
    
67. }  
    
68. 
    
69. void MotoR(int Power){
    
70.   if (Power > MaxSpeed){
    
71.      Power = MaxSpeed;
    ! M- J- r4 i6 `3 c" m9 R
72.   }
    
73.   if (Power < -MaxSpeed){
    1 K3 S8 V" P, }
74.      Power = -MaxSpeed;
    . i  w& f1 D( }- u% m4 v/ j  i
75.   }
    8 `! H9 P8 @0 e
76.   motor_10.run(Power);
    
77. }  

複製代碼

; a5 Y5 c7 O0 K* ~! r4 }
% T3 r% H! G- h! g7 g2 T( z1 n您好，不知是否能向您請教。
目前和宋修賢老師在處理Ardui Car
- f2 E! J4 \( z" b雖然已使用較繁雜的方式處理了跑出黑線外的狀況
$ _* d: l9 I- ?- q7 |
) Q  S3 }% z4 S' i7 O* m但基於想追求更精簡的程式所以還是想請問一下
就是如果我只以單純寫PID控制時，常遇到CNY70跑出黑線外後便往前衝
不知道您是否願意教我可以如何處理
; O; [% |" d6 D5 {( k, `& x/ J
( c0 S- g( b  y
以下是我挑出我一般寫純PID控制的Code：

1. int MotoSpeed = 250;
   
2. double CNY70Val = 1000;
   + `' i5 ?( f9 Q3 E* R
3. int interror = 0;
   ' Q% e- C- t0 F& z6 J( c
4. int olderror = 0;
   2 n1 U! w$ C2 T& J* ~7 v) l  ~
5. double values;
   
6. 
   5 f- [& f* V0 e- S, C: _5 }, R
7. 
   
8. void CNY70()
   / Z8 L+ q8 ^1 G! ?
9. {
   , Q$ ]1 [/ b! g( W
10.   valuesRR = analogRead(RR)
    
11.   valuesMR = analogRead(MR);
    
12.   valuesMM = analogRead(MM);
    + j1 Q$ A) f$ u  N3 a% t# U
13.   valuesML = analogRead(ML);
      h# s$ P  m/ d4 W2 c0 p
14.   valuesLL = analogRead(LL);
    ' V, d+ K/ ~3 n& h% o- e
15. 
    
16.   if (valuesRR > CNY70Val)
    1 l' c1 w2 D1 c, k
17.     valuesRR = CNY70Val;
    
18.   if (valuesMR > CNY70Val)
    
19.     valuesMR = CNY70Val;
    
20.   if (valuesMM > CNY70Val)
    
21.     valuesMM = CNY70Val;
    . s7 c% E5 U4 R/ K# o* L
22.   if (valuesML > CNY70Val)
    % L" @9 w( g0 M1 I) \: L5 _( Y
23.     valuesML = CNY70Val;
    
24.   if (valuesLL > CNY70Val)
    ) a- A, x$ i5 Q- u2 b; v* P9 n6 j
25.     valuesLL = CNY70Val;
    2 |( `# d" c9 X+ C
26. 
    0 S! {# U8 I( L8 K0 L
27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    9 y, }" Y& s/ `% k
28. }
      b% Z/ Z& K& u; D# y
29. 
    
30. void Car()
    
31. {
    - f% s8 q4 b- N
32.   while (1) {
    1 P$ z5 Q& z. u7 t& N8 d
33.     CNY70();
    
34. 
    1 v, E- R3 e8 {% E
35.     int error = ((int)values);
    ( |3 |0 `2 ~7 j/ |' M
36.     interror += error;
    
37.     int lasterror = error - olderror;
    
38.     olderror = error;
    
39.     int power = error / 5 + interror / 10000 + lasterror;
    
40. 
    " A- G7 i; r( [! \. L% ~. p1 R
41.     if (power > MotoSpeed)
    
42.       power = MotoSpeed;
    
43.     if (power < -MotoSpeed)
    
44.       power = -MotoSpeed;
    6 y- y* i6 Z/ u) a5 ]
45. 
    # H1 x" h- e. K
46.     if (power > 0)
    # F! U1 a; c% ^9 t
47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作，正數正轉 負數反轉。
    
48.     else
    
49.       Speed(MotoSpeed + power, MotoSpeed);
    1 t/ s: p, E, U0 S( U: Y
50.   }
    
51. }

複製代碼

感謝分享

感謝分享