PID 循跡自走車範例

1. #include <Arduino.h>
   
2. #include <Wire.h>
   6 |4 k+ J: d6 s1 F2 \
3. #include <Servo.h>
   0 L; x$ e, K; r5 w! @" P! `( ~
4. 
   * T1 ~3 c+ ~; x1 x
5. #include "MePort.h"
   ' f' P# h2 z9 J. {4 t  g
6. #include "MeUltrasonic.h"
   
7. #include "MeDCMotor.h"
   
8. 
   ( A. i+ P* q  W# p, k$ j4 C, }
9. //double Input, Output ;
   ' n7 y7 j+ v6 t; Y6 C4 T7 i
10. float MaxSpeed = 255;
    
11. float MaxPower = 180;
    6 a* g, z% e# a  v& N. ^
12. float MinPower = 120;
    
13. float Error,ErrorAcc,ErrorDec;
    
14. 
    
15. float Kp=0.14;
    / W  N) M7 J  y, X! G
16. float Kd=0.00020;//23;
    # W8 D, r' x" B
17. float Ki=0.000201;
    
18. 
      e8 l# b2 W9 {0 k9 u
19. float nPower;
    
20. MePort lightsensor_6(6);
    
21. MePort lightsensor_8(8);
    ; R) ^0 ?3 R5 T
22. MeDCMotor motor_9(9);
    
23. MeDCMotor motor_10(10);
    
24. unsigned long previousMillis = 0;
    
25. const long interval = 1;
    9 Q; C% T/ v* t! l/ e
26. 
    
