PID 循跡自走車範例

1. #include <Arduino.h>
   * V- C" Z$ V5 f( C* P3 G
2. #include <Wire.h>
   
3. #include <Servo.h>
   4 s" P9 j; {, y7 @
4. 
   
5. #include "MePort.h"
   * b1 r: Y4 e" s: E9 e+ r1 q
6. #include "MeUltrasonic.h"
   4 }4 Q& L$ O1 _* k! K
7. #include "MeDCMotor.h"
   - ~5 i/ \/ S$ o6 @. ~
8. 
   ) ~* b  U' M8 a/ f+ ~
9. //double Input, Output ;
   " c2 t2 }3 C2 U. x
10. float MaxSpeed = 255;
    ! L7 [  }. \: u1 D( t& K
11. float MaxPower = 180;
    
12. float MinPower = 120;
    
13. float Error,ErrorAcc,ErrorDec;
    
14. 
    3 ]% S- Z2 p% _* W0 L
15. float Kp=0.14;
    
16. float Kd=0.00020;//23;
    ) u- F$ k$ c6 d" ~& I/ Z5 C
17. float Ki=0.000201;
    
18. 
    0 X/ s& M0 V! X. N6 t! b
19. float nPower;
    5 e" w  r, B, L1 N. c* G
20. MePort lightsensor_6(6);
    ' u/ y% V1 u9 ?8 P* c/ c
21. MePort lightsensor_8(8);
    
22. MeDCMotor motor_9(9);
    ( C" P: k7 |/ |3 U- S, |+ c1 N
23. MeDCMotor motor_10(10);
    
24. unsigned long previousMillis = 0;
    6 l% _# M' y0 m* Y3 `1 U  J
25. const long interval = 1;
    + P7 U* a( D% \/ E- Q& M  S! R3 ]
26. 
    
27. void setup(){
    ) B+ D; Q1 m% y0 v7 H
28.     lightsensor_6.dWrite1(1);
    
29.     nPower = 160;
    
30.     Error=0;
    ; u# z1 k/ R0 Z% T
31.     ErrorAcc=0;
    
32. }
    
33. 
    
