PID 循跡自走車範例

1. #include <Arduino.h>
   
2. #include <Wire.h>
   
3. #include <Servo.h>
   6 v3 N' f8 k  `1 D: r" E, p
4. 
   1 r* f# e& k$ X
5. #include "MePort.h"
   
6. #include "MeUltrasonic.h"
   * q. s" y% ^8 J4 s
7. #include "MeDCMotor.h"
   
8. 
   ; ]& i8 k" ]- r; R0 s
9. //double Input, Output ;
   ) R9 [1 _/ P" P+ K7 ^
10. float MaxSpeed = 255;
    
11. float MaxPower = 180;
    
12. float MinPower = 120;
    
13. float Error,ErrorAcc,ErrorDec;
    1 L2 s2 J( u. A) k# O( M' B
14. 
    3 a0 I2 X: b$ P2 _/ s
15. float Kp=0.14;
    
16. float Kd=0.00020;//23;
    
17. float Ki=0.000201;
    / u; Q) I9 A, S" n
18. 
    ) r, U" j1 ]5 N- W2 s) o
19. float nPower;
    
20. MePort lightsensor_6(6);
    " z: ^/ b2 H6 r
21. MePort lightsensor_8(8);
    - ~. u9 d& X6 U- n: ^7 X1 X
22. MeDCMotor motor_9(9);
    
23. MeDCMotor motor_10(10);
    
24. unsigned long previousMillis = 0;
    
25. const long interval = 1;
    
26. 
    
27. void setup(){
    % H" Q$ J+ |5 O/ \5 W! ^; t7 Z
28.     lightsensor_6.dWrite1(1);
    
29.     nPower = 160;
    9 O, Z5 i0 q8 F6 [. J
30.     Error=0;
    5 z# y. @( D2 L8 `0 d) B# }- U
31.     ErrorAcc=0;
    % V" f3 {& J$ w" {9 L' s) |
32. }
    : S/ H  N  F* D% o. c3 w
33. 
    
34. void loop(){
    7 V1 M& G+ c+ U8 T% J$ Q
35.   unsigned long StartTime = millis();
    / O  h" D7 Y4 R* M( z! ?9 h5 i
36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    - Y: G, J7 E: g6 P) Q: ?
37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    
38.     ErrorAcc +=  nError*Kd ;
    % h7 S$ O, j- p* O9 j+ S, k7 u% e
39.     ErrorDec -=  nError*Ki ;
    
40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    
41.     if(nError < 80 && nError > -80){
    + v1 F% \4 q+ [
42.       if(nPower < MaxPower){
    3 p6 @$ I) }1 b
43.         nPower += 3;
    * J! o% U6 m+ Q. W! @+ C  Y
44.       }
    
45.     } else{
    + H3 Y' y0 q% K1 ~# z% V$ I
46.       if(nPower > MinPower){
    
47.         nPower -= 2;
    
48.       }
    
49.     }
    
50.     MotoL(nPower-Error);
    
51.     MotoR(nPower+Error);   
    
52.   }else{
    . f' |) ]8 `+ G
53.     motor_9.run(0);
    
54.     motor_10.run(0);
    
55.   }
    
56.   do{}while(millis() - StartTime < interval);
    ' i7 ]) [! ^5 R/ e
57. }
    & X- N: D9 ]& `  P+ a
58. 
    
59. void MotoL(int Power){
    + I' x/ n$ D5 q3 b, z) u
60.   if (Power > MaxSpeed){
    
61.      Power = MaxSpeed;
    
62.   }
    
63.   if (Power < -MaxSpeed){
    - t6 H4 u8 U3 M# L: F1 v8 I9 H! V
64.      Power = -MaxSpeed;
    
65.   }
    0 l; o/ Q* H, d- s0 |5 _; M. D9 c, I
66.   motor_9.run(Power);
    
67. }  
    + n, }! k$ V# y8 E+ p
68. 
    
69. void MotoR(int Power){
      R9 r0 V0 f* U. c/ o* e' d
70.   if (Power > MaxSpeed){
    
71.      Power = MaxSpeed;
    
72.   }
    
73.   if (Power < -MaxSpeed){
    
74.      Power = -MaxSpeed;
    
75.   }
    
76.   motor_10.run(Power);
    
77. }  

複製代碼

8 X9 C9 U7 K& _9 \! ?8 B

4 s" t; I+ ^  L; G8 j8 w
您好，不知是否能向您請教。
目前和宋修賢老師在處理Ardui Car
雖然已使用較繁雜的方式處理了跑出黑線外的狀況

但基於想追求更精簡的程式所以還是想請問一下
就是如果我只以單純寫PID控制時，常遇到CNY70跑出黑線外後便往前衝
不知道您是否願意教我可以如何處理
7 v1 I2 q; `$ q8 u$ X! a$ K

$ D6 X) Y5 t) u% ?7 }; t6 Q以下是我挑出我一般寫純PID控制的Code：

1. int MotoSpeed = 250;
   2 T+ h0 h. v! F- j4 y+ \
2. double CNY70Val = 1000;
   
3. int interror = 0;
   : _+ @" ?9 P6 k5 A0 c5 Z& n
4. int olderror = 0;
   : U0 L( H( n3 d( G% Z
5. double values;
   
6. 
   6 W" a4 \' e" Y+ z* F# t  v% S  X
7. 
   , d( A1 r( |& e) Q
8. void CNY70()
   $ @9 G: e- O) i  ^1 L# x
9. {
   
10.   valuesRR = analogRead(RR)
    
11.   valuesMR = analogRead(MR);
    
12.   valuesMM = analogRead(MM);
    ) r( @! `' z! D) T# A
13.   valuesML = analogRead(ML);
    
14.   valuesLL = analogRead(LL);
    
15. 
    
16.   if (valuesRR > CNY70Val)
      T% `; s4 Y- A4 L  `6 l$ w
17.     valuesRR = CNY70Val;
    , ?' y0 `$ g' r! U% e( w( \, D
18.   if (valuesMR > CNY70Val)
    
19.     valuesMR = CNY70Val;
    % k+ t" }, N: {3 e5 s5 o3 a4 L
20.   if (valuesMM > CNY70Val)
    ' Q- l. B2 g6 ~& \
21.     valuesMM = CNY70Val;
    
22.   if (valuesML > CNY70Val)
    
23.     valuesML = CNY70Val;
    
24.   if (valuesLL > CNY70Val)
    2 m' e- l5 j# S* T
25.     valuesLL = CNY70Val;
    
26. 
    
27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    ! ~5 a; F  D! Q
28. }
    4 D  }$ C+ W1 \* U# R/ F
29. 
    , ~- [$ t6 _# J5 w  ^9 ~$ A9 U. ]2 m
30. void Car()
    
31. {
    & q  r. `) o0 D
32.   while (1) {
    ( [4 ~9 O8 n' j9 K
33.     CNY70();
    
34. 
    
35.     int error = ((int)values);
    
36.     interror += error;
    
37.     int lasterror = error - olderror;
    8 b6 r- A* a" P1 Q  @, Y9 ~
38.     olderror = error;
    
39.     int power = error / 5 + interror / 10000 + lasterror;
    4 g5 L, @6 g* c/ u- Y
40. 
    
41.     if (power > MotoSpeed)
    
42.       power = MotoSpeed;
    0 @$ K/ X% P' A
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.   }
    . D1 p& @1 p4 Z* e1 L
51. }

複製代碼

" D5 @) n: q+ V
! U- E% |% V2 p( E- Z

感謝分享

感謝分享