27. void setup(){
    
28.     lightsensor_6.dWrite1(1);
    
29.     nPower = 160;
    5 [1 R: s6 J& [6 z$ r
30.     Error=0;
    
31.     ErrorAcc=0;
    , K" [% F6 T3 ^, Y- l
32. }
    
33. 
    ' I  H8 o( S: ]: h4 _2 ~
34. void loop(){
    / X( n% n/ o  R6 W
35.   unsigned long StartTime = millis();
    
36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    & N& v  S  M: A1 m
37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    
38.     ErrorAcc +=  nError*Kd ;
    
39.     ErrorDec -=  nError*Ki ;
    
40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    4 W$ \7 L8 C/ @5 K0 j7 q: }( K
41.     if(nError < 80 && nError > -80){
    
42.       if(nPower < MaxPower){
    
43.         nPower += 3;
    
44.       }
    , H. [  R5 q* j: B% w- e$ k
45.     } else{
    / b1 X1 \% P' _0 Z+ Z
46.       if(nPower > MinPower){
    1 l" W9 G* Q* y
47.         nPower -= 2;
    % n9 y6 _; ?9 x* O  y3 B
48.       }
    3 Y' I' \6 l- T. C; |
49.     }
    / v4 ?( E! a/ j8 c7 w
50.     MotoL(nPower-Error);
    $ L/ j, _9 X& u# a( ?8 x( l
51.     MotoR(nPower+Error);   
    4 I" f3 B- w: r! W  g- X# N8 |( a
52.   }else{
    
53.     motor_9.run(0);
    
54.     motor_10.run(0);
    1 d. U% [2 v: D7 u' O: h, a; n
55.   }
    4 w, ^# D7 O3 S7 x" ?7 F
56.   do{}while(millis() - StartTime < interval);
    
57. }
    
58. 
    
59. void MotoL(int Power){
    
60.   if (Power > MaxSpeed){
    
61.      Power = MaxSpeed;
    9 \6 c" n" r9 e+ P0 [8 d0 x9 }
62.   }
    # L) j1 o: E  c7 g/ B; b, }
63.   if (Power < -MaxSpeed){
    
64.      Power = -MaxSpeed;
    
65.   }
    2 Z- i9 C) m% P% r7 H
66.   motor_9.run(Power);
    
67. }  
    
68. 
    ' o) `6 b' y4 `# k4 t
69. void MotoR(int Power){
    + q# L% M. [: W6 p) P5 Y3 Z- L
70.   if (Power > MaxSpeed){
    
71.      Power = MaxSpeed;
    
72.   }
    
73.   if (Power < -MaxSpeed){
    
74.      Power = -MaxSpeed;
    
75.   }
    
76.   motor_10.run(Power);
    4 ]2 u9 V; Z! `. B( Q. U8 ?6 x
77. }  

複製代碼

& v% J7 G  r1 w& C8 o. [8 J

% `& _$ N! L8 t4 B  W
  Q/ I: e8 P* ?# m: ~您好，不知是否能向您請教。
$ o; M' X% ?4 @5 Z. u  c) U目前和宋修賢老師在處理Ardui Car
雖然已使用較繁雜的方式處理了跑出黑線外的狀況
0 ^! D$ `: z! ]% F( e  _5 I# D
: B) o% g" b8 K6 b' y4 C! D4 q! Y# S但基於想追求更精簡的程式所以還是想請問一下
就是如果我只以單純寫PID控制時，常遇到CNY70跑出黑線外後便往前衝
( G- [4 g5 W0 u! w6 x% S6 a不知道您是否願意教我可以如何處理


0 \6 n; ^4 b8 x以下是我挑出我一般寫純PID控制的Code：

1. int MotoSpeed = 250;
   
2. double CNY70Val = 1000;
   
3. int interror = 0;
   : \" u1 p, y$ h# d' b3 {1 D
4. int olderror = 0;
   
5. double values;
     W. _* N2 }; [* w
6. 
   
7. 
   + T5 \- ^; K8 I4 u
8. void CNY70()
   
9. {
   ; _  y* w' l2 f' |0 t" B
10.   valuesRR = analogRead(RR)
    
11.   valuesMR = analogRead(MR);
    ! q# u# {" E& a/ _6 V4 N) e
12.   valuesMM = analogRead(MM);
    
13.   valuesML = analogRead(ML);
    
14.   valuesLL = analogRead(LL);
    % R- I2 P1 L9 g$ `$ \
15. 
    
16.   if (valuesRR > CNY70Val)
    + X! F% r: O2 d( R1 S) W. W5 u
17.     valuesRR = CNY70Val;
    
18.   if (valuesMR > CNY70Val)
    
19.     valuesMR = CNY70Val;
    
20.   if (valuesMM > CNY70Val)
    
21.     valuesMM = CNY70Val;
    8 }: H$ [5 X0 e2 R4 L! j
22.   if (valuesML > CNY70Val)
    ; w5 I  g6 h% U; n2 g
23.     valuesML = CNY70Val;
    $ O2 C1 l0 R& F) f; V! Y; z: T
24.   if (valuesLL > CNY70Val)
    
25.     valuesLL = CNY70Val;
    , E. p! Y6 J- A
26. 
    5 ?" ]  Z$ `* E
27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    
28. }
    
29. 
    2 x5 i, [4 ^  H( a. q: A7 ?! o5 S
30. void Car()
      g# Q" }$ R" ?# \0 T
31. {
    
32.   while (1) {
    : z2 t5 c  r+ O& g# ?3 I' k( R) f
33.     CNY70();
    
34. 
    
35.     int error = ((int)values);
    
36.     interror += error;
    & K& v/ a4 c/ C( ~) `
37.     int lasterror = error - olderror;
    ) s) Y, R9 b2 ]( V
38.     olderror = error;
    
39.     int power = error / 5 + interror / 10000 + lasterror;
    
40. 
    ) h' ?% e2 W- x8 i8 e$ P
41.     if (power > MotoSpeed)
    
42.       power = MotoSpeed;
    
43.     if (power < -MotoSpeed)
    3 v( R/ [1 s1 Y
44.       power = -MotoSpeed;
    
45. 
    
46.     if (power > 0)
    
47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作，正數正轉 負數反轉。
    
48.     else
    
49.       Speed(MotoSpeed + power, MotoSpeed);
    / t, ~0 x+ w# @7 `: W1 N+ F
50.   }
    : B7 W3 A" R2 V# D% \
51. }

複製代碼

感謝分享

感謝分享