34. void loop(){
    0 z9 {/ H0 d2 l  N
35.   unsigned long StartTime = millis();
    . m0 [2 i$ y0 T2 G' g+ p
36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    , I. g' w/ _. K- X9 j: _
37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    
38.     ErrorAcc +=  nError*Kd ;
    
39.     ErrorDec -=  nError*Ki ;
    
40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    9 f0 h% X2 M! ?; c; x
41.     if(nError < 80 && nError > -80){
    
42.       if(nPower < MaxPower){
    
43.         nPower += 3;
    
44.       }
    
45.     } else{
    6 l# k7 @- t* G2 I6 K* v) m& q# G
46.       if(nPower > MinPower){
    
47.         nPower -= 2;
    , x$ u, [& w! j: ]
48.       }
    
49.     }
    0 d" G. w( F$ J" e6 Z- r
50.     MotoL(nPower-Error);
    
51.     MotoR(nPower+Error);   
    ( x# y+ }$ ^* \- L# \
52.   }else{
    ) C& t1 j' K* \- M' ~3 i
53.     motor_9.run(0);
    : l& R9 p+ W( K! _3 f
54.     motor_10.run(0);
    0 T, J& p  R- k1 I. {! t( b
55.   }
    % Z, K* \7 X/ P( p% u
56.   do{}while(millis() - StartTime < interval);
    
57. }
    . q9 ^8 U2 K* P8 [- {( \
58. 
    
59. void MotoL(int Power){
    3 J9 \6 s8 [. k' y8 L! p# _
60.   if (Power > MaxSpeed){
    
61.      Power = MaxSpeed;
    3 r! `. u. E3 I. o5 |3 S
62.   }
    ; G: I" `$ {6 q& ^) c* \" X
63.   if (Power < -MaxSpeed){
    
64.      Power = -MaxSpeed;
    1 }# H5 ?- \6 x4 m( k5 \
65.   }
    2 C% _6 d  j" u  J% w4 E
66.   motor_9.run(Power);
    
67. }  
    # g8 r- ^* H8 F5 Q
68. 
    
69. void MotoR(int Power){
    
70.   if (Power > MaxSpeed){
    / \2 x1 D/ [/ }4 u9 B0 g6 z) b3 a
71.      Power = MaxSpeed;
    
72.   }
    ) K# C2 w9 m4 _% R
73.   if (Power < -MaxSpeed){
    + O& {: q! Q. Z+ H6 B: h
74.      Power = -MaxSpeed;
    ! g. T# b4 d, z( l9 w
75.   }
    6 L6 @/ d2 Z6 h5 s: a& }
76.   motor_10.run(Power);
    & N  B) F  u+ [+ k$ J# E
77. }  

複製代碼

; D& x+ {. Z) }+ `- L$ G
+ e: v' H2 Q; V! p$ \' x4 N3 b2 G您好，不知是否能向您請教。
& K8 {# K' X) F: O. Z/ p; W目前和宋修賢老師在處理Ardui Car
雖然已使用較繁雜的方式處理了跑出黑線外的狀況
- Q/ M6 a! V. x* z5 \. r
' i  w+ u7 z- d但基於想追求更精簡的程式所以還是想請問一下
- h- k$ {% D- g# T* [) Z就是如果我只以單純寫PID控制時，常遇到CNY70跑出黑線外後便往前衝
不知道您是否願意教我可以如何處理


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

1. int MotoSpeed = 250;
   ( [. p- J7 k+ k* i& B$ \
2. double CNY70Val = 1000;
   
3. int interror = 0;
   
4. int olderror = 0;
   
5. double values;
   ( R  z3 f. K, L# C% [: I/ V
6. 
   
7. 
   - C3 H* J6 j8 x- L: f3 `
8. void CNY70()
   
9. {
   9 Q7 O2 b2 N! g2 k8 m7 n
10.   valuesRR = analogRead(RR)
    
11.   valuesMR = analogRead(MR);
    $ v9 O2 G/ i" W8 X5 ~
12.   valuesMM = analogRead(MM);
      L, h' ^0 N) I' `
13.   valuesML = analogRead(ML);
    
14.   valuesLL = analogRead(LL);
    8 j- ]* w) J0 p9 W3 D
15. 
    1 Y$ u! @8 g$ ~" o
16.   if (valuesRR > CNY70Val)
    " `0 E5 n  q. f" y1 y) ~
17.     valuesRR = CNY70Val;
    5 W5 N$ X5 D4 F/ Q1 {' Q7 t
18.   if (valuesMR > CNY70Val)
    
19.     valuesMR = CNY70Val;
    - \5 U( O' m- S% p3 K
20.   if (valuesMM > CNY70Val)
    
21.     valuesMM = CNY70Val;
    * z2 l2 F9 c( f* p1 l# `! W1 v
22.   if (valuesML > CNY70Val)
    9 l$ g( f- T( T( }: R
23.     valuesML = CNY70Val;
    - K0 x; Q8 W& U# S* w  q; h, L
24.   if (valuesLL > CNY70Val)
    
25.     valuesLL = CNY70Val;
    , q, c: Z4 @- }
26. 
    
27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    
28. }
    
29. 
    & h% k7 l4 h- a8 `. E. r
30. void Car()
    
31. {
    5 z# U" V/ g: A* O, Z2 u
32.   while (1) {
    
33.     CNY70();
    9 k0 D1 w9 @! K$ X
34. 
    * |3 O$ G2 u  g0 S4 H4 l1 @5 d
35.     int error = ((int)values);
    
36.     interror += error;
    
37.     int lasterror = error - olderror;
    
38.     olderror = error;
    ; X6 O) ~# S. [' A1 r- v
39.     int power = error / 5 + interror / 10000 + lasterror;
    , U/ B8 L" v& g1 g% _3 A
40. 
    0 T- z7 P1 a! i2 g  E
41.     if (power > MotoSpeed)
    " G& U; ]6 Q/ [' `# _" [4 z
42.       power = MotoSpeed;
    
43.     if (power < -MotoSpeed)
    
44.       power = -MotoSpeed;
    7 N7 I8 z* d" f: I
45. 
    ( x/ R& C+ R2 [- {; [* _% E( @) ?
46.     if (power > 0)
    + @& J3 U1 }7 w* R, k2 M6 [
47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作，正數正轉 負數反轉。
    
48.     else
    ' L; J* v4 C( t( x- J
49.       Speed(MotoSpeed + power, MotoSpeed);
    . C+ k& e8 P! \- N# p' V
50.   }
    * m9 U% w2 |+ G3 X8 t
51. }

複製代碼

感謝分享

感謝分享