PID 循跡自走車範例

1. #include <Arduino.h>
   
2. #include <Wire.h>
   
3. #include <Servo.h>
   4 i% i0 U- o0 D, C
4. 
   
5. #include "MePort.h"
   1 i/ `+ O* ?" u5 U8 d: J+ a7 u
6. #include "MeUltrasonic.h"
   ' z; `5 e, D8 P, J
7. #include "MeDCMotor.h"
   
8. 
   $ E1 q& @* ]) U* ~7 `7 r
9. //double Input, Output ;
   
10. float MaxSpeed = 255;
    - T7 J5 c% `; F7 F# T& g* h! c
11. float MaxPower = 180;
    2 {1 {! }* Y4 p8 I
12. float MinPower = 120;
    
13. float Error,ErrorAcc,ErrorDec;
    - d' u5 W& T8 R* r$ A, S
14. 
    
15. float Kp=0.14;
    
16. float Kd=0.00020;//23;
    
17. float Ki=0.000201;
    6 i! X  N& F! f2 s
18. 
    
19. float nPower;
    
20. MePort lightsensor_6(6);
    
21. MePort lightsensor_8(8);
    
22. MeDCMotor motor_9(9);
    
23. MeDCMotor motor_10(10);
    8 @6 {' u$ |2 C3 H8 t
24. unsigned long previousMillis = 0;
    
25. const long interval = 1;
    2 o, p8 G6 Q# a3 Q8 @
26. 
    
27. void setup(){
    4 S: |4 ?9 ^9 V/ s' b) j2 G) ]+ ?
28.     lightsensor_6.dWrite1(1);
    0 t. Q+ r8 k& [- O& Q* l0 P
29.     nPower = 160;
    
30.     Error=0;
    9 Z/ Q* c: ?$ B5 `4 I
31.     ErrorAcc=0;
    
32. }
    , b/ ~/ l6 y! p8 _% g5 A
33. 
    
34. void loop(){
    
35.   unsigned long StartTime = millis();
    
36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    1 S8 T7 t/ C3 X  c' m$ ^
37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    ! H0 x( D# ]% L2 W! T
38.     ErrorAcc +=  nError*Kd ;
    
39.     ErrorDec -=  nError*Ki ;
    
40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    
41.     if(nError < 80 && nError > -80){
    
42.       if(nPower < MaxPower){
    
43.         nPower += 3;
    
44.       }
    % B0 o! x) m0 f
45.     } else{
    + ^' x* N/ p# W& Y: g
46.       if(nPower > MinPower){
    5 k* c3 P8 i3 X- s) b# E
47.         nPower -= 2;
    , F8 a% I1 X: Y! m& Z6 g& W
48.       }
    4 T4 `0 U7 {, j( G
49.     }
    6 q+ `: k; i# Q+ o, c) a. D
50.     MotoL(nPower-Error);
    
51.     MotoR(nPower+Error);   
    
52.   }else{
    
53.     motor_9.run(0);
    
54.     motor_10.run(0);
    
55.   }
    
56.   do{}while(millis() - StartTime < interval);
    
57. }
    
58. 
    / L! J( M* `" ~; ]4 U
59. void MotoL(int Power){
    1 r, y8 e) b6 _
60.   if (Power > MaxSpeed){
    
61.      Power = MaxSpeed;
    
62.   }
    
63.   if (Power < -MaxSpeed){
    
64.      Power = -MaxSpeed;
    
65.   }
    5 R4 H6 l9 e+ I9 W; N- C
66.   motor_9.run(Power);
    & z/ V1 E6 i& Q2 C# Z1 C
67. }  
    * E* P; ^" y5 W1 k* |" x
68. 
    
69. void MotoR(int Power){
    / f" p8 q7 |' p8 M6 M
70.   if (Power > MaxSpeed){
    ' d0 i% u2 A* B/ p3 b
71.      Power = MaxSpeed;
    
72.   }
    
73.   if (Power < -MaxSpeed){
    * z+ D+ n6 a3 I$ k" m5 T% k- G' v
74.      Power = -MaxSpeed;
    
75.   }
    5 W- _+ e& g8 u1 o
76.   motor_10.run(Power);
    ) l# @( o  `8 l. m' ^; o
77. }  

複製代碼

% ?. C& u$ ]# W. N, I1 f8 N
7 S( l  l/ @$ x- f3 h6 W2 A

! K$ V+ A9 ^( T0 f  O# E
# Z7 i5 ], t6 ?8 \# _2 T2 k2 Z您好，不知是否能向您請教。
目前和宋修賢老師在處理Ardui Car
+ y. Y5 @8 q' z( p雖然已使用較繁雜的方式處理了跑出黑線外的狀況
1 m0 e1 h7 Z7 k' \+ Y9 x
但基於想追求更精簡的程式所以還是想請問一下
就是如果我只以單純寫PID控制時，常遇到CNY70跑出黑線外後便往前衝
* D% Z, ]; a; ]$ k9 w不知道您是否願意教我可以如何處理

  R) H1 r( F  R' p# t
0 p* D" H$ C; I$ F/ N以下是我挑出我一般寫純PID控制的Code：

1. int MotoSpeed = 250;
   % E: N& {* f4 D2 ]+ ~3 Y
2. double CNY70Val = 1000;
   
3. int interror = 0;
   
4. int olderror = 0;
   " o, m- u( Z! s5 I3 S9 l" ]% P+ p
5. double values;
   
6. 
   
7. 
   
8. void CNY70()
   
9. {
   
10.   valuesRR = analogRead(RR)
    4 ^- k, L  U: N' l% I' i2 D8 Q
11.   valuesMR = analogRead(MR);
    , |4 S" H' N6 {0 D9 }) q6 Y- \
12.   valuesMM = analogRead(MM);
    
13.   valuesML = analogRead(ML);
    
14.   valuesLL = analogRead(LL);
    / Y5 Q5 |! s0 X4 y: b8 j
15. 
    
16.   if (valuesRR > CNY70Val)
    
17.     valuesRR = CNY70Val;
    - l) D: q- d: |( ?" C
18.   if (valuesMR > CNY70Val)
    
19.     valuesMR = CNY70Val;
    # D  q! w! I+ ]: B3 [+ r& U3 \0 |
20.   if (valuesMM > CNY70Val)
    
21.     valuesMM = CNY70Val;
    
22.   if (valuesML > CNY70Val)
    
23.     valuesML = CNY70Val;
    2 M6 [0 a4 D2 ?1 R8 Q3 n
24.   if (valuesLL > CNY70Val)
    / h$ Y5 @: E* O& j% X% d( J
25.     valuesLL = CNY70Val;
    
26. 
    
27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    
28. }
    / Q. ^9 \  P" b* v; Q3 p* L! S
29. 
    
30. void Car()
    
31. {
    - @: ^+ V. E! b" V- e2 B5 [
32.   while (1) {
    ) I' F, |$ ^) L6 T7 ?; \' ?
33.     CNY70();
    7 V/ j8 A% e* u* \- Z2 g3 Y$ v* f
34. 
    
35.     int error = ((int)values);
    
36.     interror += error;
    
37.     int lasterror = error - olderror;
    
38.     olderror = error;
    . K2 ?7 ], [# M+ n. Q: C- ^
39.     int power = error / 5 + interror / 10000 + lasterror;
    $ t  Y) z# `( Y/ A' u
40. 
    
41.     if (power > MotoSpeed)
    
42.       power = MotoSpeed;
    
43.     if (power < -MotoSpeed)
    & r4 e, n6 U( m
44.       power = -MotoSpeed;
    
45. 
    
46.     if (power > 0)
    
47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作，正數正轉 負數反轉。
    / k5 n; I: y2 A/ B  p, {
48.     else
    
49.       Speed(MotoSpeed + power, MotoSpeed);
    * R* a2 S6 d+ E' e/ Z
50.   }
    . e& K) ~7 }2 f. t, U; b0 ]; a
51. }

複製代碼

5 [% V9 S6 D6 }7 o" U0 c( w
: I% q/ e, ?0 g  m; q* I0 N* D

感謝分享

感謝分享