PID 循跡自走車範例

1. #include <Arduino.h>
   
2. #include <Wire.h>
   
3. #include <Servo.h>
   - ~! P1 ^* j! `7 M+ B! g. V
4. 
   
5. #include "MePort.h"
   
6. #include "MeUltrasonic.h"
   6 I- N1 p% H8 ^  r+ E' [% K. w3 D
7. #include "MeDCMotor.h"
   
8. 
     Y$ ?7 K0 Z# l% k( U
9. //double Input, Output ;
   - P, }3 h) {+ \
10. float MaxSpeed = 255;
    + }8 z4 x* R5 b; z, P
11. float MaxPower = 180;
    2 t+ j" r! w" @+ U$ U8 I8 K# W# k3 L
12. float MinPower = 120;
    
13. float Error,ErrorAcc,ErrorDec;
    + L0 H  ^& V# O- q  \/ |
14. 
    
15. float Kp=0.14;
    # r/ H7 J6 i+ x2 B+ ^
16. float Kd=0.00020;//23;
    6 c5 E) e$ Z6 p9 H; k+ ^8 Q
17. float Ki=0.000201;
    
18. 
    " _7 Z4 ?6 e2 ]- l$ t1 v- C6 v
19. float nPower;
    # {1 C/ c% ]/ N+ [. Q3 o* ^
20. MePort lightsensor_6(6);
    * U7 G  G$ o! e) x9 J
21. MePort lightsensor_8(8);
    
22. MeDCMotor motor_9(9);
    1 z3 P- q- O5 p: @) @
23. MeDCMotor motor_10(10);
    3 p" ^1 X3 k4 L1 a# f
24. unsigned long previousMillis = 0;
    
25. const long interval = 1;
    
26. 
    
27. void setup(){
    
28.     lightsensor_6.dWrite1(1);
    3 @: J: k& G  ?0 {- w1 c) ^
29.     nPower = 160;
    ( x$ ]  r9 R" R9 G* m
30.     Error=0;
    
31.     ErrorAcc=0;
    ( q3 Y3 X  o, [' `
32. }
    + g! I3 A" {$ U9 \  m0 M/ J# D
33. 
    
34. void loop(){
    
35.   unsigned long StartTime = millis();
    " l4 E2 ^' j" V( c0 w
36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    
37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    ; u# d  k' X& J6 u. b
38.     ErrorAcc +=  nError*Kd ;
    
39.     ErrorDec -=  nError*Ki ;
    4 K; q0 `, [" S( s
40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
      b$ y9 Q! u% a; j2 s5 [
41.     if(nError < 80 && nError > -80){
    , r' s, f; U* c/ m% g' T# e* Y
42.       if(nPower < MaxPower){
    
43.         nPower += 3;
    ' R7 m7 N3 E- `- |  y
44.       }
    - q; ~1 B- |4 m1 x4 L, M
45.     } else{
    6 v0 I" ^9 g- E& N  T
46.       if(nPower > MinPower){
    , A: X% R) B+ b3 T/ @
47.         nPower -= 2;
    
48.       }
    : C  b: d+ J% X. D5 R2 E
49.     }
    
50.     MotoL(nPower-Error);
    % @9 B( V0 l4 Q' [  v; ]! h
51.     MotoR(nPower+Error);   
    
52.   }else{
    
53.     motor_9.run(0);
    
54.     motor_10.run(0);
    0 u! O7 f1 ]+ c; Y4 v2 x
55.   }
    - a  B+ B( D) i& S- R
56.   do{}while(millis() - StartTime < interval);
    & V! w/ q& N9 l! j
57. }
    
58. 
    1 g) m4 _6 y. F' F$ g& y
59. void MotoL(int Power){
    7 ]! R( j8 H& e
60.   if (Power > MaxSpeed){
    3 p. f! `' S# l6 _- O  X
61.      Power = MaxSpeed;
    . _" _* d' g) M$ D
62.   }
    ! M: v$ R6 e' g+ u( M6 g
63.   if (Power < -MaxSpeed){
    ( r: S$ A- {) ]) @4 n: @2 f
64.      Power = -MaxSpeed;
    
65.   }
    
66.   motor_9.run(Power);
    
67. }  
    0 P. c% |1 M3 m0 o3 U( J9 K
68. 
    
69. void MotoR(int Power){
    $ B6 R- k8 B* g* a( W
70.   if (Power > MaxSpeed){
    * ~+ }8 Z# T5 j! |9 |6 w4 y9 O8 U
71.      Power = MaxSpeed;
    
72.   }
    
73.   if (Power < -MaxSpeed){
    
74.      Power = -MaxSpeed;
    % g2 w$ Q, [1 f1 z
75.   }
    " v4 P% g2 k" P" S3 M
76.   motor_10.run(Power);
    2 \+ q5 @8 I' ?  N+ a5 e' F6 F: i
77. }  

複製代碼

0 f! G! F) e6 X, Y5 t" A" ?您好，不知是否能向您請教。
6 m+ Q6 l) Q) ]" ?! ]目前和宋修賢老師在處理Ardui Car
雖然已使用較繁雜的方式處理了跑出黑線外的狀況

但基於想追求更精簡的程式所以還是想請問一下
6 N" L3 d: _2 D就是如果我只以單純寫PID控制時，常遇到CNY70跑出黑線外後便往前衝
不知道您是否願意教我可以如何處理
( z' U2 Q# N  {4 `; Q

以下是我挑出我一般寫純PID控制的Code：

1. int MotoSpeed = 250;
   
2. double CNY70Val = 1000;
   ( I5 P1 |# Y, k4 z
3. int interror = 0;
   
4. int olderror = 0;
   
5. double values;
   
6. 
   
7. 
   
8. void CNY70()
   
9. {
   , e* \2 m8 Q% F, m7 l
10.   valuesRR = analogRead(RR)
    
11.   valuesMR = analogRead(MR);
    5 Z2 A1 H8 z6 ~/ x2 N8 c9 Y) n
12.   valuesMM = analogRead(MM);
    ( L% |% `7 @" j5 f) R: }
13.   valuesML = analogRead(ML);
    
14.   valuesLL = analogRead(LL);
    
15. 
    
16.   if (valuesRR > CNY70Val)
    
17.     valuesRR = CNY70Val;
    ( u3 k. c/ j9 X$ O7 v9 f# p9 o
18.   if (valuesMR > CNY70Val)
      R5 u4 ~+ \; j4 L, p+ s1 R3 b* g
19.     valuesMR = CNY70Val;
    5 [- Y1 U% L1 G1 L
20.   if (valuesMM > CNY70Val)
    
21.     valuesMM = CNY70Val;
    
22.   if (valuesML > CNY70Val)
    ( m1 s! N0 K+ @1 i3 C: e, }
23.     valuesML = CNY70Val;
    
24.   if (valuesLL > CNY70Val)
    
25.     valuesLL = CNY70Val;
    " @% t( i1 J4 V' r9 m1 L$ C- h! V5 d
26. 
    . }! F+ K9 h8 [& @" i
27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    
28. }
      F% Y0 |5 z* c3 x: d; Q; x
29. 
    - }* p" D4 G' B: I& Z
30. void Car()
    
31. {
    
32.   while (1) {
    " R' G. B. J& F$ f3 f- f, Z1 t
33.     CNY70();
    
34. 
    3 R* f3 [8 v* x: a4 X( Q
35.     int error = ((int)values);
    9 b& X' y8 A7 ]8 R( c9 b
36.     interror += error;
    
37.     int lasterror = error - olderror;
    1 {" M9 R/ [3 G" X+ W" v
38.     olderror = error;
    
39.     int power = error / 5 + interror / 10000 + lasterror;
    / V+ j' n  {3 ^2 i$ W; Z0 g- Z9 w
40. 
    
41.     if (power > MotoSpeed)
    8 M4 ]1 O" T7 m2 x
42.       power = MotoSpeed;
    
43.     if (power < -MotoSpeed)
    
44.       power = -MotoSpeed;
    
45. 
    
46.     if (power > 0)
    
47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作，正數正轉 負數反轉。
    
48.     else
    
49.       Speed(MotoSpeed + power, MotoSpeed);
    
50.   }
    
51. }

複製代碼

8 e3 @7 h- f  t

感謝分享

感謝分享